Home

Published

- 5 min read

Servo Motor

Microcontroller Microprocessor c
img of Servo Motor

หลักการทำงานของ Servo Motor และการควบคุมการทำงานของ Servo Motor โดยใช้ Arduino Uno โดยมีวัตถุประสงค์เพื่อควบคุม Robot Arm

การทดสอบ Servo Motor เบื้องต้น

  • เชื่อมต่อ Servo Motor เข้ากับ Arduino Uno ตามแผนภาพ.
  • เขียนโปรแกรมเพื่อควบคุม Servo Motor โดยกำหนดองศาที่ต้องการให้ Servo Motor หมุน.
  • ทดสอบโดยการป้อนค่าองศาผ่าน Serial Monitor.

Code ตัวอย่าง:

   #include <Servo.h>  // เรียกใช้ Library สำหรับควบคุม Servo Motor

Servo myservo;  // สร้าง Object ชื่อ myservo สำหรับควบคุม Servo Motor

void setup() {
  myservo.attach(13);  // กำหนดให้ Servo Motor เชื่อมต่อกับขา 13 ของ Arduino Uno
}

void loop() {
  myservo.write(90);  // สั่งให้ Servo Motor หมุนไปที่ 90 องศา
  delay(15);  // หน่วงเวลา 15 มิลลิวินาที
}

การควบคุม Servo Motor ให้เคลื่อนที่อย่างนุ่มนวล

  • เพิ่มการหน่วงเวลา (delay) ในทุกๆ องศาที่ Servo Motor เปลี่ยนตำแหน่ง.
  • ใช้ Loop ในการเพิ่มหรือลดองศาของ Servo Motor ทีละ 1 องศา.
  • ตรวจสอบทิศทางการเคลื่อนที่ของ Servo Motor ว่าเป็นการเพิ่มขึ้นหรือลดลงขององศา.

Code ตัวอย่าง:

   #include <Servo.h>

Servo base;  // สร้าง Object สำหรับควบคุม Servo Motor ที่ฐาน
Servo forward_backward;  // สร้าง Object สำหรับควบคุม Servo Motor ที่ข้อต่อหน้า-หลัง
Servo up_down;  // สร้าง Object สำหรับควบคุม Servo Motor ที่ข้อต่อขึ้น-ลง
Servo grip;  // สร้าง Object สำหรับควบคุม Servo Motor ที่ Gripper

String cmdstring;
int prevPos = 90, currentPos, i;  // prevPos คือองศาปัจจุบัน, currentPos คือองศาที่ต้องการ

void setup() {
  Serial.begin(9600);
  base.attach(8);  // กำหนดให้ Servo Motor เชื่อมต่อกับขา 8
  forward_backward.attach(9);  // กำหนดให้ Servo Motor เชื่อมต่อกับขา 9
  up_down.attach(10);  // กำหนดให้ Servo Motor เชื่อมต่อกับขา 10
  grip.attach(11);  // กำหนดให้ Servo Motor เชื่อมต่อกับขา 11
  delay(10);

  base.write(prevPos);  // กำหนดองศาเริ่มต้น
  forward_backward.write(90);
  up_down.write(90);
  grip.write(40);
  delay(1000);
}

void loop() {
  if (Serial.available() > 0) {
    cmdstring = Serial.readString();
    Serial.println(cmdstring);
    currentPos = cmdstring.toInt();  // แปลงค่าที่รับมาเป็นตัวเลข
  }

  if (prevPos < currentPos) {  // ถ้าองศาที่ต้องการมากกว่าองศาปัจจุบัน
    for (i = prevPos; i < currentPos; i++) {
      base.write(i);
      delay(10);
    }
  }
  if (prevPos > currentPos) {  // ถ้าองศาที่ต้องการน้อยกว่าองศาปัจจุบัน
    for (i = prevPos; i > currentPos; i--) {
      base.write(i);
      delay(10);
    }
  }
  prevPos = currentPos;  // ปรับปรุงองศาปัจจุบัน
  delay(500);
}

