ประดิษฐ์รถบังคับจากแผ่นซีดี ควบคุมด้วยแอพแอนดรอยด์


สิ่งประดิษฐ์ทางวิทยาศาสตร์ โดยใช้มอเตอร์ ทำจากเศษวัสดุ โดยเราจะประดิษฐ์ รถบังคับ Arduino จาก จากแผ่นซีดี ควบคุมด้วยแอพแอนดรอยด์

     อุปกรณ์ที่ต้องใช้ก็คือ

     1. แผ่น CD ที่ไม่ใช้แล้ว จำนวน 2 แผ่น

     2. Arduino UNO R3 - Made in italy

     3. Motor Drive Module L298N

     4. HC-05 Bluetooth Master Slave

     5. สาย Jumper Female to Male ยาว 10cm.

     6. สาย Jumper Male to  Male ยาว 10cm.

     7. เกียร์มอเตอร์และล้อรถ จำนวน 2 ชุด

     8. ล้อหน้า สำหรับ 2WD

     9. รางถ่าน AA 6 ก้อน 9 โวลต์

     10. ถ่านอัลคาไลน์ ขนาด AA จำนวน 6 ก้อน


ขั้นตอนการทำงาน

เตรียมปืนยิงกาว



เตรียมแผ่น CD ที่ไม่ใช้แล้ว จำนวน 2 แผ่น



ใช้ปืนยิงกาวยึด CD 2 แผ่นเข้าด้วยกันทั้ง 2 ด้าน ดังรูป



ยึด มอเตอร์ตัวที่1 เข้าที่ด้านล่าง ของแผ่น CD โดยหันขั้วของมอเตอร์เข้าด้านใน


ยึด มอเตอร์ตัวที่2 เข้าที่ด้านล่าง ของแผ่น CD โดยหันขั้วของมอเตอร์เข้าด้านใน


ประกอบ ล้อรถ ทั้ง 2 ล้อ เข้า กับ เกียร์มอเตอร์



ยึด ล้อหน้า


เมื่อกลับดูด้านบน



ยึด Motor Drive Module L298N




ยึด Arduino UNO R3




ยึด รางถ่าน AA 6 ก้อน 9 โวลต์




ประกอบวงจร






ต่อสาย สีดำ และ สีแดง ของรางถ่าน AA เข้ากับ Motor Driver ดังรูป



ต่อสาย 5V ของ Motor Drive Module L298N ไปยัง 5V ของ Arduino UNO R3




ต่อสาย จาก Motor Drive Module L298N  ไปยัง มอเตอร์ ทั้ง 2 ตัว




บัดกรี เชื่อมสาย เข้ากับมอเตอร์ด้านล่างทั้ง 2 ตัว



ต่อสาย ระหว่าง Motor Drive Module L298N  กับ Arduino UNO R3

เรียนรู้เพิ่มเติม : https://robotsiam.blogspot.com/2016/08/l298n-motor-driver-connect-arduino-r3.html

*** VCC ของ Arduino UNO R3 คือ 5V ***







ต่อสาย ระหว่าง HC-05 Bluetooth Master Slave กับ Arduino UNO R3

เรียนรู้เพิ่มเติม : https://robotsiam.blogspot.com/2016/08/hc-05-bluetooth-module.html

HC-05 ใช้ไฟ 5V จุดเดียวกับ L298N จำเป็น ต้อง เชื่อมต่อทั้ง 2 สาย ร่วมกันก่อน แล้วจึงต่อลงไปที่ 5V อีกครั้ง




บัดกรี สาย 5V ทั้ง 3 สายเข้า ด้วยกัน



แล้วจึงต่อลงไปที่ 5V อีกครั้ง





ต่อสายสีดำ GND <-> GND




ต่อสายที่เหลือ ระหว่าง HC-05 กับ บอร์ด UNO พิน 8 , 9 , 10




เชื่อมต่อ สายกราวด์ GND  ของ L298N กับ บอร์ด UNO เข้าด้วยกัน โดยถอดสายสีดำของรางถ่าน ออกจาก L298N  บัดกรี หรือ มัดรวมกัน แล้วใส่คืน GND  ของ L298N อีกครั้ง




ต่อ สายกราวด์ GND ที่เชื่อมต่อ รางถ่านสีดำ กับ GND ของ L298N ไว้แล้ว ไปยัง GND ของ บอร์ด UNO



เชื่อมต่อสาย USB ระหว่าง คอมพิวเตอร์ กับ บอร์ด UNO




เรียนรู้เพิ่มติม : https://robotsiam.blogspot.com/2016/08/arduino-ide.html
เรียนรู้เพิ่มติม : https://robotsiam.blogspot.com/2016/08/hello-world-arduino-uno-r3.html

เปิดโปรแกรม Arduino (IDE)  เขียน โค้ด และ Upload  ไปยังบอร์ด Arduino UNO ดังนี้

#include <SoftwareSerial.h>
SoftwareSerial BTSerial(9, 10);

int dir1PinA = 2;
int dir2PinA = 3;
int speedPinA = 6;
int dir1PinB = 4;
int dir2PinB = 5;
int speedPinB = 7;
int SPEED = 191;  // Speed PWM สามารถปรับความเร็วได้ถึง 255

void setup() 
{   

  pinMode(dir1PinA,OUTPUT);
  pinMode(dir2PinA,OUTPUT);
  pinMode(speedPinA,OUTPUT);
  pinMode(dir1PinB,OUTPUT);
  pinMode(dir2PinB,OUTPUT);
  pinMode(speedPinB,OUTPUT);
  pinMode(8,OUTPUT);
  digitalWrite(8, HIGH);
  Serial.begin(9600);
  BTSerial.begin(9600); 
}


