โปรเจกต์ Arduino - การควบคุม Motor PID Speed Control

ลิงก์สำหรับซื้อ:

1. Motor พร้อม encoder: https://amzn.to/2NwsNxH

2. H-bridge: https://amzn.to/2QS2FeI

3. Arduino Pro Mini: https://amzn.to/2xy4yFn

ซื้อ electronic component บน utsource.net

บทนำ

ด้วยการควบคุมแบบ PID เราสามารถควบคุมความเร็วของ Motor ได้อย่างแม่นยำ บทความนี้จะแนะนำการเขียนโปรแกรมสำหรับ Arduino Pro Mini บนคอมพิวเตอร์ของคุณ (โดยใช้ Visual Studio) เพื่อควบคุมความเร็วของ Motor ด้วยอัลกอริทึม PID

Arduino Pro Mini ถูกใช้เพื่อเก็บส่วนควบคุม Motor, อัลกอริทึม PID และสื่อสารกับ PC (ผ่าน COM Port) ส่วนคอมพิวเตอร์ควรมี HMI ที่สร้างด้วย Visual Studio เพื่อสื่อสารกับ Arduino โดย HMI จะแสดงกราฟความเร็วของ Motor และปรับเปลี่ยนการตั้งค่าความเร็วได้

เรามาดูรายละเอียดกัน

Hardware ที่จำเป็น:

1. Motor พร้อม Encoder

2. PCB สำหรับ H-bridge

3. Arduino Pro Mini

4. PCB สำหรับ UART

5. คอมพิวเตอร์ (พร้อม Visual Studio)

ขั้นตอนที่ 1. การเชื่อมต่อ Hardware

บทความนี้ไม่ได้กล่าวถึงรายละเอียดการเชื่อมต่อ โดยจะอ้างอิงจากบทความก่อนหน้านี้ซึ่งสามารถดูได้ที่ลิงก์นี้

ขั้นตอนที่ 2. โค้ดสำหรับ Arduino

void loop() {

   if (stringComplete) {

     // ล้างค่า string เมื่อการรับข้อมูลทาง COM เสร็จสิ้น

     mySt = "";  //หมายเหตุ: ในโค้ดด้านล่าง mySt จะไม่ว่างเปล่า จนกว่าจะได้รับ '\n'

     stringComplete = false;

   }

  //รับคำสั่งจาก Visual Studio

   if (mySt.substring(0,8) == "vs_start"){

     digitalWrite(pin_fwd,1);      //ให้ Motor หมุนไปข้างหน้า

     digitalWrite(pin_bwd,0);

     motor_start = true;

   }

   if (mySt.substring(0,7) == "vs_stop"){

     digitalWrite(pin_fwd,0);

     digitalWrite(pin_bwd,0);      //หยุด Motor

     motor_start = false;

   }

   if (mySt.substring(0,12) == "vs_set_speed"){

     set_speed = mySt.substring(12,mySt.length()).toFloat();  //รับค่า string หลังจาก set_speed

   }

   if (mySt.substring(0,5) == "vs_kp"){

     kp = mySt.substring(5,mySt.length()).toFloat(); //รับค่า string หลังจาก vs_kp

   }

   if (mySt.substring(0,5) == "vs_ki"){

     ki = mySt.substring(5,mySt.length()).toFloat(); //รับค่า string หลังจาก vs_ki

   }

   if (mySt.substring(0,5) == "vs_kd"){

     kd = mySt.substring(5,mySt.length()).toFloat(); //รับค่า string หลังจาก vs_kd

   } 

 }

void detect_a() {

   encoder+=1; //เพิ่มค่า encoder เมื่อมี Pulse ใหม่

   m_direction = digitalRead(pin_b); //อ่านทิศทางของ Motor

 }

 ISR(TIMER1_OVF_vect)        // interrupt service routine - ทำงานทุกๆ 0.1 วินาที

 {

   TCNT1 = timer1_counter;   // ตั้งค่า timer

   pv_speed = 60.0*(encoder/200.0)/0.1;  //คำนวณความเร็ว Motor หน่วยเป็น rpm

   encoder=0;

   //แสดงค่าความเร็ว

   if (Serial.available() <= 0) {

     Serial.print("speed");

     Serial.println(pv_speed);         //ส่งค่าความเร็ว (rpm) ไปยัง Visual Studio

     }

  //โปรแกรม PID

   if (motor_start){

     e_speed = set_speed - pv_speed;

     pwm_pulse = e_speed*kp + e_speed_sum*ki + (e_speed - e_speed_pre)*kd;

     e_speed_pre = e_speed;  //บันทึกค่าความคลาดเคลื่อนล่าสุด (ก่อนหน้า)

     e_speed_sum += e_speed; //ผลรวมของความคลาดเคลื่อน

     if (e_speed_sum >4000) e_speed_sum = 4000;

   if (e_speed_sum <-4000) e_speed_sum = -4000;

   }

   else{

     e_speed = 0;

     e_speed_pre = 0;

     e_speed_sum = 0;

     pwm_pulse = 0;

   }

  //อัปเดตความเร็วใหม่

   if (pwm_pulse <255 & pwm_pulse >0){

     analogWrite(pin_pwm,pwm_pulse);  //ตั้งค่าความเร็ว Motor 

   }

   else{

     if (pwm_pulse>255){

       analogWrite(pin_pwm,255);

     }

     else{

       analogWrite(pin_pwm,0);

     }

   }

}

