หุ่นยนต์ปรับสมดุลตนเองจาก Magicbit: 6 ขั้นตอน

2024 ผู้เขียน: John Day | [email protected]. แก้ไขล่าสุด: 2024-01-30 13:02

บทช่วยสอนนี้แสดงวิธีสร้างหุ่นยนต์ทรงตัวโดยใช้บอร์ด Magicbit dev เราใช้ magicbit เป็นบอร์ดพัฒนาในโปรเจ็กต์นี้ ซึ่งใช้ ESP32 ดังนั้นสามารถใช้บอร์ดพัฒนา ESP32 ในโครงการนี้ได้

เสบียง:

• magicbit
• ตัวขับมอเตอร์ H-bridge L298 คู่
• ลิเนียร์เรกูเลเตอร์ (7805)
• แบตเตอรี่ Lipo 7.4V 700mah
• หน่วยวัดแรงเฉื่อย (IMU) (6 องศาอิสระ)
• มอเตอร์เกียร์ 3V-6V DC

ขั้นตอนที่ 1: เรื่องราว

สวัสดีทุกคน วันนี้ในบทช่วยสอนนี้ เราจะเรียนรู้เกี่ยวกับสิ่งที่ซับซ้อนเล็กน้อย นั่นคือหุ่นยนต์ทรงตัวโดยใช้ Magicbit กับ Arduino IDE มาเริ่มกันเลยดีกว่า

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

ขั้นตอนที่ 2: ทฤษฎีและระเบียบวิธี

ในการปรับสมดุลของหุ่นยนต์ อันดับแรก เราได้รับข้อมูลจากเซ็นเซอร์บางตัวเพื่อวัดมุมของหุ่นยนต์เป็นระนาบแนวตั้ง เพื่อจุดประสงค์นั้น เราใช้ MPU6050 หลังจากได้รับข้อมูลจากเซ็นเซอร์แล้ว เราจะคำนวณความเอียงเป็นระนาบแนวตั้ง หากหุ่นยนต์อยู่ในตำแหน่งตรงและสมดุล มุมเอียงจะเป็นศูนย์ ถ้าไม่เช่นนั้นมุมเอียงจะเป็นค่าบวกหรือค่าลบ หากหุ่นยนต์เอียงไปด้านหน้า หุ่นยนต์ควรเคลื่อนไปข้างหน้า นอกจากนี้ หากหุ่นยนต์เอียงไปด้านหลัง หุ่นยนต์ควรเคลื่อนที่ไปในทิศทางย้อนกลับ หากมุมเอียงนี้สูง ความเร็วในการตอบสนองควรสูง ในทางกลับกัน มุมเอียงจะต่ำ จากนั้นความเร็วปฏิกิริยาควรต่ำ เพื่อควบคุมกระบวนการนี้ เราใช้ทฤษฎีบทเฉพาะที่เรียกว่า PID PID คือระบบควบคุมที่ใช้ควบคุมหลายกระบวนการ PID ย่อมาจาก 3 กระบวนการ

• P- สัดส่วน
• I- ปริพันธ์
• D- อนุพันธ์

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

อันแรกคือการคูณความผิดพลาดจากการได้รับตัวเลข กำไรนี้มักจะเรียกว่า Kp

P=ผิดพลาด*Kp

อันที่สองคือการสร้างอินทิกรัลของข้อผิดพลาดในโดเมนเวลาและคูณจากกำไรบางส่วน กำไรนี้เรียกว่า Ki

I= ปริพันธ์(ผิดพลาด)*Ki

อันที่สามคืออนุพันธ์ของข้อผิดพลาดในโดเมนเวลาและคูณด้วยกำไรจำนวนหนึ่ง กำไรนี้เรียกว่า Kd

D=(d(ผิดพลาด)/dt)*kd

หลังจากเพิ่มการดำเนินการข้างต้นแล้ว เราจะได้ผลลัพธ์สุดท้าย

ผลลัพธ์=P+I+D