void loop() 
{  
  if (BTSerial.available())
    Serial.write(BTSerial.read());

  if (Serial.available())
    BTSerial.write(Serial.read());

  if (BTSerial.available() > 0) {

    int inByte = BTSerial.read();
    int speed;
    switch (inByte) {

    case 'F':

      analogWrite(speedPinA, SPEED);
      analogWrite(speedPinB, SPEED);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir1PinB, HIGH);
      digitalWrite(dir2PinA, HIGH);
      digitalWrite(dir2PinB, LOW);
      Serial.println("Motor 1 Forward");
      Serial.println("Motor 2 Forward");
      Serial.println("   "); 

      break;



    case 'S': 

      analogWrite(speedPinA, 0);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir2PinA, HIGH);
      Serial.println("Motor 1 Stop");
      analogWrite(speedPinB, 0);
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      Serial.println("Motor 2 Stop");
      Serial.println("   ");

      break;

    case 'B':

      analogWrite(speedPinA, SPEED);
      digitalWrite(dir1PinA, HIGH);
      digitalWrite(dir2PinA, LOW);
      Serial.println("Motor 1 Back");
      analogWrite(speedPinB, SPEED);
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      Serial.println("Motor 2 Back");
      Serial.println("   ");

      break;

    case 'L':

      analogWrite(speedPinA, 0);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir2PinA, HIGH);
      Serial.println("Motor 1 Left");
      analogWrite(speedPinB, SPEED);
      digitalWrite(dir1PinB, HIGH);
      digitalWrite(dir2PinB, LOW);
      Serial.println("Motor 2 Left");
      Serial.println("   ");

      break;

    case 'R':

      analogWrite(speedPinA, SPEED);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir2PinA, HIGH);
      Serial.println("Motor 1 Right");
      analogWrite(speedPinB, 0);
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      Serial.println("Motor 2 Right");
      Serial.println("   ");

      break;

    case 'I':

      analogWrite(speedPinA, 150);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir2PinA, HIGH);
      Serial.println("Motor 1 Forward L");
      analogWrite(speedPinB, SPEED);
      digitalWrite(dir1PinB, HIGH);
      digitalWrite(dir2PinB, LOW);
      Serial.println("Motor 2 Forward L");
      Serial.println("   ");

      break;

    case 'G':

      analogWrite(speedPinA, SPEED);
      digitalWrite(dir1PinA, LOW);
      digitalWrite(dir2PinA, HIGH);
      Serial.println("Motor 1 Forward R");
      analogWrite(speedPinB, 150);
      digitalWrite(dir1PinB, HIGH);
      digitalWrite(dir2PinB, LOW);
      Serial.println("Motor 2 Forward R");
      Serial.println("   ");

      break;

    case 'J':

      analogWrite(speedPinA, 200);
      digitalWrite(dir1PinA, HIGH);
      digitalWrite(dir2PinA, LOW);
      Serial.println("Motor 1 Back L");
      analogWrite(speedPinB, SPEED);
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      Serial.println("Motor 2 Back L");
      Serial.println("   ");

      break;

    case 'H':

      analogWrite(speedPinA, SPEED);
      digitalWrite(dir1PinA, HIGH);
      digitalWrite(dir2PinA, LOW);
      Serial.println("Motor 1 Back R");
      analogWrite(speedPinB, 200);
      digitalWrite(dir1PinB, LOW);
      digitalWrite(dir2PinB, HIGH);
      Serial.println("Motor 2 Back R");
      Serial.println("   ");

      break;


    default:



      for (int thisPin = 2; thisPin < 11; thisPin++) 

      {

        digitalWrite(thisPin, LOW);

      }

    }

  }

}




หาแก้วสำหรับ ยกตัวรถ เพื่อไม่ให้ล้อแตะพื้น สำหรับการทดสอบ






จากนั้น ทดลองนำ โทรศัพท์มือถือ แอนดรอยด์  เปิด บลูทูธ  แล้ว ดาวน์โหลดและติดตั้ง โปรแกรม Arduino Bluetooth RC Car ดังนี้






สังเกตุ เมื่อเปิดขึ้นมา รูปวงกลมซ้ายมือ จะเป็นสีแดง



คลิกที่ ไอคอนเฟือง ขวามือสุด แล้ว เลือก Connect to car




เลือก HC-05 (ถ้าถามหา พาสเวิร์ด ให้คีย์ 1234)




สังเกตุ รูปวงกลมซ้ายมือเป็นสีเขียวแสดงว่า โปรแกรมสามารถใช้งานได้แล้ว ทดลองควบคุมดูเลยครับ





เมื่อล้อรถหมุน ตรวจสอบการหมุนของล้อ ว่าถูกต้องหรือไม่ ถ้าไม่ถูกต้องตรวจสอบแก้ไขการต่อสายต่างๆ ขั้วของมอเตอร์ถูกต้องหรือไม่




ถ้าทุกอย่างถูกต้อง รถบังคับ Arduino จาก แผ่น CD เก่า ของเรา ก็พร้อมใช้งานแล้วครับ




วีดีโอผลลัพธ์การทำงานของ ประดิษฐ์รถบังคับจากแผ่นซีดี ควบคุมด้วยแอพแอนดรอยด์




Arduino Five Sensor PID Line Following Robot

Arduino Five Sensor PID Line Following Robot เราจะใช้ PID ในการควบคุม มอเตอร์ซ้ายและขวา ของหุ่นยนต์ โดยให้เส้นสีดำเป็นศูนย์กลาง และจะคำนวณ การเบี่ยงเบนของหุ่นยนต์จากศูนย์กลางของเส้นดำ จากข้อมูลที่ส่งเข้ามาของ IR เซนเซอร์ 5 ตัว

ระบบควบคุมแบบสัดส่วน-ปริพันธ์-อนุพันธ์ ( PID controller) เป็นระบบควบคุมแบบป้อนกลับที่นิยมใช้กันซึ่งค่าที่นำไปใช้ในการคำนวณเป็นค่าความผิดพลาดที่ได้มาจากความแตกต่างของตัวแปร ซึ่งเราต้องการ ลดค่าความผิดพลาดให้เหลือน้อยที่สุดด้วยการปรับค่าตัวแปรของ PID

รายละเอียดเพิ่มเติม PID  https://th.wikipedia.org/wiki/ระบบควบคุมพีไอดี

   
     11. สกรูหัวกลมน็อตตัวเมีย ขนาด 3มม ยาว12 มม.

     12. Mounting Bracket for HC-SR04 Ultrasonic Module แบบสั้น
 
     13. ตัดแผ่นอะคริลิค หนา 3 มิลลิเมตร ขนาด  3 x 15 เซ็นติเมตร 

เริ่มต้นด้วยการ ประกอบ Smart Robot Car Chassis Kit

ต่อวงจร Arduino  UNO กับ L298N  Motor Driver ตามรูปการต่อวงวงจร

นำไฟจากแบตเตอรี่ลิเธียม 18650 ไปต่อตรงกับ L298N  Motor Driver (ไม่ต่อกับ พอร์ต Power Supply ของ บอร์ด Arduino  UNO R3 ) และ นำไฟ 5 โวลต์ ที่ออกจาก L298N  Motor Driver ต่อออกไปเลี้ยง บอร์ด Arduino  UNO R3 ตามรูปการต่อวงวงจร







ประกอบ IR เซนเซอร์ 5 ตัว เข้ากับ แผ่นอะคริลิค ขนาด 3 x 15 เซ็นติเมตร 



ยึดเข้ากับ Mounting Bracket for HC-SR04 Ultrasonic Module แบบสั้น


การเขียนโปรแกรม


