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 การเปลี่ยนแปลงความเร็ว