เนื่องจากหุ่นยนต์ส่วน P สามารถรับตำแหน่งที่มั่นคงซึ่งเป็นสัดส่วนกับส่วนเบี่ยงเบนได้ ฉันคำนวณพื้นที่ข้อผิดพลาดเทียบกับกราฟเวลา ดังนั้นจึงพยายามทำให้หุ่นยนต์อยู่ในตำแหน่งที่มั่นคงแม่นยำเสมอ ส่วน D วัดความชันในเวลาเทียบกับกราฟข้อผิดพลาด หากข้อผิดพลาดเพิ่มขึ้น ค่านี้เป็นค่าบวก หากข้อผิดพลาดลดลง ค่านี้จะเป็นค่าลบ ด้วยเหตุนี้ เมื่อหุ่นยนต์เคลื่อนที่ไปยังตำแหน่งที่มั่นคง ความเร็วของปฏิกิริยาจะลดลง ซึ่งจะช่วยขจัดปัญหาที่ไม่จำเป็นออกไป คุณสามารถเรียนรู้เพิ่มเติมเกี่ยวกับทฤษฎี PID ได้จากลิงค์นี้ที่แสดงด้านล่าง

www.arrow.com/en/research-and-events/articles/pid-controller-basics-and-tutorial-pid-implementation-in-arduino

เอาต์พุตของฟังก์ชัน PID ถูกจำกัดไว้ที่ 0-255 ช่วง (ความละเอียด PWM 8 บิต) และจะป้อนไปยังมอเตอร์เป็นสัญญาณ PWM

ขั้นตอนที่ 3: การตั้งค่าฮาร์ดแวร์

ตอนนี้เป็นส่วนการตั้งค่าฮาร์ดแวร์ การออกแบบหุ่นยนต์ขึ้นอยู่กับคุณ เมื่อคุณออกแบบร่างกายของหุ่นยนต์ คุณต้องพิจารณาความสมมาตรเกี่ยวกับแกนแนวตั้งซึ่งอยู่ในแกนมอเตอร์ ก้อนแบตเตอรี่อยู่ด้านล่าง หุ่นยนต์จึงปรับสมดุลได้ง่าย ในการออกแบบของเรา เรายึดบอร์ด Magicbit ในแนวตั้งเข้ากับตัวเครื่อง เราใช้มอเตอร์เกียร์ 12V สองตัว แต่คุณสามารถใช้มอเตอร์เกียร์ชนิดใดก็ได้ ที่ขึ้นอยู่กับขนาดหุ่นยนต์ของคุณ

เมื่อเราพูดถึงวงจร มันใช้พลังงานจากแบตเตอรี่ Lipo 7.4V Magicbit ใช้ 5V เพื่อจ่ายไฟ ดังนั้นเราจึงใช้ตัวควบคุม 7805 เพื่อควบคุมแรงดันแบตเตอรี่เป็น 5V ใน Magicbit รุ่นที่ใหม่กว่านั้นไม่จำเป็นต้องใช้ตัวควบคุม เพราะมันจ่ายไฟได้ถึง 12V. เราจัดหา 7.4V โดยตรงสำหรับไดรเวอร์มอเตอร์

เชื่อมต่อส่วนประกอบทั้งหมดตามแผนภาพด้านล่าง

ขั้นตอนที่ 4: การติดตั้งซอฟต์แวร์

ในรหัสเราใช้ไลบรารี PID เพื่อคำนวณเอาต์พุต PID

ไปที่ลิงค์ต่อไปนี้เพื่อดาวน์โหลด

www.arduinolibraries.info/libraries/pid

ดาวน์โหลดเวอร์ชันล่าสุดของมัน

เพื่อให้อ่านค่าเซ็นเซอร์ได้ดีขึ้น เราใช้ไลบรารี DMP DMP ย่อมาจากกระบวนการเคลื่อนไหวแบบดิจิทัล นี่คือคุณสมบัติ inbuilt ของ MPU6050 ชิปนี้มีหน่วยประมวลผลการเคลื่อนไหวแบบบูรณาการ จึงต้องอ่านและวิเคราะห์ หลังจากที่สร้างเอาต์พุตที่แม่นยำไร้เสียงรบกวนไปยังไมโครคอนโทรลเลอร์ (ในกรณีนี้คือ Magicbit(ESP32)) แต่มีงานมากมายในด้านไมโครคอนโทรลเลอร์ที่จะอ่านค่านั้นและคำนวณมุม พูดง่ายๆ คือ เราใช้ไลบรารี MPU6050 DMP ดาวน์โหลดโดยไปที่ลิงค์ต่อไปนี้

github.com/ElectronicCats/mpu6050

ในการติดตั้งไลบรารี่ ในเมนู Arduino ให้ไปที่ tools-> include library->add.zip library และเลือกไฟล์ไลบรารีที่คุณดาวน์โหลด

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

