LAB 10 ฟังก์ชั่นหาตำแหน่ง Servo Motor

#include <SPI.h>

#include <Wire.h>

#include <Servo.h>     //เรียกใช้งาน library เซอโว

#include <Adafruit_GFX.h>

#include <Adafruit_SSD1306.h>

#define display_RESET 4  

Adafruit_SSD1306 display(display_RESET);

Servo sv1;                  // กำหนดชื่อเซอโวตัวที่ 1 ว่า sv1  

Servo sv2;                 // กำหนดชื่อเซอโวตัวที่ 2 ว่า sv2  

const int buttonPin = 2;  //ประกาศตัวแปร  buttonPin  แทน Pin ของปุ่มกด ซึ่งต่อไว้กับ D2

void setup() {

  sv1.attach(7);         //กำหนดขา Servo ตัวที่ 1  ตัวบีบ เป็น Pin 7

  sv2.attach(8);        //กำหนดขา Servo ตัวที่ 2  ตัวบีบ เป็น Pin 8

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  // initialize with the I2C addr 0x3D (for the 128x64)

}

void loop(){

int sw = digitalRead(buttonPin);

if (sw == 1){

  while(1) {

 int vr = analogRead(A7);  // กำหนดตัวแปรจำนวนเต็มอ่านค่าอนาล็อกที่พอร์ต 7

  int nob = map(vr, 0, 1023, 0, 180); // ทำการ map อัตราส่วนจากสัญญาณ analog 0-1023 เป็น 0-180

  display.clearDisplay();    // เคลียร์หน้าจอ display

  display.setTextColor(WHITE, BLACK);  //สีอักษรเป็นสีขาว ,พื้นหลังดำ

  display.setCursor(0, 0);       // เซตตำแหน่ง 0,0

  display.setTextSize(2);        // เซตขนาดอักษรมีขนาดเป็น 2

  display.println(nob);     // นำค่า nob มาแสดงใน display

  display.display();        // เปิดฟังก์ชันแสดงผล

  sv1.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  sv2.write(nob);        // สั่งเซอร์โวมอเตอร์ให้หมุนไปตามค่าองศาที่ทำการ nob ไว้

  delay(50);             // หน่วงเวลา 0.05 วินาที

  }

}

}