เนื่องจากเรามีการให้ IR เซ็นเซอร์  เป็นเอาท์พุท แบบดิจิตอล โดยหากตรวจพบพื้นสีขาว  จะได้ผลลัพธ์เป็น 0 และ หากตรวจพบสีดำ จะได้ผลลัพธ์เป็น 1

เซนเซอร์ | ตำแหน่ง

0 0 1 0 0 | เดินหน้า

1 0 0 0 0 | เลี้ยวขวา

0 0 0 0 1 | เลี้ยวซ้าย


เริ่มเขียนโปรแกรม

โดยเริ่มต้นตั้งค่าตัวแปรทั้งหมดเป็น 0  
ตั้งค่าความเร็ว PWM ของมอเตอร์เป็น 100
กำหนดพิน 10 , 11 , 4 , 5 , 6, 7 , 8   ให้เป็นโหมดเอาท์พุท

ตั้งค่าความเร็วในการรับ-ส่งข้อมูล 9600 เพื่อให้สามารถติดต่อสื่อสารกับ USB Port (Serial Port  หรือ พอร์ตอนุกรม) เพื่อดูค่าตัวแปรที่แตกต่างกัน ซึ่งจะเป็นประโยชน์ในการปรับแต่ง

float Kp=0,Ki=0,Kd=0;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;
int sensor[5]={0, 0, 0, 0, 0};
int initial_motor_speed=100; 

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
 pinMode(10,OUTPUT); //PWM Pin 1
 pinMode(11,OUTPUT); //PWM Pin 2
 pinMode(4,OUTPUT); //Left Motor Pin 1
 pinMode(5,OUTPUT); //Left Motor Pin 2
 pinMode(6,OUTPUT); //Right Motor Pin 1
 pinMode(7,OUTPUT);  //Right Motor Pin 2
 Serial.begin(9600); // Enable Serial Communications
}


การคำนวณค่าความผิดพลาด:

เราจะได้รับการกำหนดค่าที่แตกต่างกันสำหรับการรับค่าต่างๆของค่าเซ็นเซอร์นำไปใช้ในการคำนวณค่าความเบี่ยงเบนจากศูนย์ เพื่อใช้เป็นเงื่อนไขในการเขียนโปรแกรม


เซนเซอร์ค่าอาร์เรย์ค่าความผิดพลาด
0 0 0 0 14
0 0 0 1 13
0 0 0 1 02
0 1 0 1 01
0 0 1 0 00
1 0 1 0 0-1
0 1 0 0 0-2
1 1 0 0 0-3
1 0 0 0 0-4
0 0 0 0 0-5 หรือ 5 (ขึ้นอยู่กับค่าก่อนหน้านี้)

เขียนโค้ดได้ดังนี้

void read_sensor_values()
{
  sensor[0]=digitalRead(A0);
  sensor[1]=digitalRead(A1);
  sensor[2]=digitalRead(A2);
  sensor[3]=digitalRead(A3);
  sensor[4]=digitalRead(A4);

  if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==1))
  error=4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==1))
  error=3;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==0))
  error=2;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==1)&&(sensor[4]==0))
  error=1;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
  error=0;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-1;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-2;
  else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-3;
  else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
    if(error==-4) error=-5;
    else error=5;

}



การคำนวณ PID

ตัวแปรข้อผิดพลาดนี้ จะใช้ฟังก์ชัน calculate_pid ซึ่งจะคำนวณค่า PID และการส่งออกมา 

เขียนโค้ดได้ดังนี้


void calculate_pid()
{
    P = error;
    I = I + error;
    D = error – previous_error;
    
    PID_value = (Kp*P) + (Ki*I) + (Kd*D);
    
    previous_error=error;
}


การควบคุมมอเตอร์

PID_value อาจจะเป็นบวกหรือเป็นค่าลบ ดังนั้นถ้ามันเป็นลบด้านซ้ายจะเพิ่มความเร็วมอเตอร์และในทางกลับกันถ้า PID_value เป็นบวก


เขียนโค้ดได้ดังนี้


void motor_control()
{
    // Calculating the effective motor speed:
    int left_motor_speed = initial_motor_speed-PID_value;
    int right_motor_speed = initial_motor_speed+PID_value;
    
    // The motor speed should not exceed the max PWM value
    constrain(left_motor_speed,0,255);
    constrain(right_motor_speed,0,255);
    
    analogWrite(10,left_motor_speed);   //Left Motor Speed
    analogWrite(11,right_motor_speed);  //Right Motor Speed
    //following lines of code are to make the bot move forward
    /*The pin numbers and high, low values might be different
    depending on your connections */
    digitalWrite(4,HIGH);
    digitalWrite(5,LOW);
    digitalWrite(6,LOW);
    digitalWrite(7,HIGH);
}



เมื่อนำมารวมกันจะได้โค้ดตามด้านล่าง

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


float Kp=0,Ki=0,Kd=0;
float error=0, P=0, I=0, D=0, PID_value=0;
float previous_error=0, previous_I=0;
int sensor[5]={0, 0, 0, 0, 0};
int initial_motor_speed=100;

void read_sensor_values(void);
void calculate_pid(void);
void motor_control(void);

void setup()
{
 pinMode(10,OUTPUT); //PWM Pin 1
 pinMode(11,OUTPUT); //PWM Pin 2
 pinMode(4,OUTPUT); //Left Motor Pin 1
 pinMode(5,OUTPUT); //Left Motor Pin 2
 pinMode(6,OUTPUT); //Right Motor Pin 1
 pinMode(7,OUTPUT);  //Right Motor Pin 2
 Serial.begin(9600); //Enable Serial Communications
}

void loop()
{
    read_sensor_values();
    calculate_pid();
    motor_control();
}

