คำอธิบาย
EDPI10 ชุดเรียนรู้+ชุดทดลอง เพนดูลัมผกผันพีไอดี Arduino PID Controled Inverted Pendulum
เข้าใจการทำงาน ระบบแพนดูลัมผกผัน ระบบที่มีจุดสมดุลอยู่รอบแกนหมุนด้วยกันสองจุด ได้แก่จุดที่เพนดูลัมตั้งตรงอยู่ในแนวดิ่ง และ จุดที่เพนดูลัมอยู่ทิ้งตัวลงในดิ่ง เพนดูลัมผกผัน เป็นปัญหาพื้นฐานที่ใช้ในการเรียนการสอนและในการสาธิตการประยุกต์ทฤษฎีระบบควบคุม ซึ่งเป็นแบบจําลองของการควบคุมระบบที่ไม่เป็นเชิงเส้นและไม่มีเสถียรภาพ เป็นสาขาหนึ่งของคณิตศาสตร์และวิศวกรรมศาสตร์ ผ่าน ทฤษฎีระบบควบคุม PID
ทฤษฎีการควบคุมและพลศาสตร์
เพนดูลัมผกผัน Inverted Pendulum เป็นปัญหาคลาสสิกในวิชาพลศาสตร์และทฤษฎีการควบคุมซึ่งโดยทั่วไปแล้วจะอธิบายไว้ในวิชาฟิสิกส์หรือคณิตศาสตร์ระดับมัธยมปลายและระดับปริญญาตรี ชุดทดลอง ช่วยเสริมสร้างความเข้าใจแนวคิด ทั้งยังทำให้มองเห็น มิติใหม่ของปัญหาและความท้าทายที่เกี่ยวข้องกับการปฏิบัติจริงและสถานการณ์ในชีวิตจริงที่ไม่มีใครสามารถพบได้ในชั้นเรียนทฤษฎี ระบบควบคุม Inverted Pendulum Inverted Pendulum เป็นปัญหาพื้นฐานที่ใช้ในการเรียนการสอนและในการสาธิตการประยุกต์ทฤษฎีระบบควบคุม Inverted Pendulum เป็นระบบที่มีจุดสมดุลอยู่รอบแท่งหมุนด้วยกันสองจุด ได้แก่จุดที่ Pendulum ตั้งตรงอยู่ในแนวดิ่ง และจุดที่ Pendulum อยู่ทิ้งตัวลงในดิ่ง จุดที่มีเสถียรภาพเมื่อไม่มีตัวควบคุมนั้นจะมีจุดเดียวคือ จุดที่แท่งทิ้งตัวลงเท่านั้น ไม่ว่าเราจะปล่อย Pendulum ที่จุดใดก็ตาม Pendulum จะตกลงสู่จุดนี้เสมอ
การใช้งาน
- จ่ายไฟ +12V จาก อะแดปเตอร์เข้าวงจรควบคุม (Control Board)
- เลื่อน Cart มายังตรงกลางของราง
- จับแท่ง Pendulum ให้ตั้งตรง
- เสียบสาย USB เพื่อจ่ายไฟ ให้ Arduino
- ดาวน์โหลด อัพเดท Arduino Source Code ได้ที่ >> https://drive.google.com/drive/folders/1XQ9czTazgo2l5IGwM1yYVXSmOOn01Z7Z?usp=sharing
- กดปุ่ม Reset ที่ Arduino
- จากนั้นปล่อยมือจาก แท่ง Pendulum ระบบควบคุม PID จะทำการเลี้ยง แท่ง Pendulum ให้ตั้งตรงโดยอัตโนมัติ
การต่อสายบอร์ดคอนโทรลและ Arduino
วัตถุประสงค์การควบคุม
การควบคุมการออกแรง F ที่แปรผันกับรถเข็น (Cart) ตัวควบคุมจำเป็นต้องรักษาระดับ แท่ง Pendulum ให้ตั้งตรงในขณะที่ Cart เคลื่อนไปยังตำแหน่งใหม่ แม้แท่ง Pendulum ถูกรบกวนด้วยแรงกระทำ จากภายนอก dF (Disturbance Forces) ก็ตาม ตัวควบคุมก็จำเป็นที่จะต้องรักษาระดับแท่ง Pendulum ให้ตั้งตรงอยู่ให้ได้
- เมื่อ Cart จอดนิ่งอยู่กับที่ x = 0
- เมื่อแท่ง Pendulum หยุดนิ่งที่ตำแหน่งตั้งตรง มุม Ɵ = 0 องศา
- Cart จะเคลื่อนที่อยู่ระหว่างช่วง 0 ถึง 2,000 เพื่อรักษาแท่ง Pendulum ให้ตั้งตรง
- เมื่อแท่ง Pendulum ตั้งตรงได้แล้ว Cart จะเคลื่อนเข้าไปอยู่ตำแหน่งกลางคือ 1000 เพื่อทดลองให้เห็นว่าระบบสามารถเลี้ยงให้แท่งตั้งตรงอยู่ได้แม้มีแรงกระทำจากภายนอก
ตัวแปร
- ตัวแปรหลักที่ต้องควบคุม คือแรง F ซึ่งกระทำได้โดยการควบความความเร็วของมอเตอร์โดยใช้ PWM
- เอาต์พุตที่วัดได้จากระบบ ตำแหน่งของ Cart
- เอาต์พุตที่วัดได้จากระบบ มุมของ ลูกตุ้ม Pendulum theta (Setpoint) วัดได้จากการอ่านค่า Potentiometer
- แรงรบกวนจากภายนอกที่ไม่สามารถวัดได้ dF
อุปกรณ์
- ชุด Inverted Pendulum
- ชุด Control Board
- Power supply DC12V
- User Manual ระบบควบคุมเพนดูลัมผกผัน PID Arduino 2023
|
1 |
ซอร์สโค้ดภาษาซีสำหรับควบคุม Inverted Pendulum |
การควบคุมระบบ (PID Control)
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 |
///////// วงจรขับ TB6612 drive pin //// #define PWM 9 //ขาส่งสัญญาณ PWM ไปยังวงจรขับ #define IN2 10 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ #define IN1 11 //ขาควบคุมทิศทางการหมุน ไปยังวงจรขับ ///////// ขาสัญญาณที่อ่านเอ็นโค้ดเดอร์ ///////// #define ENCODER_A 2// Motor encoder pin เฟส A ที่ขา 2 #define ENCODER_B 4// Motor encoder pin เฟส B ที่ขา 4 ///////// ขาสัญญาณจากโพเทนชิโอมิเตอร์ที่ขา Analog ///////// #define Analogin 1 #include <PinChangeInt.h> // ไลเบอรี่สำหรับใช้งานอินเตอร์รัปที่ขาดิจิตอลอินพุต ในที่นี้ใช้สำหรับอ่านพัลส์เอ็นโค้ดเดอร์ #include <MsTimer2.h> // ไลเบอรี่สำหรับใช้งานไทเมอร์ int Setpoint = 512; // ค่าตรงกลางของตัวโพเทนชิโอมิเตอร์ ที่ทำให้แกนตั้งฉากกับพื้น float Position = 1000; // ตัวแปร Position ค่าจะวิ่งอยู่ในช่วง +/- 500 float Sensor; // Angle displacement sensor parameters float Motor_PWM; // Motor PWM parameters อยู่ในชว่ง -255 ถึง +255 float Balance_Pwm; // Pendulum uplight loop PWM parameters ตัวแปรเอาต์พุตสัญญาณ PWM ของการควบคุมแกนเพนดูลัม float Position_Pwm; // Position loop PWM parameters int Target_Position = 1000; // Target value static float Position_Int; int EnablePendulum = 0; // Start/Stop control pendulum float Balance_KP = 300; // Balance Gain float Balance_KI = 0; // Balance Gain float Balance_KD = 200; // Balance Gain float Position_KP = 100; // Position Gain float Position_KI = 0; // Position Gain float Position_KD = 100; // Position Gain /************************************************************************** Function: PD control Entry parameters: Angle Return Value: Pendulum uplight Tilt Control PWM **************************************************************************/ float Balance_Control(float sensor){ float Error; // ตัวแปรเก็บค่า Error static float Last_Error; // ตัวแปรที่เกี่ยวข้องกับ PD control static float D_Error; // ตัวแปรที่เกี่ยวข้องกับ PD control int PD_Output; // ตัวแปรเอาต์พุตของ PD control Error = sensor - Setpoint; // หาค่า Error D_Error = Error - Last_Error; // หาค่าการเปลี่ยนแปลงชั่วขณะ(เทอม D) PD_Output = Balance_KP/10 * Error + D_Error * Balance_KD/1000; // PWM PD control for motors that calculate tilt control Last_Error = Error; // ทำให้ Errorใหม่ เป็น Error เก่า return PD_Output; // ส่งค่าเอาต์พุต } /************************************************************************** Function: Position PD control Entry parameters: Encoder Return Value: Position control PWM **************************************************************************/ float Position_Control(int Encoder){ static float Position_PWM; //Define related variables static float Last_Position; //Define related variables static float Position_Bias; //Define related variables static float Position_Differential; //Define related variables static float Position_Least; //Define related variables Position_Least = Target_Position - Encoder; // Get deviation value Position_Bias *= 0.8; // first-order low-pass filter Position_Bias += Position_Least*0.2; // first order difference Position_Int = Position_Int + Position_Least; if(Position_Int > 500)Position_Int = 500; if(Position_Int < -500)Position_Int = -500; Position_Differential = Position_Bias - Last_Position; // Keep the last deviation Last_Position = Position_Bias; // เปลี่ยนค่าล่าสุดเป็นค่าเก่า สำหรับการคำนวณรอบหน้า Position_PWM = (Position_Bias*Position_KP/1000) + (Position_Int*Position_KI/100) + (Position_Differential*Position_KD/10); // Position control return Position_PWM; //Return control } /************************************************************************** Function: ส่งค่า PWM ออกที่ขา 9 และกำหนดทิศทางการหมุนด้วยขา 10 และ 11 Entry parameters: PWM Return Value: None **************************************************************************/ void Set_PWM(int motor){ if (motor< 0) digitalWrite(IN1, LOW), digitalWrite(IN2, HIGH); // ค่าติดลบ else digitalWrite(IN1, HIGH), digitalWrite(IN2, LOW); // ค่าบวก analogWrite(PWM, abs(motor)); // ส่งสัญญาณ PWM ค่ามาก มอเตอร์หมุนเร็ว ค่าน้อย มอเตอร์หมุนช้า } /************************************************************************** Function: หาค่าเฉลี่ยของค่าที่อ่านได้จากเซนเซอร์ตรวจจับมุมเอียง Entry parameters: None Return Value: None **************************************************************************/ u16 Get_Adc_Average(u8 ch,u8 times){ unsigned int temp_val=0; unsigned char t; for(t=0;t<times;t++){ temp_val+=analogRead(ch);//อ่านค่าตำแหน่งแกนจาก Adc แล้วบวกในตัวเอง } return temp_val/times;//ครบแล้วหารด้วยค่า time } /************************************************************************** Function: ทำฟังชัน ทุกๆ 5ms control จากไทเมอร์2 Entry parameters: None Return Value: None **************************************************************************/ void control(){ static unsigned char Position_Count; // Variable for position control frequency division sei(); // เปิดให้อินเตอร์รัปต์ทำงาน Sensor= 1023 - Get_Adc_Average(1,5); // หาค่าเฉลี่ยของแรงดันที่อ่านได้จาก Adc ต่อเข้าที่ขา A1 หาค่าเฉลี่ย 5 ครั้ง Balance_Pwm = Balance_Control(Sensor); // เรียกฟังก์ชัน PD control เพื่อเลี้ยงแกนให้ตรง if(++Position_Count>4) { Position_Pwm = Position_Control(Position); // Position PD control Position_Count = 0; } Motor_PWM = Balance_Pwm - Position_Pwm; // Calculate motor final PWM if(Motor_PWM>255) Motor_PWM = +255; //ลิมิตค่า PWM มากสุด, ถ้าค่าเกิน 255 มอเตอร์จะหยุด if(Motor_PWM<-255)Motor_PWM = -255; //ลิมิตค่า PWM น้อยสุด if(Sensor<(Setpoint-200)||Sensor>(Setpoint+200)) // ถ้ามุมการเอียงมากไปจะส่งให้มอเตอร์หยุดทำงาน { Motor_PWM = 0; digitalWrite(IN1, LOW); //TB6612 level control digitalWrite(IN2, LOW); //TB6612 level control } if(EnablePendulum == 1) { Set_PWM(Motor_PWM); //Output motor control } else { Set_PWM(0); //Stop motor control } } /************************************************************************** Function: คอนฟิกพอร์ตและกำหนดค่าเริ่มต้นต่างๆ Entry parameters: None Return Value: None **************************************************************************/ voidsetup() { int fff = 1; TCCR1B =(TCCR1B & 0xF8) | fff; pinMode(IN1, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต pinMode(IN2, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต pinMode(PWM, OUTPUT); //กำหนดขาควบคุมไอซี TB6612 ให้เป็นเอาต์พุต digitalWrite(IN1, 0); //ส่งลอจิก 0 digitalWrite(IN2, 0); //ส่งลอจิก 0 digitalWrite(PWM, 0); //ส่งลอจิก 0 pinMode(ENCODER_A, INPUT); // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต pinMode(ENCODER_B, INPUT); // กำหนดขาอ่านสัญญาณเอ็นโค้ดเดอร์ให้เป็น อินพุต Serial.begin(115200); // กำหนดบอตเรตการสื่อสาร serial เพื่อส่งข้อมูลไปยังคอมพิวเตอร์ delay(200); // หน่วงเวลาเล็กน้อยเพื่อให้แน่ใจว่าการตั้งค่าต่างๆ เสร็จเรียบร้อยแล้ว MsTimer2::set(5, control); // Use Timer 2 to set 5ms timer interrupt MsTimer2::start(); // เปิดการทำงานไทเมอร์ attachInterrupt(0, READ_ENCODER_A, CHANGE); //เปิดใช้งานอินเตอร์รัปต์สำหรับอ่านค่าเอ็นโค้ดเดอร์ ทุกครั้งที่มีการเปลี่ยนแปลง จาก 0 เป็น 1 หรือ จาก 1 เป็น 0 } /************************************************************************** Function function: ส่งค่าไปที่ serial port เพื่อพล็อตกราฟ Entry parameters: None Return Value: None **************************************************************************/ void loop(){ // แสดงผลกราฟ Serial.print("\n"); //1 Serial.print(Motor_PWM); Serial.print("\t"); //2 Serial.print(Sensor); Serial.print("\t"); //3 Serial.print(Setpoint); Serial.print("\t"); //4 Serial.print(Position); Serial.print("\t"); //5 Serial.print(Target_Position); Serial.print("\t"); //6 Serial.print(Balance_KP); Serial.print("\t"); //7 Serial.print(Position_KP); Serial.print("\t"); //8 Serial.print(Position_KI); Serial.print("\t"); //9 Serial.print(Position_KD); Serial.print("\t"); //10 Serial.print(Balance_KI); Serial.print("\t"); //11 Serial.print(Balance_KD); Serial.print("\t"); //delay(5); switch (Serial.read()) { //************ Balance **************** case 'c': Setpoint = Setpoint +1; break; case 'd': Setpoint = Setpoint -1; break; case 'a': Balance_KP = Balance_KP+1; break; case 'b': Balance_KP = Balance_KP-1; break; case 'q': Balance_KI = Balance_KI+1; break; case 'r': Balance_KI = Balance_KI-1; break; case 's': Balance_KD = Balance_KD+1; break; case 't': Balance_KD = Balance_KD-1; break; //************ Position ************** case 'e': Target_Position = Target_Position +1; break; case 'f': Target_Position = Target_Position -1; break; case 'g': Position_KP = Position_KP +1; break; case 'h': Position_KP = Position_KP -1; break; case 'i': Position_KI = Position_KI +1; break; case 'j': Position_KI = Position_KI -1; break; case 'k': Position_KD = Position_KD +1; break; case 'l': Position_KD = Position_KD -1; break; //*************** Additional ***************** case 'x': EnablePendulum = 1; break; case 'y': EnablePendulum = 0; break; case 'u': Position = 1000; break; } } /************************************************************************** Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์ เป็นกรณีที่สัญญาณเฟส A นำเฟส B Entry parameters: None Return Value: None **************************************************************************/ voidREAD_ENCODER_A(){ if(digitalRead(ENCODER_A) == HIGH) { //เกิดอินเตอร์รัปต์ที่ขอบขาลง if (digitalRead(ENCODER_B) == LOW) Position--; //เพิ่มค่า ตัวแปร Position else Position++; } else { //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น if (digitalRead(ENCODER_B) == LOW) Position++; //ลดค่า ตัวแปร Position else Position--; } } /************************************************************************** Function: อ่านสัญญาณเอ็นโค้ดเดอร์ตตามการอินเตอร์รัฟต์ เป็นกรณีที่สัญญาณเฟส B นำเฟส A Entry parameters: None Return Value: None **************************************************************************/ voidREAD_ENCODER_B(){ if(digitalRead(ENCODER_B) == LOW) {//เกิดอินเตอร์รัปต์ที่ขอบขาลง if (digitalRead(ENCODER_A) == LOW) Position--;//เพิ่มค่า ตัวแปร Position else Position++; } else { //เกิดอินเตอร์รัปต์ที่ขอบขาขึ้น if (digitalRead(ENCODER_A) == LOW) Position++; //ลดค่า ตัวแปร Position else Position--; } } |
รีวิว
ยังไม่มีบทวิจารณ์