การควบคุม Servo Motor หลายตัว

  • กำหนดรูปแบบคำสั่งในการควบคุม Servo Motor แต่ละตัว เช่น “base=90”, “fb=90”, “ud=90”, “gp=40”.
  • ใช้ Function ในการแยกคำสั่งและองศาที่ต้องการ.
  • ใช้ Function ในการควบคุมการเคลื่อนที่ของ Servo Motor ให้เป็นไปอย่างนุ่มนวล.

Code ตัวอย่าง:

   #include <Servo.h>

Servo base;
Servo forward_backward;
Servo up_down;
Servo grip;

int servoPos1, servoPos2, servoPos3, servoPos4;
int currentPos[4];
int prevPos[4] = {90, 90, 90, 40};
int servonum;
String cmdstring;
String cmdcode;
String parmstring;
bool newcmd = 0;
int sepIndex;
bool noparm = 0;

void servodrive(int servonum, int servoPos) {  // Function สำหรับขับเคลื่อน Servo Motor
  if (servonum == 0) {
    base.write(servoPos);
  } else if (servonum == 1) {
    forward_backward.write(servoPos);
  } else if (servonum == 2) {
    up_down.write(servoPos);
  } else if (servonum == 3) {
    grip.write(servoPos);
  }
}

void servomotion(int servonum, int servoPos) {  // Function สำหรับควบคุมการเคลื่อนที่อย่างนุ่มนวล
  int i;
  currentPos[servonum] = servoPos;
  if (prevPos[servonum] < currentPos[servonum]) {
    for (i = prevPos[servonum]; i < currentPos[servonum]; i++) {
      servodrive(servonum, i);
      delay(10);
    }
  }
  if (prevPos[servonum] > currentPos[servonum]) {
    for (i = prevPos[servonum]; i > currentPos[servonum]; i--) {
      servodrive(servonum, i);
      delay(10);
    }
  }
  prevPos[servonum] = currentPos[servonum];
}

void setup() {
  Serial.begin(9600);
  base.attach(8);
  forward_backward.attach(9);
  up_down.attach(10);
  grip.attach(11);
  delay(10);
  base.write(prevPos[0]);
  forward_backward.write(prevPos[1]);
  up_down.write(prevPos[2]);
  grip.write(prevPos[3]);
  delay(1000);
}

void loop() {
  if (Serial.available() > 0) {
    cmdstring = Serial.readString();
    newcmd = 1;
  }
  if (newcmd == 1) {
    Serial.println(cmdstring);
    callcmd();
    newcmd = 0;
  }
  delay(1000);
}

void callcmd() {  // Function สำหรับแยกคำสั่ง
  cmdstring.trim();
  sepIndex = cmdstring.indexOf('=');
  if (sepIndex == -1) {
    cmdcode = cmdstring;
    noparm = 1;
  } else {
    cmdcode = cmdstring.substring(0, sepIndex);
    cmdcode.trim();
    parmstring = cmdstring.substring(sepIndex + 1);
    parmstring.trim();
    noparm = 0;
  }

  if (cmdcode.equalsIgnoreCase("base")) {
    if (noparm == 0) {
      servoPos1 = parmstring.toInt();
      servomotion(0, servoPos1);
      Serial.print("Base's Degree: ");
      Serial.println(servoPos1);
    }
  } else if (cmdcode.equalsIgnoreCase("fb")) {
    if (noparm == 0) {
      servoPos2 = parmstring.toInt();
      servomotion(1, servoPos2);
      Serial.print("Forward Backward's Degree: ");
      Serial.println(servoPos2);
    }
  } else if (cmdcode.equalsIgnoreCase("ud")) {
    if (noparm == 0) {
      servoPos3 = parmstring.toInt();
      servomotion(2, servoPos3);
      Serial.print("Up Down's Degree: ");
      Serial.println(servoPos3);
    }
  } else if (cmdcode.equalsIgnoreCase("gp")) {
    if (noparm == 0) {
      servoPos4 = parmstring.toInt();
      servomotion(3, servoPos4);
      Serial.print("Gripper's Degree: ");
      Serial.println(servoPos4);
    }
  } else if (cmdcode.equalsIgnoreCase("rs")) {
    if (noparm == 0) {
      servomotion(0, 90);
      servomotion(1, 90);
      servomotion(2, 90);
      servomotion(3, 40);
      Serial.print("Ready ");
      Serial.println();
    }
  }
}