void read_sensor_values()
{
  sensor[0]=digitalRead(A0);
  sensor[1]=digitalRead(A1);
  sensor[2]=digitalRead(A2);
  sensor[3]=digitalRead(A3);
  sensor[4]=digitalRead(A4);
  
  if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[4]==0)&&(sensor[4]==1))
  error=4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==1))
  error=3;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==1)&&(sensor[4]==0))
  error=2;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==1)&&(sensor[4]==0))
  error=1;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
  error=0;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==1)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-1;
  else if((sensor[0]==0)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-2;
  else if((sensor[0]==1)&&(sensor[1]==1)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-3;
  else if((sensor[0]==1)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
  error=-4;
  else if((sensor[0]==0)&&(sensor[1]==0)&&(sensor[2]==0)&&(sensor[3]==0)&&(sensor[4]==0))
    if(error==-4) error=-5;
    else error=5;

}

void calculate_pid()
{
    P = error;
    I = I + previous_I;
    D = error-previous_error;
    
    PID_value = (Kp*P) + (Ki*I) + (Kd*D);
    
    previous_I=I;
    previous_error=error;
}

void motor_control()
{
    // Calculating the effective motor speed:
    int left_motor_speed = initial_motor_speed-PID_value;
    int right_motor_speed = initial_motor_speed+PID_value;
    
    // The motor speed should not exceed the max PWM value
    constrain(left_motor_speed,0,255);
    constrain(right_motor_speed,0,255);
 
 analogWrite(10,initial_motor_speed-PID_value);   //Left Motor Speed
    analogWrite(11,initial_motor_speed+PID_value);  //Right Motor Speed
    //following lines of code are to make the bot move forward
    /*The pin numbers and high, low values might be different
    depending on your connections */
    digitalWrite(4,HIGH);
    digitalWrite(5,LOW);
    digitalWrite(6,LOW);
    digitalWrite(7,HIGH);
}






ปรับ Kp, Ki, Kd ค่า

นี้เป็นส่วนที่สำคัญที่สุดของโปรแกรมของคุณ ค่าคงที่ PID คือ. Kp, Ki และ Kd ค่าปรับเท่านั้นโดยการทดลองและวิธีการผิดพลาด ค่าเหล่านี้จะแตกต่างกันสำหรับทุกหุ่นยนต์และการกำหนดค่าสำหรับทุกคน ลองใช้วิธีนี้ในขณะที่การปรับจูน:

เริ่มต้นด้วย Kp, Ki และ Kd เท่ากับ 0 และทำงานกับ Kp แรก ลองตั้งค่า Kp ค่าเป็น 1 และสังเกตหุ่นยนต์ เป้าหมายคือการได้รับหุ่นยนต์ที่จะปฏิบัติตามเส้นแม้ว่ามันจะสั่นคลอนมาก ถ้าหุ่นยนต์ overshoots และสูญเสียเส้นลดค่า Kp ถ้าหุ่นยนต์ไม่สามารถเลื่อนเปิดหรือดูเหมือนซบเซาเพิ่มค่า Kp
เมื่อหุ่นยนต์สามารถที่จะทำตามบ้างเส้นกำหนดค่า 1 ถึง Kd (ข้าม Ki ในขณะนั้น) ลองเพิ่มค่านี้จนกว่าคุณจะเห็นจำนวนเงินที่น้อยกว่าของโยกเยก

เมื่อหุ่นยนต์ค่อนข้างคงที่ต่อไปนี้สายที่กำหนดค่า 0.5 ถึง 1.0 Ki ถ้าค่า Ki สูงเกินไปหุ่นยนต์จะเหวี่ยงซ้ายและขวาได้อย่างรวดเร็ว ถ้ามันต่ำเกินไปคุณจะไม่เห็นความแตกต่างรับรู้ใด ๆ เนื่องจากเป็นส่วนประกอบสะสมค่า Ki มีผลกระทบอย่างมีนัยสำคัญ คุณอาจจะจบลงด้วยการปรับเพิ่มขึ้นทีละ 01 โดย
เมื่อหุ่นยนต์ต่อไปนี้สายที่มีความแม่นยำที่ดีคุณสามารถเพิ่มความเร็วและดูว่ามันจะยังคงสามารถที่จะปฏิบัติตามสาย ส่งผลกระทบต่อความเร็วในการควบคุม PID และจะต้อง retuning การเปลี่ยนแปลงความเร็ว


โปรเจค หุ่นยนต์ฮิวแมนนอยด์ Arduino 12 DOF เพื่อการศึกษา

12 DOF Biped Robotic Educational Robot Humanoid Robot Servo Bracket



โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF อุปกรณ์ที่ต้องใช้ก็คือ

     1. Large U Bracket  จำนวน 1 ชิ้น
 
     2. Servo Bracket จำนวน 12 ชิ้น
 
     3. Long U Bracket จำนวน  3 ชิ้น
   
     4. Short U Bracket จำนวน  7 ชิ้น
 
     5. L Bracket จำนวน 4 ชิ้น

     6. Tilt U Bracket จำนวน 2 ชิ้น
     
     7. Big Feet จำนวน 2 ชิ้น
 
     8. Disc 25T Metal จำนวน 12 ชิ้น
 
     9. Metal Cup Bearing จำนวน 10 ชิ้น
 
    10. สกรูหัวกลม + น็อตตัวเมีย ขนาด 3 มม. ยาว 10 มม. จำนวน 124 ชิ้น
 
    11. Arduino UNO R3 - Made in italy จำนวน 1 ชิ้น
 
    12. Sensor Shield V5.0  จำนวน 1 ชิ้น
 
    13. เซอร์โวมอเตอร์ MG996R จำนวน 12 ชิ้น
 
    14. Jumper (F2M) cable wire 40pcs 10 cm 2.54mm Female to Male
   
    15. รางถ่านแบบ 18650 ใส่ถ่าน 2 ก้อน

    16. แบตเตอรี่ลิเธียม 18650 จำนวน 2 ก้อน



ขั้นตอนการประกอบ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา ส่วนล่างจะคล้ายๆกับ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 8 DOF เพื่อการศึกษา

เพียงแต่เปลี่ยน Large U Bracket (ด้านบนสุด) จากแนวตั้ง


เป็นแนวนอน โดยส่วนอื่นๆจะประกอบเหมือนกัน




ยึด L Bracket 2 ชิ้น  ไปบน  Large U Bracket ดังรูป



ยึด Servo Bracket 2 ชิ้น เข้ากับ L Bracket




ยึด เซอร์โวมอเตอร์ MG996R และ Disc Metal  2 ชิ้น เข้ากับ Servo Bracket




ยึด Tilt U Bracket (ยูเอียง) 2 ชิ้น เข้ากับ Disc Metal  ของ เซอร์โวมอเตอร์ MG996R  เพื่อเป็น ไหล่ของหุ่นยนต์




ยึด Servo Bracket 2 ชิ้น เข้ากับ Tilt U Bracket




ประกอบ L Bracket 2 ชิ้น เข้ากับ Short U Bracket




 ยึด ชุด L Bracket ที่ประกอบอยู่กับ Short U Bracket เข้ากับ Servo Bracket ทั้ง 2 ด้าน





ยึด เซอร์โวมอเตอร์ MG996R เข้าไปที่แขนของหุ่นยนต์ ทั้ง 2 ด้าน



ยึด Long U Bracket เข้ากับ Large U Bracket เพื่อเป็นลำตัวของหุ่นยนต์

และ ยึด  Short U Bracket เข้าที่ด้านบนของ Long U Bracket เพื่อเป็นส่วนหัว ของหุ่นยนต์



ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้าเมื่อประกอบเสร็จ





ด้านหลัง ยึด แผ่นอะคริลิค ขนาด 9  x 11 เซ็นติเมตร จำนวน 1 แผ่น เข้าที่ด้านหลังของหุ่นยนต์

และ ประกอบ Arduino UNO R3 กับ Sensor Shield V5.0เข้า กับ แผ่นอะคริลิค




ยึด รางถ่านแบบ 18650 (2 ก้อน) นำสายสีแดง ขั้วบวก ของรางถ่าน ต่อเข้ากับ VCC และ สายสีดำ ขั้วลบ ต่อเข้ากับ GND ของ บอร์ด Sensor Shield V5.0

และ ต่อสาย เซอร์โวมอเตอร์ MG996R ทุกตัว เข้าที่ บอร์ด Sensor Shield V5.0

//Servo input pins
  rightAnkle.attach(2); // เท้าขวา
  rightKnee.attach(3);  // เข่าขวา
  rightThigh.attach(4); // ต้นขาขวา
  rightHip.attach(5);  // สะโพกขวา
  leftAnkle.attach(6);  // เท้าซ้าย
  leftKnee.attach(7); // เข่าซ้าย
  leftThigh.attach(8);  // ต้นขาซ้าย
  leftHip.attach(9);  // สะโพกซ้าย
  leftShoulder.attach(10); // ไหล่ซ้าย
  rightShoulder.attach(11); // ไหล่ขวา
  leftElbow.attach(12); // ข้อศอกซ้าย
  rightElbow.attach(13); // ข้อศอกขวา




ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหลัง เมื่อประกอบเสร็จ




ภาพรวม หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ด้านหน้า เมื่อประกอบเสร็จ



credit : 


ลิงค์ youtube ต้นแบบ https://www.youtube.com/watch?v=eP7R-Hlo8pQ

ลิงค์โค้ดต้นแบบ https://www.dropbox.com/s/d2c3mxcu74s84qr/Biped_2.ino?dl=0

เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3


#include <Servo.h>


int delayVal2 = 25;
int delayVal = 40;
bool time = true;

//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle;  // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee;   // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh;  // ต้นขาซ้าย
Servo rightHip;  // สะโพกขวา
Servo leftHip;  // สะโพกซ้าย
Servo leftShoulder;  // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow;  // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา

void setup() {
  //Servo input pins
  rightAnkle.attach(2); // เท้าขวา
  rightKnee.attach(3);  // เข่าขวา
  rightThigh.attach(4); // ต้นขาขวา
  rightHip.attach(5);  // สะโพกขวา
  leftAnkle.attach(6);  // เท้าซ้าย
  leftKnee.attach(7); // เข่าซ้าย
  leftThigh.attach(8);  // ต้นขาซ้าย
  leftHip.attach(9);  // สะโพกซ้าย
  leftShoulder.attach(10); // ไหล่ซ้าย
  rightShoulder.attach(11); // ไหล่ขวา
  leftElbow.attach(12); // ข้อศอกซ้าย
  rightElbow.attach(13); // ข้อศอกขวา

  delay(2000);
  stand();


}

void loop()
{

stand();
turnLeft();
//forward();

}

int rightAnkPos = 90;
int rightKneePos = 90;
int rightThighPos = 90;
int rightHipPos = 90;
int leftAnkPos = 90;
int leftKneePos = 90;
int leftThighPos = 90;
int leftHipPos = 90;



//ปรับค่าตามความเหมาะสม//

void stand ()
{

  rightAnkle.write(100); // เท้าขวา
  int rightAnkPos = 100;
  rightKnee.write(100);   // เข่าขวา
  int rightKneePos = 100;
  rightThigh.write(110);  // ต้นขาขวา
  int rightThighPos = 110;
  rightHip.write(90);   // สะโพกขวา
  int rightHipPos = 90;
  leftAnkle.write(80);  // เท้าซ้าย
  int leftAnkPos = 90;
  leftKnee.write(90);  // เข่าซ้าย
  int leftKneePos = 90;
  leftThigh.write(80);  // ต้นขาซ้าย
  int leftThighPos = 80;
  leftHip.write(92);  // สะโพกซ้าย
  int leftHipPos = 92;

  rightShoulder.write(90); // ไหล่ขวา
  leftShoulder.write(90); // ไหล่ซ้าย
  rightElbow.write(90); // ข้อศอกขวา
  leftElbow.write(90);  // ข้อศอกซ้าย

}

int rightShoulderPos = 180;
int rightElbowPos = 90;
int leftShoulderPos = 180;
int leftElbowPos = 90;

void handDemo()
{
  stand();
  delay(1000);
  handsHalf();
  delay(250);
  handsRight();
  delay(250);
  handsLeft();
  delay(250);
  handsRight();
  delay(250);
  handsHalfdown();
  delay(250);
  rightHandsUp();
  delay(250);
  rightHandsDown();
  delay(250);
  leftHandsUp();
  delay(250);
  leftHandsDown();
  delay(250);
  handWave();

}
void handsUp()
{

  for (int i = 0; i < 18; i += 1)
  {
    rightShoulderPos -= 9;
    leftShoulderPos -= 10;
    leftShoulder.write(leftShoulderPos);
    rightShoulder.write(rightShoulderPos );
    delay(50);
  }
}

void leftHandsUp()
{

  for (int i = 0; i < 18; i += 1)
  {

    leftShoulderPos -= 10;
    leftShoulder.write(leftShoulderPos);

    delay(50);
  }
}
void leftHandsDown()
{

  for (int i = 0; i < 18; i += 1)
  {

    leftShoulderPos += 10;
    leftShoulder.write(leftShoulderPos);

    delay(50);
  }
}
void rightHandsUp()
{

  for (int i = 0; i < 18; i += 1)
  {
    rightShoulderPos -= 9;
    rightShoulder.write(rightShoulderPos );
    delay(50);
  }
}
void rightHandsDown()
{

  for (int i = 0; i < 18; i += 1)
  {
    rightShoulderPos += 9;
    rightShoulder.write(rightShoulderPos );
    delay(50);
  }
}
void handsHalf()
{

  for (int i = 0; i < 9; i += 1)
  {
    rightShoulderPos -= 9;
    leftShoulderPos -= 10;
    leftShoulder.write(leftShoulderPos);
    rightShoulder.write(rightShoulderPos );
    delay(50);
  }
}

void handsHalfdown()
{

  for (int i = 0; i < 9; i += 1)
  {
    rightShoulderPos += 9;
    leftShoulderPos += 10;
    leftShoulder.write(leftShoulderPos);
    rightShoulder.write(rightShoulderPos );
    delay(50);
  }
}

void handsDown()
{
  for (int i = 0; i < 18; i += 1)
  {
    rightShoulderPos += 9;
    leftShoulderPos += 10;
    leftShoulder.write(leftShoulderPos);
    rightShoulder.write(rightShoulderPos );
    delay(25);
  }
}




void handWave()
{
  leftThigh.write(75);
  rightThigh.write(90);

  handsUp();


  for (int i = 0; i < 3; i++)
  {
    handsRight();
    handsLeft();
    handsRight();
  }
  delay(250);

  handsDown();

}



void handsRight()
{
  for (int i = 0; i < 9; i += 1)
  {
    rightElbowPos += 7;
    leftElbowPos += 7;
    leftElbow.write(leftElbowPos);
    rightElbow.write(rightElbowPos );
    delay(25);
  }
}

void handsLeft()
{
  for (int i = 0; i < 18; i += 1)
  {
    rightElbowPos -= 7;
    leftElbowPos -= 7;
    leftElbow.write(leftElbowPos);
    rightElbow.write(rightElbowPos );
    delay(25);
  }
}


void hello()
{
  leftHandsUp();
  for (int i = 0; i < 3; i += 1)
  {
    for (int i = 0; i < 9; i += 1)
    {

      leftElbowPos += 7;
      leftElbow.write(leftElbowPos);
      delay(25);
    }

    for (int i = 0; i < 9; i += 1)
    {

      leftElbowPos -= 7;
      leftElbow.write(leftElbowPos);
      delay(25);
    }

    for (int i = 0; i < 9; i += 1)
    {

      leftElbowPos -= 7;
      leftElbow.write(leftElbowPos);
      delay(25);
    }

    for (int i = 0; i < 9; i += 1)
    {

      leftElbowPos += 7;
      leftElbow.write(leftElbowPos);
      delay(25);
    }
  }
  delay(500);
  leftHandsDown();
}



///////////////Legs//////////////////////




void legs()
{
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 2;
    rightAnkPos -= 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(delayVal2);
  }
  delay(10);
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos -= 2;
    rightAnkPos += 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    delay(delayVal2);
  }
  delay(10);
  for (int i = 0; i < 6 ; i += 1)
  {
    leftAnkPos -= 2;
    rightAnkPos += 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    delay(delayVal2);
  }
  delay(10);
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 2;
    rightAnkPos -= 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(delayVal2);
  }
  delay(10);
}


