- การเลือกชิ้นส่วนสำหรับ Self Balancing Robot
- การพิมพ์ 3 มิติและการประกอบหุ่นยนต์ปรับสมดุลในตัวเอง
- แผนภูมิวงจรรวม
- รหัสหุ่นยนต์ปรับสมดุลด้วยตนเอง
- การทำงานของ Arduino Self Balancing Robot
หลังจากได้รับแรงบันดาลใจจากมอเตอร์ RYNO และสกู๊ตเตอร์ทรงตัวเองอื่น ๆ จาก Segway ฉันก็อยากจะสร้างหุ่นยนต์ Arduino Segwayของตัวเองมาโดยตลอด คิดในขณะที่ฉันตัดสินใจที่จะสร้างตัวเอง Balancing หุ่นยนต์โดยใช้ Arduino ด้วยวิธีนี้ฉันจะสามารถเข้าใจแนวคิดพื้นฐานที่อยู่เบื้องหลังสกูตเตอร์เหล่านี้และเรียนรู้วิธีการทำงานของอัลกอริทึม PID
เมื่อฉันเริ่มสร้างฉันตระหนักว่าบอทนี้เป็นความท้าทายเล็กน้อยในการสร้าง มีตัวเลือกมากมายให้เลือกและด้วยเหตุนี้ความสับสนจึงเริ่มจากรูปแบบที่ถูกต้องในการเลือกมอเตอร์และยังคงอยู่จนกว่าจะปรับค่า PID และมีหลายสิ่งที่ต้องพิจารณาเช่นประเภทของแบตเตอรี่ตำแหน่งของแบตเตอรี่กริปล้อประเภทของตัวขับมอเตอร์การรักษา CoG (จุดศูนย์ถ่วง) และอื่น ๆ อีกมากมาย
แต่ผมขอแบ่งให้คุณเมื่อคุณสร้างมันขึ้นมาคุณจะยอมรับว่ามันไม่ยากอย่างที่คิด มาดูกันในบทช่วยสอนนี้ฉันจะบันทึกประสบการณ์ของฉันในการสร้างหุ่นยนต์ปรับสมดุลตัวเอง คุณอาจเป็นมือใหม่ที่เพิ่งเริ่มต้นหรืออาจจะมาที่นี่หลังจากหงุดหงิดมานานที่ไม่ได้ให้บอททำงาน สถานที่แห่งนี้มุ่งหวังให้เป็นจุดหมายปลายทางสุดท้ายของคุณ มาเริ่มกันเลย……
การเลือกชิ้นส่วนสำหรับ Self Balancing Robot
ก่อนที่ฉันจะบอกตัวเลือกทั้งหมดในการสร้างบอทให้ฉันแสดงรายการที่ฉันใช้ในโครงการหุ่นยนต์ปรับสมดุลตัวเองนี้
- Arduino UNO
- มอเตอร์กระแสตรงแบบมีเกียร์ (สีเหลือง) - 2Nos
- L298N โมดูลไดรเวอร์มอเตอร์
- MPU6050
- คู่ของล้อ
- แบตเตอรี่ Li-ion 7.4V
- การเชื่อมต่อสายไฟ
- ร่างกายพิมพ์ 3D
คุณสามารถผสมและเลือกส่วนประกอบใด ๆ ข้างต้นได้ตามความพร้อมใช้งานในการสร้างชุดหุ่นยนต์ปรับสมดุลในตัวของคุณเองเพียงตรวจสอบให้แน่ใจว่าส่วนประกอบตรงตามเกณฑ์ต่อไปนี้
คอนโทรลเลอร์: คอนโทรลเลอร์ที่ฉันใช้อยู่ที่นี่คือ Arduino UNO ทำไมมันถึงใช้งานง่าย คุณยังสามารถใช้ Arduino Nano หรือ Arduino mini ได้ แต่ฉันอยากแนะนำให้คุณใช้ UNO เนื่องจากเราสามารถตั้งโปรแกรมได้โดยตรงโดยไม่ต้องใช้ฮาร์ดแวร์ภายนอก
มอเตอร์: ตัวเลือกที่ดีที่สุดของมอเตอร์ที่คุณสามารถใช้สำหรับหุ่นยนต์ปรับสมดุลตัวเองได้โดยไม่ต้องสงสัยเลยว่าจะเป็นมอเตอร์ Stepper แต่เพื่อให้ง่ายขึ้นฉันใช้มอเตอร์เกียร์ DC ใช่ไม่จำเป็นต้องมีสเต็ปเปอร์ บอททำงานได้ดีกับมอเตอร์เกียร์ DC สีเหลืองราคาถูกที่หาได้ทั่วไปเช่นกัน
ตัวขับมอเตอร์:หากคุณเลือกมอเตอร์เกียร์ DC เหมือนของฉันคุณสามารถใช้โมดูลไดรเวอร์ L298N เหมือนฉันหรือแม้แต่ L293D ก็น่าจะใช้งานได้ดี เรียนรู้เพิ่มเติมเกี่ยวกับการควบคุมมอเตอร์กระแสตรงโดยใช้ L293D และ Arduino
ล้อ:อย่าประมาณพวกนี้; ฉันมีช่วงเวลาที่ยากลำบากในการพบว่าปัญหาเกิดจากล้อของฉัน ดังนั้นตรวจสอบให้แน่ใจว่าล้อของคุณยึดเกาะกับพื้นได้ดี จับตาดูอย่างใกล้ชิดไม่ควรปล่อยให้ล้อไถลไปกับพื้น
Accelerometer และ Gyroscope:ตัวเลือกที่ดีที่สุดของ Accelerometer และ Gyroscope สำหรับบอทของคุณคือ MPU6050 ดังนั้นอย่าพยายามสร้างด้วย Accelerometer แบบปกติเช่น ADXL345 หรืออะไรทำนองนั้นมันจะไม่ทำงาน คุณจะรู้ว่าทำไมในตอนท้ายของบทความนี้ คุณยังสามารถตรวจสอบบทความเฉพาะของเราเกี่ยวกับการใช้ MPU6050 กับ Arduino
แบตเตอรี่: เราต้องการแบตเตอรี่ที่เบาที่สุดเท่าที่จะเป็นไปได้และแรงดันไฟฟ้าที่ใช้งานควรมากกว่า 5V เพื่อให้เราสามารถจ่ายไฟให้ Arduino ของเราได้โดยตรงโดยไม่ต้องใช้โมดูลเพิ่ม ดังนั้นตัวเลือกที่ดีที่สุดคือแบตเตอรี่ Li-polymer 7.4V ที่นี่เนื่องจากฉันมีแบตเตอรี่ Li-ion 7.4V พร้อมใช้งานฉันจึงใช้มัน แต่จำไว้ว่า Li-po มีประโยชน์กว่า Li-ion
แชสซี: อีกที่หนึ่งที่คุณไม่ควรประนีประนอมคือแชสซีบอทของคุณ คุณสามารถใช้กระดาษแข็งไม้พลาสติกอะไรก็ได้ที่คุณถนัด แต่ตรวจสอบให้แน่ใจว่าแชสซีแข็งแรงและไม่ควรกระดิกเมื่อบอทพยายามทรงตัว ฉันได้ออกแบบโดยแชสซีของตัวเองบน Solidworks โดยอนุมานจากบอทอื่น ๆ และพิมพ์ 3 มิติ หากคุณมีเครื่องพิมพ์คุณสามารถพิมพ์งานออกแบบได้ไฟล์การออกแบบจะถูกแนบในหัวข้อต่อไป
การพิมพ์ 3 มิติและการประกอบหุ่นยนต์ปรับสมดุลในตัวเอง
หากคุณตัดสินใจที่จะพิมพ์ 3 มิติบนแชสซีเดียวกันกับที่ฉันใช้สร้างบอทของฉันไฟล์ STL สามารถดาวน์โหลดได้จาก thingiverse ฉันได้เพิ่มไฟล์การออกแบบเข้าไปด้วยเพื่อให้คุณสามารถแก้ไขได้ตามความต้องการของบุคลากรของคุณ
ชิ้นส่วนไม่มีโครงสร้างที่ยื่นออกมาดังนั้นคุณสามารถพิมพ์ได้อย่างง่ายดายโดยไม่ต้องมีการรองรับใด ๆ และการเติม 25% จะทำงานได้ดี การออกแบบนั้นค่อนข้างธรรมดาและเครื่องพิมพ์พื้นฐานใด ๆ ก็ควรจะจัดการได้อย่างง่ายดาย ฉันใช้ซอฟต์แวร์ Cura เพื่อแบ่งแบบจำลองและพิมพ์โดยใช้ Tevo Tarantula ของฉันการตั้งค่าดังแสดงด้านล่าง
คุณจะต้องพิมพ์ส่วนของร่างกายและชิ้นส่วนยึดมอเตอร์สี่ชิ้น การประกอบค่อนข้างตรงไปตรงมา ใช้น็อตและสลักเกลียว 3 มม. เพื่อยึดมอเตอร์และบอร์ดให้เข้าที่ หลังจากประกอบแล้วควรมีลักษณะดังนี้ดังแสดงในภาพด้านล่าง
การออกแบบที่แท้จริงได้รับการวางแผนโดยใช้โมดูลไดรฟ์ L298N ในชั้นวางด้านล่าง Arduino และแบตเตอรี่ที่อยู่ด้านบนดังที่แสดงไว้ด้านบน หากคุณทำตามคำสั่งเดียวกันคุณสามารถขันสกรูบอร์ดให้ตรงกับรูที่ให้มาและใช้ป้ายลวดสำหรับแบตเตอรี่ Li-po การจัดเรียงนี้ควรใช้งานได้เช่นกันยกเว้นล้อซุปเปอร์ธรรมดาที่ฉันต้องเปลี่ยนในภายหลัง
ในบอทของฉันฉันได้สลับตำแหน่งของแบตเตอรี่และบอร์ด Arduino UNO เพื่อความสะดวกในการเขียนโปรแกรมและยังต้องแนะนำบอร์ด perf สำหรับการเชื่อมต่อให้เสร็จสมบูรณ์ บอทของฉันจึงดูไม่เป็นไปตามที่ฉันวางแผนไว้ในระยะแรก หลังจากเสร็จสิ้นการทดสอบการเขียนโปรแกรมการเดินสายไฟและทุกอย่างในที่สุดหุ่นยนต์สองล้อของฉันก็มีลักษณะเช่นนี้
แผนภูมิวงจรรวม
การเชื่อมต่อสำหรับหุ่นยนต์ปรับสมดุลด้วยตนเองที่ใช้ Arduinoนี้ค่อนข้างง่าย นี่คือหุ่นยนต์ปรับสมดุลตัวเองโดยใช้ Arduino และ MPU6050ดังนั้นเราจึงต้องเชื่อมต่อ MPU6050 กับ Arduino และเชื่อมต่อมอเตอร์ผ่านโมดูลไดรเวอร์มอเตอร์ การตั้งค่าทั้งหมดใช้พลังงานจากแบตเตอรี่ลิเธียมไอออน 7.4V แผนภาพวงจรสำหรับสิ่งเดียวกันแสดงอยู่ด้านล่าง
โมดูลไดรเวอร์ Arduino และ L298N Motor ใช้พลังงานโดยตรงผ่านพิน Vin และขั้ว 12V ตามลำดับ ตัวควบคุมออนบอร์ดบนบอร์ด Arduino จะแปลงอินพุต 7.4V เป็น 5V และ ATmega IC และ MPU6050 จะขับเคลื่อนด้วย มอเตอร์กระแสตรงสามารถทำงานจากแรงดัน 5V ถึง 12V แต่เราจะเชื่อมต่อสายบวก 7.4V จากแบตเตอรี่ไปยังขั้วอินพุต 12V ของโมดูลไดรเวอร์มอเตอร์ สิ่งนี้จะทำให้มอเตอร์ทำงานด้วย 7.4V ตารางต่อไปนี้จะแสดงว่าโมดูลไดรเวอร์มอเตอร์ MPU6050 และ L298N เชื่อมต่อกับ Arduino อย่างไร
|
พินส่วนประกอบ |
Arduino Pin |
|
MPU6050 |
|
|
Vcc |
+ 5V |
|
พื้น |
Gnd |
|
SCL |
A5 |
|
SDA |
A4 |
|
INT |
D2 |
|
L298N |
|
|
IN1 |
D6 |
|
IN2 |
D9 |
|
IN3 |
D10 |
|
IN4 |
D11 |
MPU6050 สื่อสารกับ Arduino ผ่านอินเทอร์เฟซ I2C ดังนั้นเราจึงใช้หมุด SPI A4 และ A5 ของ Arduino มอเตอร์กระแสตรงเชื่อมต่อกับหมุด PWM D6, D9 D10 และ D11 ตามลำดับ เราจำเป็นต้องเชื่อมต่อกับพิน PWM เพราะเราจะควบคุมความเร็วของมอเตอร์กระแสตรงโดยการเปลี่ยนรอบการทำงานของสัญญาณ PWM หากคุณไม่คุ้นเคยกับส่วนประกอบทั้งสองนี้ขอแนะนำให้อ่านผ่านบทแนะนำเกี่ยวกับ MPU6050 Interfacing และ L298N Motor
รหัสหุ่นยนต์ปรับสมดุลด้วยตนเอง
ตอนนี้เราต้องตั้งโปรแกรมบอร์ด Arduino UNO ของเราเพื่อปรับสมดุลหุ่นยนต์ นี่คือจุดที่ความมหัศจรรย์ทั้งหมดเกิดขึ้น แนวคิดเบื้องหลังนั้นเรียบง่าย เราต้องตรวจสอบว่าบอทเอนไปทางด้านหน้าหรือไปทางด้านหลังโดยใช้ MPU6050จากนั้นถ้ามันเอนไปทางด้านหน้าเราจะต้องหมุนล้อไปในทิศทางข้างหน้าและถ้ามันเอนไปทางด้านหลังเราต้องหมุนล้อ ในทิศทางกลับกัน
ในขณะเดียวกันเราก็ต้องควบคุมความเร็วที่ล้อกำลังหมุนหากบอทสับสนเล็กน้อยจากตำแหน่งกึ่งกลางล้อจะหมุนช้าลงและความเร็วจะเพิ่มขึ้นเมื่ออยู่ห่างจากตำแหน่งศูนย์กลางมากขึ้น เพื่อให้บรรลุตรรกะนี้เราใช้อัลกอริทึม PID ซึ่งมีตำแหน่งกึ่งกลางเป็นจุดที่กำหนดและระดับความสับสนเป็นผลลัพธ์
หากต้องการทราบตำแหน่งปัจจุบันของบอทเราใช้MPU6050ซึ่งเป็นมาตรความเร่ง 6 แกนและเซ็นเซอร์ไจโรสโคปรวมกัน เพื่อให้ได้ค่าตำแหน่งที่เชื่อถือได้จากเซ็นเซอร์เราจำเป็นต้องใช้ค่าของทั้งมาตรความเร่งและไจโรสโคปเนื่องจากค่าจากมาตรความเร่งมีปัญหาเรื่องเสียงรบกวนและค่าจากไจโรสโคปมีแนวโน้มที่จะล่องลอยไปตามกาลเวลา ดังนั้นเราต้องรวมทั้งสองอย่างเข้าด้วยกันและได้ค่าของการหันเหและการหมุนของหุ่นยนต์ของเราซึ่งเราจะใช้เฉพาะค่าของการหันเห
ฟังดูหัวสั่นใช่มั้ย? แต่ไม่ต้องกังวลขอบคุณชุมชน Arduino ที่เรามีไลบรารีที่พร้อมใช้งานซึ่งสามารถทำการคำนวณ PID และรับค่าของการหันเหจาก MPU6050 ห้องสมุดได้รับการพัฒนาโดย br3ttb และ jrowberg ตามลำดับ ก่อนดำเนินการดาวน์โหลดไลบรารีของพวกเขาจากลิงค์ต่อไปนี้และเพิ่มลงในไดเร็กทอรี Arduino lib ของคุณ
github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
ตอนนี้เราได้เพิ่มไลบรารีลงใน Arduino IDE แล้ว มาเริ่มการเขียนโปรแกรมสำหรับหุ่นยนต์ปรับสมดุลตนเอง เช่นเดียวกับรหัสที่สมบูรณ์สำหรับหุ่นยนต์ปรับสมดุล MPU6050จะได้รับที่ส่วนท้ายของหน้านี้ฉันแค่อธิบายส่วนย่อยที่สำคัญที่สุดในโค้ด บอกไว้ก่อนหน้านี้ว่าโค้ดถูกสร้างขึ้นด้านบนของโค้ดตัวอย่าง MPU6050 เรากำลังจะเพิ่มประสิทธิภาพโค้ดให้เหมาะกับจุดประสงค์ของเราและเพิ่ม PID และเทคนิคการควบคุมสำหรับหุ่นยนต์ปรับสมดุลตัวเองของเรา
อันดับแรกเรารวมไลบรารีที่จำเป็นสำหรับโปรแกรมนี้ในการทำงาน ประกอบด้วยไลบรารี I2C ในตัว, PID Library และ MPU6050 Library ที่เราเพิ่งดาวน์โหลด
# รวม "I2Cdev.h" # รวม
จากนั้นเราประกาศตัวแปรที่จะต้องได้รับข้อมูลจากเซ็นเซอร์ MPU6050 เราอ่านทั้งเวกเตอร์แรงโน้มถ่วงและค่าควอเทอร์เนียนจากนั้นคำนวณระยะห่างของการหันเหและค่าการหมุนของบอท YPR ลอยอาร์เรย์ จะถือผลสุดท้าย
// การควบคุม MPU / สถานะ vars bool dmpReady = false; // ตั้งค่าเป็นจริงหากเริ่ม DMP สำเร็จ uint8_t mpuIntStatus; // เก็บไบต์สถานะการขัดจังหวะจริงจาก MPU uint8_t devStatus; // ส่งคืนสถานะหลังจากการทำงานของแต่ละอุปกรณ์ (0 = สำเร็จ,! 0 = ข้อผิดพลาด) uint16_t packetSize; // ขนาดแพ็กเก็ต DMP ที่คาดไว้ (ค่าเริ่มต้นคือ 42 ไบต์) uint16_t fifoCount; // จำนวนไบต์ทั้งหมดที่อยู่ใน FIFO uint8_t fifoBuffer; // บัฟเฟอร์การจัดเก็บ FIFO // การวางแนว / การเคลื่อนไหว vars Quaternion q; // แรงโน้มถ่วงของ ภาชนะ quaternion VectorFloat; // เวกเตอร์แรงโน้มถ่วงลอย ypr; // ภาชนะที่หันเห / pitch / ม้วนและเวกเตอร์แรงโน้มถ่วง
ถัดไปเป็นส่วนที่สำคัญมากของโค้ดและนี่คือที่ที่คุณจะใช้เวลานานในการปรับแต่งชุดค่าที่เหมาะสม หากหุ่นยนต์ของคุณถูกสร้างขึ้นโดยมีจุดศูนย์ถ่วงที่ดีมากและส่วนประกอบต่างๆถูกจัดเรียงอย่างสมมาตร (ซึ่งในกรณีส่วนใหญ่ไม่ใช่) ค่าของ set-point ของคุณจะเท่ากับ 180 มิฉะนั้นให้เชื่อมต่อบอทของคุณกับจอภาพอนุกรม Arduino และเอียงไปจนถึง คุณพบตำแหน่งสมดุลที่ดีอ่านค่าที่แสดงบนจอภาพอนุกรมและนี่คือค่าจุดที่ตั้งไว้ของคุณ ค่าของ Kp, Kd และ Ki จะต้องได้รับการปรับแต่งตามบอทของคุณ ไม่มีบอทที่เหมือนกันสองตัวที่จะมีค่า Kp, Kd และ Ki เท่ากันดังนั้นจึงไม่มีการหลบหนีจากมันดูวิดีโอในตอนท้ายของหน้านี้เพื่อรับทราบวิธีปรับค่าเหล่านี้
/ ********* ปรับค่า 4 ค่านี้สำหรับ BOT ของคุณ ********* / double setpoint = 176; // ตั้งค่าเมื่อบอทตั้งฉากกับกราวด์โดยใช้มอนิเตอร์แบบอนุกรม // อ่านเอกสารโครงการบน circuitdigest.com เพื่อเรียนรู้วิธีตั้งค่าเหล่านี้ สองเท่า Kp = 21; // ตั้งค่า Kd คู่แรกนี้= 0.8; // ตั้งค่าsecound double Ki = 140; // สุดท้ายตั้งค่านี้ / ****** สิ้นสุดการตั้งค่า ********* /
ในบรรทัดถัดไปเราinitialise ขั้นตอนวิธี PID โดยผ่านตัวแปรนำเข้าส่งออกชี้ชุด Kp, Ki และ Kd จากสิ่งเหล่านี้เราได้กำหนดค่าของ set-point Kp, Ki และ Kd ไว้แล้วในส่วนย่อยของโค้ดด้านบน ค่าของอินพุตจะเป็นค่าปัจจุบันของการหันเหที่อ่านจากเซ็นเซอร์ MPU6050 และค่าของเอาต์พุตจะเป็นค่าที่คำนวณโดยอัลกอริทึม PID ดังนั้นโดยพื้นฐานแล้วอัลกอริทึม PID จะให้ค่าเอาต์พุตซึ่งควรใช้เพื่อแก้ไขค่าอินพุตให้ใกล้เคียงกับจุดที่กำหนด
PID pid (& input, & output, & setpoint, Kp, Ki, Kd, DIRECT);
ภายในฟังก์ชัน การตั้งค่าโมฆะ เราเริ่มต้น MPU6050 โดยการกำหนดค่า DMP (Digital Motion Processor) สิ่งนี้จะช่วยเราในการรวมข้อมูล Accelerometer กับข้อมูล Gyroscope และให้ค่า Yaw, Pitch และ Roll ที่เชื่อถือได้ เราจะไม่ลงลึกในเรื่องนี้มากนักเนื่องจากมันจะไกลเกินกว่าหัวข้อนี้ ส่วนหนึ่งของโค้ดที่คุณต้องค้นหาในฟังก์ชันการตั้งค่าคือค่าออฟเซ็ตไจโร เซ็นเซอร์ MPU6050 แต่ละตัวมีค่าออฟเซ็ตของตัวเองคุณสามารถใช้ร่าง Arduino นี้เพื่อคำนวณค่าออฟเซ็ตของเซ็นเซอร์ของคุณและอัปเดตบรรทัดต่อไปนี้ในโปรแกรม
// ระบุการชดเชยไจโรของคุณเองที่นี่ปรับขนาดสำหรับความไวขั้นต่ำ mpu.setXGyroOffset (220); mpu.setYGyroOffset (76); mpu.setZGyroOffset (-85); mpu.setZAccelOffset (1688);
เราต้องเริ่มต้นพิน Digital PWMที่เราใช้เชื่อมต่อกับมอเตอร์ของเราด้วย ในกรณีของเราคือ D6, D9, D10 และ D11 ดังนั้นเราจึงเริ่มต้นพินเหล่านี้เป็นพินเอาต์พุตทำให้ต่ำตามค่าเริ่มต้น
// initialise มอเตอร์outpu หมุด pinMode (6, OUTPUT); PinMode (9, เอาท์พุท); PinMode (10, เอาท์พุท); pinMode (11, เอาท์พุท); // โดยค่าเริ่มต้นให้ปิดทั้งมอเตอร์ analogWrite (6, LOW); analogWrite (9, ต่ำ); analogWrite (10, ต่ำ); analogWrite (11, ต่ำ);
ภายในฟังก์ชัน ลูป หลักเราตรวจสอบว่าข้อมูลจาก MPU6050 พร้อมที่จะอ่าน หรือไม่ ถ้าใช่เราจะใช้มันเพื่อคำนวณค่า PID จากนั้นแสดงค่าอินพุตและเอาต์พุตของ PID บนมอนิเตอร์แบบอนุกรมเพื่อตรวจสอบว่า PID ตอบสนองอย่างไร จากนั้นขึ้นอยู่กับมูลค่าของผลลัพธ์ที่เราตัดสินใจว่าบอทต้องเดินหน้าหรือถอยหลังหรือหยุดนิ่ง
เนื่องจากเราคิดว่า MPU6050 จะคืนค่า 180 เมื่อบอทตั้งตรง เราจะได้รับการแก้ไขค่าบวกเมื่อบอทจะลดลงไปทางด้านหน้าและเราจะได้รับค่าในเชิงลบหาก ธ ปทจะลดลงไปทางด้านหลัง ดังนั้นเราจึงตรวจสอบเงื่อนไขนี้และเรียกใช้ฟังก์ชันที่เหมาะสมเพื่อย้ายบอทไปข้างหน้าหรือหลังวอร์ด
ในขณะที่ (! mpuInterrupt && fifoCount <packetSize) { // ไม่มีข้อมูล mpu - ทำการคำนวณ PID และส่งออกไปยังมอเตอร์pid.Compute (); // พิมพ์ค่าของอินพุตและเอาต์พุตบนมอนิเตอร์แบบอนุกรมเพื่อตรวจสอบการทำงานSerial.print (อินพุต); Serial.print ("=>"); Serial.println (เอาต์พุต); if (อินพุต> 150 && อินพุต <200) {// หากบอทล้มหาก (เอาต์พุต> 0) // ตกลงไปด้านหน้าไปข้างหน้า (); // หมุนล้อไปข้างหน้าอย่างอื่นถ้า (เอาท์พุท <0) // ล้มไปทางย้อนกลับ (); // หมุนล้อไปข้างหลัง} else // ถ้า Bot ไม่ล้มStop (); // จับล้อให้นิ่ง}
ตัวแปรเอาท์พุท PID ยังตัดสินใจวิธีการที่รวดเร็วมอเตอร์จะต้องมีการหมุนหากบอทกำลังจะล้มเราทำการแก้ไขเล็กน้อยโดยหมุนวงล้อช้าๆ หากการแก้ไขเล็กน้อยเหล่านี้ทำงานได้และยังคงอยู่หากบอทล้มลงเราจะเพิ่มความเร็วของมอเตอร์ อัลกอริทึม PI จะพิจารณาค่าของการหมุนของล้อที่เร็วเพียงใด โปรดทราบว่าสำหรับฟังก์ชัน Reverse เราได้คูณค่าของเอาต์พุตด้วย -1 เพื่อให้เราสามารถแปลงค่าลบเป็นค่าบวกได้
เป็นโมฆะ Forward () // รหัสเพื่อหมุนล้อไปข้างหน้า { analogWrite (6, output); analogWrite (9,0); analogWrite (10, เอาต์พุต); analogWrite (11,0); Serial.print ("F"); // ข้อมูลการแก้จุดบกพร่อง } void Reverse () // Code เพื่อหมุนวงล้อย้อนกลับ { analogWrite (6,0); analogWrite (9, เอาต์พุต * -1); analogWrite (10,0); analogWrite (11, เอาต์พุต * -1); Serial.print ("R"); } void Stop () // รหัสหยุดทั้งสองล้อ { analogWrite (6,0); analogWrite (9,0); analogWrite (10,0); analogWrite (11,0); Serial.print ("S"); }
การทำงานของ Arduino Self Balancing Robot
เมื่อคุณพร้อมกับฮาร์ดแวร์แล้วคุณสามารถอัปโหลดรหัสไปยังบอร์ด Arduino ของคุณได้ ตรวจสอบให้แน่ใจว่าการเชื่อมต่อถูกต้องเนื่องจากเราใช้แบตเตอรี่ Li-ion ด้วยความระมัดระวังเป็นอย่างยิ่ง ดังนั้นตรวจสอบการลัดวงจรอีกครั้งและตรวจสอบให้แน่ใจว่าขั้วจะไม่สัมผัสกันแม้ว่าบอทของคุณจะได้รับผลกระทบเล็กน้อยก็ตาม เปิดเครื่องโมดูลของคุณและเปิดจอภาพอนุกรมของคุณหาก Arduino ของคุณสามารถสื่อสารกับ MPU6050 ได้สำเร็จและหากทุกอย่างทำงานได้ตามที่คาดไว้คุณจะเห็นหน้าจอต่อไปนี้
ที่นี่เราเห็นอินพุตและเอาต์พุตค่าของขั้นตอนวิธี PID ในการป้อนข้อมูลรูปแบบ => เอาท์พุท หาก ธ ปทที่มีความสมดุลอย่างสมบูรณ์แบบมูลค่าของการส่งออกจะเป็น 0 ค่าอินพุตคือค่าปัจจุบันจากเซ็นเซอร์ MPU6050 ตัวอักษร“ F” แสดงว่าบอทกำลังเคลื่อนที่ไปข้างหน้าและ“ R” แสดงว่าบอทกลับด้าน
ในช่วงเริ่มต้นของ PID ฉันขอแนะนำให้ปล่อยสาย Arduino ของคุณเชื่อมต่อกับบอทเพื่อให้คุณสามารถตรวจสอบค่าของอินพุตและเอาต์พุตได้อย่างง่ายดายและจะง่ายต่อการแก้ไขและอัปโหลดโปรแกรมของคุณสำหรับค่า Kp, Ki และ Kd วิดีโอด้านล่างแสดงให้เห็นถึงการทำงานที่สมบูรณ์ของบอทและยังแสดงให้เห็นถึงวิธีการแก้ไขค่า PID ของคุณ
หวังว่านี่จะช่วยในการสร้างหุ่นยนต์ปรับสมดุลในตัวของคุณเองหากคุณมีปัญหาในการทำให้มันใช้งานได้จากนั้นฝากคำถามไว้ในส่วนความคิดเห็นด้านล่างหรือใช้ฟอรัมสำหรับคำถามทางเทคนิคเพิ่มเติม หากคุณต้องการความสนุกสนานมากขึ้นคุณสามารถใช้ตรรกะเดียวกันในการสร้างหุ่นยนต์ทรงตัวบอลได้