การควบคุม Servo Motor ด้วย Joystick

  • อ่านค่าจากแกน X และ Y ของ Joystick.
  • นำค่าที่ได้จาก Joystick มาควบคุมองศาของ Servo Motor แต่ละตัว.

Code ตัวอย่าง:

   #include <Servo.h>

Servo base;
Servo forward_backward;
Servo up_down;
Servo grip;

const int joystick_x_pin = A0;
const int joystick_y_pin = A1;
int x_adc_val, y_adc_val;
float x_volt, y_volt;
int baseAngle, fbAngle, udAngle, gripAngle;
int currentPos[4];
int prevPos[4] = {90, 90, 90, 40};
int servonum;

void servodrive(int servonum, int servoPos) {
  if (servonum == 0) {
    base.write(servoPos);
  } else if (servonum == 1) {
    forward_backward.write(servoPos);
  } else if (servonum == 2) {
    up_down.write(servoPos);
  } else if (servonum == 3) {
    grip.write(servoPos);
  }
}

void servomotion(int servonum, int servoPos) {
  int i;
  currentPos[servonum] = servoPos;
  if (prevPos[servonum] < currentPos[servonum]) {
    for (i = prevPos[servonum]; i < currentPos[servonum]; i++) {
      servodrive(servonum, i);
      delay(10);
    }
  }
  if (prevPos[servonum] > currentPos[servonum]) {
    for (i = prevPos[servonum]; i > currentPos[servonum]; i--) {
      servodrive(servonum, i);
      delay(10);
    }
  }
  prevPos[servonum] = currentPos[servonum];
}

void setup() {
  Serial.begin(9600);

  base.attach(8);
  forward_backward.attach(9);
  up_down.attach(10);
  grip.attach(11);

  base.write(prevPos[0]);
  forward_backward.write(prevPos[1]);
  up_down.write(prevPos[2]);
  grip.write(prevPos[3]);

  baseAngle = prevPos[0];
  fbAngle = prevPos[1];
  udAngle = prevPos[2];
  gripAngle = prevPos[3];

  delay(2000);
}

void loop() {
  int x_adc_val, y_adc_val;
  float x_volt, y_volt;
  x_adc_val = analogRead(joystick_x_pin);
  y_adc_val = analogRead(joystick_y_pin);
  x_volt = ((x_adc_val * 5.0) / 1023);
  y_volt = ((y_adc_val * 5.0) / 1023);
  Serial.print("X_Vol = ");
  Serial.print(x_volt);
  Serial.print("\t");
  Serial.print("Y_Vol = ");
  Serial.println(y_volt);

  if ((x_volt <= 2.60 && x_volt >= 2.40) && (y_volt <= 0.1 && y_volt >= 0.00)) {
    if (baseAngle < 180) {
      baseAngle++;
    } else {
      baseAngle = 180;
    }
    servomotion(0, baseAngle);
  }

  if ((x_volt <= 2.60 && x_volt >= 2.40) && (y_volt <= 5.00 && y_volt >= 4.90)) {
    if (baseAngle > 0) {
      baseAngle--;
    } else {
      baseAngle = 0;
    }
    servomotion(0, baseAngle);
  }

  if ((x_volt <= 5.00 && x_volt >= 4.90) && (y_volt <= 2.60 && y_volt >= 2.40)) {
    if (fbAngle < 150) {
      fbAngle++;
    } else {
      fbAngle = 150;
    }
    servomotion(1, fbAngle);
  }

  if ((x_volt <= 0.1 && x_volt >= 0.00) && (y_volt <= 2.60 && y_volt >= 2.40)) {
    if (fbAngle > 70) {
      fbAngle--;
    } else {
      fbAngle = 70;