////////////FORWARD///////////////////////////////////////////////////////

void forward()
{

  for (int i = 0; i < 5; i += 1)
  {

    if (time) {
      leftThighPos -= 2;
      leftKneePos += 2;
      leftThigh.write( leftThighPos);
      leftKnee.write(leftKneePos );

      delay(delayVal2);
    }
  }



  //Left and Right Ankle Bend Left
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 2;
    rightAnkPos += 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(delayVal2);
  }

  delay(delayVal);



  //Straighten Left Leg
  for (int i = 0; i < 5; i += 1)
  {

    leftThighPos += 2;
    leftKneePos -= 2;
    leftThigh.write( leftThighPos);
    leftKnee.write(leftKneePos );



    delay(delayVal2);
  }

  //right leg forward
  for (int i = 0; i < 5; i += 1)
  {
    rightThighPos += 2;
    rightKneePos -= 2;
    rightThigh.write( rightThighPos);
    rightKnee.write(rightKneePos );

    delay(delayVal2);
  }



  //Right and Left Ankle back to Normal
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos -= 2;
    rightAnkPos -= 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    delay(delayVal2);
  }






  //Right and left Ankle Bend right

  for (int i = 0; i < 6 ; i += 1)
  {
    leftAnkPos -= 2;
    rightAnkPos -= 2;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    delay(delayVal2);
  }

  delay(delayVal);

  //Straighten Right Leg
  for (int i = 0; i < 5; i += 1)
  {


    rightThighPos -= 2;
    rightKneePos += 2;
    rightThigh.write( rightThighPos);
    rightKnee.write(rightKneePos );

    delay(delayVal2);
  }



  //Left Leg bend Forward
  for (int i = 0; i < 5; i += 1)
  {
    leftThighPos -= 2;
    leftKneePos += 2;
    leftThigh.write( leftThighPos);
    leftKnee.write(leftKneePos );


    delay(delayVal2);
  }

  //Ankles back to normal
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 2;
    rightAnkPos += 2;
    rightAnkle.write(rightAnkPos);
    leftAnkle.write(leftAnkPos);
    delay(delayVal2);
  }

  time = false;

  /*

  */
}