ในช่วงเริ่มต้นของโปรแกรม จะเป็นการรับคำสั่งจากคอมพิวเตอร์ (เริ่ม/หยุด Motor, การตั้งค่าความเร็ว Motor, ค่า kP, kI, kD gain ของ PID) ถัดมาคือ void detect_a(): ซึ่งเป็นส่วนของ encoder สำหรับการคำนวณผลรวมที่ใช้ในการคำนวณความเร็วใน Timer interrupt routine ส่วน Timer interrupt routine ISR(TIMER1_OVF_vect): จะถูกเรียกใช้งานทุกๆ 0.1 วินาที โดยประกอบด้วย: (1) คำนวณความเร็ว Motor (2) ส่งความเร็ว Motor ไปยังคอมพิวเตอร์ (3) คำนวณ PWM pulse (อ้างอิงตามอัลกอริทึม PID) (4) ส่งผลลัพธ์ของ PWM ไปยัง H-brigde สามารถดาวน์โหลดโค้ดทั้งหมดสำหรับ Arduino Pro Mini ได้ที่ลิงก์นี้.

ขั้นตอนที่ 3. โค้ดบนคอมพิวเตอร์

ใช้ Visual Studio 2012 ในการสร้างโปรแกรม HMI ซึ่งมีหน้าที่: (1) ส่งการตั้งค่าความเร็วไปยัง Arduino    (2) ส่ง PID gain (kP, kI, kD) ไปยัง Arduino (3) รับความเร็ว Motor -> แสดงผลบนกราฟ

สามารถดาวน์โหลดโค้ดทั้งหมดของโปรแกรม Visual Studio ได้ที่ลิงก์นี้ สำหรับการสร้างโปรแกรม Visual Studio สามารถดูขั้นตอนอย่างละเอียดได้ในบทความนี้ โดยทั่วไปโค้ดจะมีลักษณะดังนี้:

#pragma endregion

 private: System::Void Form1_Load(System::Object^  sender, System::EventArgs^  e) {

 serialPort1->Open();

 timer1->Start();

 mStr = "0";

 i=300;

 }

 private: System::Void button1_Click(System::Object^  sender, System::EventArgs^  e) {

 serialPort1->WriteLine("vs_set_speed"+textBox1->Text); //ส่งค่า set_speed ไปยัง Arduino

 serialPort1->WriteLine("vs_kp"+textBox2->Text); //ส่งค่า kP ไปยัง Arduino

 serialPort1->WriteLine("vs_ki"+textBox3->Text); //ส่งค่า kI ไปยัง Arduino

 serialPort1->WriteLine("vs_kd"+textBox4->Text); //ส่งค่า kD ไปยัง Arduino

 }

 private: System::Void timer1_Tick(System::Object^  sender, System::EventArgs^  e) {

 String^ length;

 length=mStr->Length.ToString();

 if(mStr->Substring(0,5)=="speed"){

 speed=mStr->Substring(5,System::Convert::ToInt32(length)-6);

 label1->Text=speed;

 //แสดงความเร็ว Motor บน Chart

 this->chart1->Series["Series1"]->Points->AddXY(i,System::Convert::ToDouble(speed));

 i++;

 this->chart1->ChartAreas["ChartArea1"]->AxisX->Minimum=i-300; //เลื่อนแกน X

 }

 }

 private: System::Void serialPort1_DataReceived(System::Object^  sender, System::IO::Ports::SerialDataReceivedEventArgs^  e) {

 mStr=serialPort1->ReadLine();

 }

 private: System::Void button2_Click(System::Object^  sender, System::EventArgs^  e) {

 serialPort1->WriteLine("vs_start"); //เริ่มทำงาน Motor

 }

 private: System::Void button3_Click(System::Object^  sender, System::EventArgs^  e) {

 serialPort1->WriteLine("vs_stop");  //หยุดการทำงาน Motor

 }

ผลลัพธ์