ข้ามไปที่เนื้อหาหลัก

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


ป้ายกำกับ:

โพสต์ยอดนิยมจากบล็อกนี้

การใช้งาน IR Infrared Obstacle Avoidance Sensor Module

โมดูลเซ็นเซอร์แสงสำหรับตรวจจับวัตถุกีดขวาง   IR Infrared Obstacle Avoidance Sensor Module โมดูลเซ็นเซอร์แสงสำหรับตรวจจับวัตถุกีดขวาง    IR Infrared Obstacle Avoidance Sensor Module โดยโมดูลนี้ จะมีตัวรับและตัวส่ง infrared ในตัว ตัวสัญญาณ(สีขาว) infrared จะส่งสัญญาณออกมา และเมื่อมีวัตถุมาบัง คลื่นสัญญาณ infrared  ที่ถูกสั่งออกมาจะสะท้องกลับไปเข้าตัวรับสัญญาณ (สีดำ) สามารถนำมาใช้ตรวจจับวัตถุที่อยู่ตรงหน้าได้ และสามารถปรับความไว ระยะการตรวจจับ ใกล้หรือไกลได้ ภายตัวเซ็นเซอร์แบบนี้จะมีตัวส่ง Emitter และ ตัวรับ Receiver ติดตั้งภายในตัวเดียวกัน ทำให้ไม่จำเป็นต้องเดินสายไฟทั้งสองฝั่ง เหมือนแบบ Opposed Mode ทำให้การติดตั้งใช้งานได้ง่ายกว่า แต่อย่างไรก็ตามจำเป็นต้องติดตั้งตัวแผ่นสะท้อนหรือ Reflector ไว้ตรงข้ามกับตัวเซ็นเซอร์เอง โดยโฟโต้เซ็นเซอร์แบบที่ใช้แผ่นสะท้อนแบบนี้จะเหมาะสำหรับชิ้นงานที่มีลักษณะทึบแสงไม่เป็นมันวาว เนื่องจากอาจทำให้ตัวเซ็นเซอร์เข้าใจผิดว่าเป็นตัวแผ่นสะท้อน และ ทำให้ทำงานผิดพลาดได้ เซ็นเซอร์แบบนี้จะมีช่วงในการทำงาน หรือ ระยะในการตรวจจับจะได้ใกล้กว่าแบบ O
อ่านเพิ่มเติม

การติดตั้ง Library ของ DHT Sensor DHT11 , DHT21 , DHT22

การติดตั้ง Library ของ DHT Sensor ไลบรารี DHT ใช้สำหรับในการให้เซ็นเซอร์ DHT  อ่านอุณหภูมิและความชื้นด้วย  Arduino หรือ ESP8266 ได้ คลิกที่นี่เพื่อดาวน์โหลดไลบรารี ของเซ็นเซอร์ DHT https://github.com/adafruit/DHT-sensor-library เปิดโปรแกรม Arduino IDE  ไปที่ Skecth -> Include Library -> Add .ZIP Library... ไปที่ ไลบรารี DHT-sensor-library ที่เรา ดาวน์โหลด มา ตรวจสอบที่ Skecth -> Include Library  จะพบ ไลบรารี DHT sensor library เพิ่มเข้ามาใน Arduino IDE ของเรา ไปที่ Skecth -> Include Library -> Manage Libraries... ไปที่ช่องค้นหา พิมพ์ DHT -> Enter (เพื่อค้นหา DHT sensor library ) เมื่อพบ DHT sensor library แล้ว ให้คลิก More info คลิกที่ Select Vers.. ในตัวอย่าง เลือก Version 1.2.3 คลิก Install คลิก Close เพิ่ม #include <DHT.h> ไปที่ส่วนบนสุดของโค้ด #include <DHT.h> void setup() {   // put your setup code here, to run once: } void loop() {   // put your main
อ่านเพิ่มเติม

ESP32 #2: การติดตั้ง Arduino core for ESP32 WiFi chip

ในบทความนี้จะเป็นการแนะนำการติดตั้งโปรแกรม Arduino IDE ตั้งแต่ต้น ไปจนถึงการติดตั้งชุดพัฒนา Arduino core for ESP32 WiFi chip และ การตรวจสอบว่าติดตั้งสำเร็จหรือไม่ “Arduino” แต่เดิมเป็นแพลตฟอร์มที่ใช้ในการพัฒนาเฟิร์มแวร์ให้กับบอร์ด Arudino เท่านั้น แต่ภายหลังกลุ่มผู้พัฒนาโปรแกรม Arduino IDE ได้เริ่มรองรับการติดตั้งชุดพัฒนาเฟิร์มแวร์ให้กับบอร์ดอื่น ๆ ด้วย ทำให้บอร์ดอื่น ๆ ที่รองรับการเขียนโปรแกรมด้วยภาษา C/C++ สามารถเข้ามาใช้โปรแกรม Arduino IDE ในการพัฒนาได้ นอกจากข้อดีของโปรแกรม Arduino IDE แล้ว ชุดไลบารี่ต่าง ๆ ที่ทำมารองรับกับแพลตฟอร์ม Arduino ก็จะสามารถนำมาใช้งานกับบอร์ดอื่น ๆ ได้ด้วย การจะใช้ Arduino core for ESP32 กับ Arduino IDE ได้นั้น มีขั้นตอนดังนี้ คือ 1. ติดตั้งโปรแกรม Arduino (IDE) ลิงค์ดาวโหลด Arduino (IDE)  https://www.arduino.cc/en/Main/Software 2. ติดตั้ง แพลตฟอร์ม ESP32 ในการเริ่มต้นเราจะต้องอัปเดตผู้จัดการบอร์ดด้วย URL ที่กำหนดเอง เปิด Arduino IDE และไปที่ File > Preferences คัดลอก URL ด้านล่างลงใน Additional Board Manager URLs: แล้ว คลิก
อ่านเพิ่มเติม