///////////////LEFT/////////////////////////////////////////


void turnLeft()
{

  for (int i = 0; i < 5; i += 1)
  {
    leftThighPos -= 2;
    leftKneePos += 2;
    leftThigh.write( leftThighPos);
    leftKnee.write(leftKneePos );
    rightShoulder.write(90); // ไหล่ขวา
    leftShoulder.write(180); // ไหล่ซ้าย
    delay(60);
  }

  //Left and Right Ankle Bend Left
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    rightShoulder.write(180); // ไหล่ขวา
    leftShoulder.write(90); // ไหล่ซ้าย
    delay(60);
  }

  delay(delayVal);

  //Left Hip Counter ClockWise,

  for (int i = 0; i < 5; i += 1)
  {
    leftHipPos -= 4;
    leftHip.write(leftHipPos);



    delay(60);
  }

  delay(delayVal);

  //Left and Right Ankle back to normal
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }



  //Left and Right Ankle Bend Right Left Hip Straighten
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    leftHipPos += 4;
    leftHip.write(leftHipPos);

    delay(60);
  }

  delay(delayVal);



  //Right Hip Counter Clockwise
  for (int i = 0; i < 5; i += 1)
  {
    rightShoulder.write(90); // ไหล่ขวา
    rightHipPos -= 4;

    rightHip.write(rightHipPos);
    leftHipPos += 4;

    leftHip.write(leftHipPos);

    delay(60);
  }

  delay(delayVal);


  //Left and Right Ankle Normal
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }

  delay(delayVal);

  ///////SECOND TIME///////////////

  //Left and Right Ankle BEND LEFT
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    leftThighPos += 2;
    leftKneePos -= 2;
    leftThigh.write( leftThighPos);
    leftKnee.write(leftKneePos );

    delay(60);
  }

  delay(delayVal);



  //LEFT HIP CC
  for (int i = 0; i < 5; i += 1)
  {
    leftHipPos -= 4;
    leftHip.write(leftHipPos);

    rightHipPos += 4;

    rightHip.write(rightHipPos);
    rightShoulder.write(90); // ไหล่ขวา
    leftShoulder.write(180); // ไหล่ซ้าย
    delay(60);
  }

  delay(delayVal);




  //LEFT RIGHT ANKLE NORMAL
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }



  //Left and Right Ankle Bend Right
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );



    delay(60);
  }

  delay(delayVal);


  //Right Hip Counter Clockwise
  for (int i = 0; i < 5; i += 1)
  {

    rightHipPos -= 5;

    rightHip.write(rightHipPos);

    leftHipPos += 2;
    leftHip.write(leftHipPos);

    delay(60);
  }


  //LEFT RIGHT ANKLE NORMAL
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }

  //LEFT RIGHT ANKLE BEND LEFT
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    rightHipPos += 5;
    rightHip.write(rightHipPos);
    leftHipPos -= 2;
    leftHip.write(leftHipPos);

    delay(60);
  }


  //LEFT RIGHT ANKLE STRAIGHT
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }
}