เลือกพอร์ต COM ที่ถูกต้องและพิมพ์บอร์ด อัปโหลดรหัส ตอนนี้คุณสามารถเล่นกับหุ่นยนต์ DIY ของคุณ

ขั้นตอนที่ 5: แผนผัง

ขั้นตอนที่ 6: รหัส

#รวม

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 mpu; บูล dmpReady = เท็จ; // ตั้งค่าจริงหาก DMP init สำเร็จ uint8_t mpuIntStatus; // เก็บไบต์สถานะขัดจังหวะจริงจาก MPU uint8_t devStatus; // ส่งคืนสถานะหลังจากการทำงานของอุปกรณ์แต่ละเครื่อง (0 = สำเร็จ, !0 = ข้อผิดพลาด) uint16_t packetSize; // ขนาดแพ็กเก็ต DMP ที่คาดไว้ (ค่าเริ่มต้นคือ 42 ไบต์) uint16_t fifoCount; // นับไบต์ทั้งหมดที่อยู่ใน FIFO uint8_t fifoBuffer[64]; // บัฟเฟอร์การจัดเก็บ FIFO Quaternion q; // [w, x, y, z] คอนเทนเนอร์ควอเทอร์เนียน VectorFloat แรงโน้มถ่วง; // [x, y, z] เวกเตอร์แรงโน้มถ่วง float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container และเวกเตอร์แรงโน้มถ่วง double setpoint = 172.5; setpoint สองเท่า = originalSetpoint; การเคลื่อนไหวสองครั้งAngleOffset = 0.1; อินพุตคู่, เอาต์พุต; int moveState=0; double Kp = 23;//set P first double Kd = 0.8;//ค่านี้โดยทั่วไปจะเล็ก double Ki = 300;//ค่านี้ควรสูงเพื่อความเสถียรที่ดีขึ้น PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT); // pid initialize int motL1=26; ///4 pins for motor drive int motL2=2; int motR1=27; int motR2=4; บูลผันผวน mpuInterrupt = false; // ระบุว่าพินขัดจังหวะ MPU นั้นเป็นโมฆะสูงหรือไม่ dmpDataReady () { mpuInterrupt = true; } การตั้งค่าเป็นโมฆะ () { ledcSetup (0, 20000, 8); // การตั้งค่า pwm ledcSetup (1, 20000, 8); ledcSetup(2, 20000, 8); ledcSetup(3, 20000, 8); ledcAttachPin(motL1, 0); // โหมดพินของมอเตอร์ ledcAttachPin(motL2, 1); ledcAttachPin(motR1, 2); ledcAttachPin(motR2, 3); // เข้าร่วมบัส I2C (ไลบรารี I2Cdev ไม่ทำโดยอัตโนมัติ) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); Wire.setClock(400000); // นาฬิกา 400kHz I2C แสดงความคิดเห็นบรรทัดนี้หากมีปัญหาในการรวบรวม #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif Serial.println(F("กำลังเริ่มต้นอุปกรณ์ I2C…")); โหมดพิน (14, อินพุต); // เริ่มต้นการสื่อสารแบบอนุกรม // (เลือก 115200 เนื่องจากจำเป็นสำหรับเอาต์พุต Teapot Demo แต่ขึ้นอยู่กับคุณจริงๆ ขึ้นอยู่กับโครงการของคุณ) Serial.begin(9600); ในขณะที่ (!ซีเรียล); // รอการแจงนับของ Leonardo คนอื่นทำต่อทันที // เริ่มต้นอุปกรณ์ Serial.println(F("กำลังเริ่มต้นอุปกรณ์ I2C…")); mpu.initialize(); // ตรวจสอบการเชื่อมต่อ Serial.println(F("กำลังทดสอบการเชื่อมต่ออุปกรณ์…")); Serial.println(mpu.testConnection() ? F("การเชื่อมต่อ MPU6050 สำเร็จ"): F("การเชื่อมต่อ MPU6050 ล้มเหลว")); // โหลดและกำหนดค่า DMP Serial.println(F("Initializing DMP…")); devStatus = mpu.dmp เริ่มต้น (); // จัดหาไจโรออฟเซ็ตของคุณเองที่นี่ ปรับขนาดสำหรับความไวขั้นต่ำ mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 ค่าเริ่มต้นจากโรงงานสำหรับชิปทดสอบของฉัน // ตรวจสอบให้แน่ใจว่ามันใช้งานได้ (คืนค่า 0 ถ้าใช่) ถ้า (devStatus == 0) { // เปิด DMP ตอนนี้ก็พร้อมแล้ว Serial.println(F("Enable DMP… ")); mpu.setDMPEnabled(จริง); // เปิดใช้งานการตรวจจับการขัดจังหวะ Arduino Serial.println (F("การเปิดใช้งานการตรวจจับการขัดจังหวะ (Arduino อินเตอร์รัปต์ภายนอก 0)…")); AttachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // ตั้งค่าสถานะ DMP Ready ของเราเพื่อให้ฟังก์ชัน main loop() รู้ว่าสามารถใช้มันได้ Serial.println(F("DMP ready! Waiting for first interrupt…")); dmpReady = จริง; // รับขนาดแพ็คเก็ต DMP ที่คาดไว้สำหรับการเปรียบเทียบในภายหลัง packetSize = mpu.dmpGetFIFOPacketSize(); //ตั้งค่า PID pid. SetMode(อัตโนมัติ); pid. SetSampleTime(10); pid. SetOutputLimits(-255, 255); } อื่น ๆ { // ข้อผิดพลาด! // 1 = การโหลดหน่วยความจำเริ่มต้นล้มเหลว // 2 = การอัพเดตการกำหนดค่า DMP ล้มเหลว // (หากมันจะพัง โดยปกติรหัสจะเป็น 1) Serial.print(F("DMP Initialization failed (code ")); Serial.print พิมพ์ (devStatus); Serial.println (F(")")); } } void loop() { // หากการเขียนโปรแกรมล้มเหลว อย่าพยายามทำอะไรหาก (!dmpReady) กลับมา; // รอการขัดจังหวะ MPU หรือแพ็กเก็ตพิเศษที่พร้อมใช้งานในขณะที่ (!mpuInterrupt && fifoCount < packetSize) { pid. Compute();//ช่วงเวลานี้ใช้เพื่อโหลดข้อมูล ดังนั้นคุณสามารถใช้สิ่งนี้สำหรับการคำนวณอื่น ๆ motorSpeed(เอาท์พุท); } // รีเซ็ตแฟล็กขัดจังหวะและรับ INT_STATUS ไบต์ mpuInterrupt = false; mpuIntStatus = mpu.getIntStatus (); // รับจำนวน FIFO ปัจจุบัน fifoCount = mpu.getFIFOCount(); // ตรวจสอบโอเวอร์โฟลว์ (สิ่งนี้ไม่ควรเกิดขึ้นเว้นแต่โค้ดของเราไม่มีประสิทธิภาพเกินไป) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // รีเซ็ตเพื่อให้เราสามารถดำเนินการต่อได้อย่างสมบูรณ์ mpu.resetFIFO (); Serial.println(F("FIFO ล้น!")); // มิฉะนั้น ให้ตรวจสอบข้อมูล DMP พร้อมอินเตอร์รัปต์ (ซึ่งควรเกิดขึ้นบ่อยๆ) } อื่นหาก (mpuIntStatus & 0x02) { // รอให้ความยาวข้อมูลที่ถูกต้องถูกต้อง ควรรอสักครู่ (fifoCount 1 แพ็กเก็ตพร้อมใช้งาน // (นี่) ให้เราอ่านเพิ่มเติมได้ทันทีโดยไม่ต้องรอการขัดจังหวะ) fifoCount -= packetSize; mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); #if LOG_INPUT Serial print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); // มุมออยเลอร์ Serial.print("\t"); Serial.print(ypr[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif input = ypr[1] * 180/M_PI + 180; } } โมฆะ motorSpeed (int PWM) { float L1, L2, R1, R2; if(PWM>=0){//forward direction L2=0; L1=abs(PWM); R2=0; R1=abs(PWM); if(L1>=255){ L1=R1=255; } } อื่น {// ทิศทางถอยหลัง L1=0; L2=abs(PWM); R1=0; R2=abs(PWM); if(L2>=255){ L2=R2=255; } } //มอเตอร์ไดรฟ์ ledcWrite(0, L1); ledcWrite(1, L2); ledcWrite(2, R1*0.97);//0.97 คือความเร็วจริง หรือเนื่องจากมอเตอร์ด้านขวามีความเร็วสูงกว่ามอเตอร์ด้านซ้าย เราจึงลดความเร็วของมอเตอร์ลงจนกว่าความเร็วของมอเตอร์จะเท่ากัน ledcWrite(3, R2*0.97);

}