/////////////////TURN RIGHT///////////////////////////////////////////////

void turnRight ()
{
  for (int i = 0; i < 5; i += 1)
  {
    rightThighPos += 1;
    rightKneePos -= 1;
    rightThigh.write( rightThighPos);
    rightKnee.write(rightKneePos );
    delay(60);
  }

  //Left and Right Ankle Bend Right
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }

  delay(delayVal);



  //right Hip ClockWise, Left leg forward
  for (int i = 0; i < 5; i += 1)
  {
    rightHipPos += 4;
    rightHip.write(rightHipPos);

    delay(60);
  }

  delay(delayVal);


  //Left and Right Ankle back to normal
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }




  //Left and Right Ankle Bend Leftt right Hip Straighten
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    rightHipPos -= 4;
    rightHip.write(rightHipPos);
    delay(60);
  }

  delay(delayVal);


  //Left Hip Counter Clockwise
  for (int i = 0; i < 5; i += 1)
  {

    rightHipPos -= 4;

    rightHip.write(rightHipPos);
    leftHipPos += 4;

    leftHip.write(leftHipPos);

    delay(60);
  }

  delay(delayVal);

  //Left and Right Ankle Normal
  for (int i = 0; i < 5; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }

  delay(delayVal);

  //Second Time

  //Left and Right Ankle BEND RIGHT, Right Thigh/Knee Straighten
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );


    delay(60);
  }

  delay(delayVal);

  //Right HIP CW, Left Hip Straighten
  for (int i = 0; i < 5; i += 1)
  {


    rightHipPos += 4;
    leftHipPos -= 4;
    leftHip.write(leftHipPos);
    rightHip.write(rightHipPos);

    delay(60);
  }

  delay(delayVal);

  //LEFT RIGHT ANKLE NORMAL
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }
  ////////Third Time //////////

  //Left and Right Ankle Bend Left
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );


    delay(60);
  }

  delay(delayVal);


  //Left Right Hip CW
  for (int i = 0; i < 5; i += 1)
  {

    rightHipPos -= 5;

    rightHip.write(rightHipPos);

    leftHipPos += 2;
    leftHip.write(leftHipPos);

    rightThighPos -= 1;
    rightKneePos += 1;
    rightThigh.write( rightThighPos);
    rightKnee.write(rightKneePos );

    delay(60);
  }


  //LEFT RIGHT ANKLE NORMAL
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }


  //LEFT RIGHT ANKLE BEND Right
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos -= 3;
    rightAnkPos -= 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );
    delay(60);
  }

  //Right Hip  Normal
  for (int i = 0; i < 5; i += 1)
  {

    rightHipPos += 5;
    rightHip.write(rightHipPos);

    delay(60);
  }


  //Left Hip  Normal
  for (int i = 0; i < 5; i += 1)
  {


    leftHipPos -= 2;
    leftHip.write(leftHipPos);

    delay(60);
  }

  //LEFT RIGHT ANKLE STRAIGHT
  for (int i = 0; i < 6; i += 1)
  {
    leftAnkPos += 3;
    rightAnkPos += 3;
    leftAnkle.write(leftAnkPos);
    rightAnkle.write(rightAnkPos );

    delay(60);
  }

  /*

  */
}



ใส่ถ่าน  18650 จำนวน 2 ก้อน แล้วลองทดสอบ

วีดีโอผลลัพธ์การทำงานของ โปรเจค หุ่นยนต์ฮิวแมนนอยด์ 12 DOF เพื่อการศึกษา




และเพื่อเพิ่มประสิทธิภาพในการจ่ายไฟเลี้ยง เซอร์โวมอเตอร์ MG996R ต้องเพิ่ม Jack 5.5 X 2.1mm สำหรับ เพาเวอร์ซัพพลาย ของ Arduino ตัวผู้ ต่อจากรางถ่าน ขั้วบวก + สีแดง และ ขั้วลบ- สีดำ ของ แบตเตอรี่ 18650 เข้าไปยัง Jack ซัพพลายตัวเมีย ของ Arduino UNO R3 ดังรูปด้วย





เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF ก้าวเท้าขวา ไปข้างหน้า
เปิดโปรแกรม Arduino (IDE) และ Upload โค้ดนี้ ไปยัง บอร์ด Arduino UNO R3

#include <Servo.h>



//Creating Servo Objects

Servo rightAnkle; // เท้าขวา
Servo leftAnkle;  // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee;   // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh;  // ต้นขาซ้าย
Servo rightHip;  // สะโพกขวา
Servo leftHip;  // สะโพกซ้าย
Servo leftShoulder;  // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow;  // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา

void setup() {

  //Servo input pins
  rightAnkle.attach(2); // เท้าขวา
  rightKnee.attach(3);  // เข่าขวา
  rightThigh.attach(4); // ต้นขาขวา
  rightHip.attach(5);  // สะโพกขวา
  leftAnkle.attach(6);  // เท้าซ้าย
  leftKnee.attach(7); // เข่าซ้าย
  leftThigh.attach(8);  // ต้นขาซ้าย
  leftHip.attach(9);  // สะโพกซ้าย
  leftShoulder.attach(10); // ไหล่ซ้าย
  rightShoulder.attach(11); // ไหล่ขวา
  leftElbow.attach(12); // ข้อศอกซ้าย
  rightElbow.attach(13); // ข้อศอกขวา

  rightShoulder.write(90); // ไหล่ขวา

  leftShoulder.write(90); // ไหล่ซ้าย
  rightElbow.write(90); // ข้อศอกขวา
  leftElbow.write(90);  // ข้อศอกซ้าย


  rightAnkle.write(100); // เท้าขวา

  rightKnee.write(100);   // เข่าขวา
  rightThigh.write(100);  // ต้นขาขวา
  rightHip.write(90);   // สะโพกขวา
  leftAnkle.write(80);  // เท้าซ้าย
  leftKnee.write(90);  // เข่าซ้าย
  leftThigh.write(90);  // ต้นขาซ้าย
  leftHip.write(92);  // สะโพกซ้าย
  rightAnkle.write(100); // เท้าขวา


  delay(3000);



}


void loop()

{

  right_foot();

  stand ();


}



//ปรับค่าตามความเหมาะสม//


void stand ()

{

  rightShoulder.write(90); // ไหล่ขวา

  leftShoulder.write(90); // ไหล่ซ้าย
  rightElbow.write(90); // ข้อศอกขวา
  leftElbow.write(90);  // ข้อศอกซ้าย

  rightAnkle.write(100); // เท้าขวา

  rightKnee.write(100);   // เข่าขวา
  rightThigh.write(100);  // ต้นขาขวา
  rightHip.write(90);   // สะโพกขวา
  leftAnkle.write(80);  // เท้าซ้าย
  leftKnee.write(90);  // เข่าซ้าย
  leftThigh.write(90);  // ต้นขาซ้าย
  leftHip.write(92);  // สะโพกซ้าย
  rightAnkle.write(100); // เท้าขวา


}



void right_foot ()

{

  rightAnkle.write(65); // บิดเท้าขวาเพื่อส่งเท้าซ้าย

  leftAnkle.write(65);  // บิดเท้าซ้ายเพื่อยืน
  rightKnee.write(80);   // ปรับเข่าขวา
  delay(1000);
  rightAnkle.write(100); // เท้าขวาคืนปรกติ
  rightThigh.write(130);  // ต้นขาขวาเครื่อนไปข้างหน้า
  delay(1000);

  leftAnkle.write(80);  // เท้าซ้ายปรกติ

  leftKnee.write(80);  // ปรับเข่าซ้าย
  delay(1000);
  rightThigh.write(100);  // ต้นขาขวาปรกติ
  rightKnee.write(100);   // เข่าขวาปรกติ
}




วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF 
ก้าวเท้าขวา ไปข้างหน้า



เราจะลองเขียนโค้ดง่ายๆ เพื่อให้ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน

#include <Servo.h>



//Creating Servo Objects
Servo rightAnkle; // เท้าขวา
Servo leftAnkle;  // เท้าซ้าย
Servo rightKnee; // เข่าขวา
Servo leftKnee;   // เข่าซ้าย
Servo rightThigh; // ต้นขาขวา
Servo leftThigh;  // ต้นขาซ้าย
Servo rightHip;  // สะโพกขวา
Servo leftHip;  // สะโพกซ้าย
Servo leftShoulder;  // ไหล่ซ้าย
Servo rightShoulder; // ไหล่ขวา
Servo leftElbow;  // ข้อศอกซ้าย
Servo rightElbow; // ข้อศอกขวา

void setup() {
  //Servo input pins
  rightAnkle.attach(2); // เท้าขวา
  rightKnee.attach(3);  // เข่าขวา
  rightThigh.attach(4); // ต้นขาขวา
  rightHip.attach(5);  // สะโพกขวา
  leftAnkle.attach(6);  // เท้าซ้าย
  leftKnee.attach(7); // เข่าซ้าย
  leftThigh.attach(8);  // ต้นขาซ้าย
  leftHip.attach(9);  // สะโพกซ้าย
  leftShoulder.attach(10); // ไหล่ซ้าย
  rightShoulder.attach(11); // ไหล่ขวา
  leftElbow.attach(12); // ข้อศอกซ้าย
  rightElbow.attach(13); // ข้อศอกขวา

  rightShoulder.write(90); // ไหล่ขวา
  leftShoulder.write(90); // ไหล่ซ้าย
  rightElbow.write(90); // ข้อศอกขวา
  leftElbow.write(90);  // ข้อศอกซ้าย


  rightAnkle.write(100); // เท้าขวา
  rightKnee.write(105);   // เข่าขวา
  rightThigh.write(110);  // ต้นขาขวา
  rightHip.write(90);   // สะโพกขวา
  leftAnkle.write(80);  // เท้าซ้าย
  leftKnee.write(90);  // เข่าซ้าย
  leftThigh.write(80);  // ต้นขาซ้าย
  leftHip.write(92);  // สะโพกซ้าย
  rightAnkle.write(100); // เท้าขวา
  delay(3000);


}

void loop()
{


  right_foot ();
  stand ();

  left_foot ();
  stand ();

}


//ปรับค่าตามความเหมาะสม//

void stand ()
{

  rightShoulder.write(90); // ไหล่ขวา
  leftShoulder.write(90); // ไหล่ซ้าย
  rightElbow.write(90); // ข้อศอกขวา
  leftElbow.write(90);  // ข้อศอกซ้าย

  rightAnkle.write(100); // เท้าขวา
  rightKnee.write(105);   // เข่าขวา
  rightThigh.write(110);  // ต้นขาขวา
  rightHip.write(90);   // สะโพกขวา
  leftAnkle.write(80);  // เท้าซ้าย
  leftKnee.write(90);  // เข่าซ้าย
  leftThigh.write(80);  // ต้นขาซ้าย
  leftHip.write(92);  // สะโพกซ้าย
  rightAnkle.write(100); // เท้าขวา


}



void right_foot ()
{

  rightAnkle.write(60); // บิดเท้าขวาเพื่อส่งเท้าซ้าย
  leftAnkle.write(60);  // บิดเท้าซ้ายเพื่อยืน
  rightKnee.write(80);   // ปรับเข่าขวา
  delay(1000);
  rightAnkle.write(100); // เท้าขวาคืนปรกติ
  rightThigh.write(130);  // ต้นขาขวาเครื่อนไปข้างหน้า
  delay(1000);
  leftAnkle.write(80);  // เท้าซ้ายปรกติ
  delay(1000);
  rightThigh.write(100);  // ต้นขาขวาปรกติ
  rightKnee.write(100);   // เข่าขวาปรกติ
}

void left_foot ()
{

  leftAnkle.write(100); // บิดเท้าซ้ายเพื่อส่งเท้าขวา
  rightAnkle.write(125); // บิดเท้าขวาเพื่อยืน
  leftKnee.write(120);  // เข่าซ้าย
  delay(1000);
  leftAnkle.write(70);  // เท้าซ้ายคืนปรกติ
  leftThigh.write(50);  // ต้นขาซ้ายเครื่อนไปข้างหน้า
  delay(1000);
  rightAnkle.write(100); // เท้าขวาคืนปรกติ
  delay(1000);
  leftThigh.write(90);  // ต้นขาซ้ายปรกติ
  leftKnee.write(90);  // เข่าซ้ายปรกติ
}

วีดีโอผลลัพธ์การทำงานของ หุ่นยนต์ฮิวแมนนอยด์ 12 DOF หัดเดิน