หลังจากที่ได้แรงบันดาลใจจากมอเตอร์ RYNO และสกูตเตอร์ที่ปรับสมดุลด้วยตนเองอื่น ๆ จาก Segway ฉันต้องการสร้างสิ่งที่คล้ายกันเสมอ คิดในขณะที่ฉันตัดสินใจที่จะสร้างตัวเอง Balancing หุ่นยนต์โดยใช้ Arduino ด้วยวิธีนี้ฉันจะสามารถเข้าใจแนวคิดเบื้องหลังสกูตเตอร์เหล่านี้ทั้งหมดและเรียนรู้วิธีการทำงานของอัลกอริธึม PID
เมื่อฉันเริ่มสร้างฉันตระหนักว่า bot นี้เป็นส่วนหนึ่งของความท้าทายในการสร้าง มีหลายทางเลือกเพื่อเลือกจากและด้วยเหตุนี้ confusions เริ่มต้นรูปแบบที่เหมาะสมเลือกมอเตอร์และยังคงจนถึงการปรับค่า PID และมีหลายสิ่งที่ต้องพิจารณาเช่นประเภทของแบตเตอรี่ตำแหน่งของแบตเตอรี่ล้อจับประเภทของคนขับมอเตอร์รักษา CoG (ศูนย์แรงโน้มถ่วง) และอื่น ๆ อีกมากมาย
แต่ให้ฉันทำลายมันให้คุณเมื่อคุณสร้างมันคุณจะเห็นว่ามันไม่ได้ยากอย่างที่มันเป็นไปได้ ลองมาดูหน้านี้ในบทแนะนำนี้จะอธิบายถึงประสบการณ์ในการสร้างหุ่นยนต์สมดุลด้วยตนเอง คุณอาจเป็นผู้เริ่มต้นใช้งานที่เพิ่งเริ่มต้นหรืออาจลุกขึ้นมาที่นี่หลังจากไม่ได้รับความยุ่งยากในการไม่ให้บอททำงาน สถานที่แห่งนี้มีเป้าหมายเพื่อเป็นปลายทางสุดท้ายของคุณ ดังนั้นขอเริ่มต้น ......

การเลือกอะไหล่สำหรับ Bot ของคุณ

ก่อนที่ฉันจะบอกคุณทุกทางเลือกสำหรับการสร้างบอตให้ฉันแสดงรายการสิ่งที่ฉันได้ใช้ในโครงการนี้
  • Arduino UNO
  • เกียร์มอเตอร์ DC (สีเหลือง) - 2 ไม่มี
  • โมดูลมอเตอร์ L298N
  • MPU6050
  • คู่ล้อ
  • แบตเตอรี่ Li-ion 7.4V
  • สายเชื่อมต่อ
  • ร่างกายที่พิมพ์ 3D

Controller:  ตัวควบคุมที่ฉันได้ใช้ที่นี่คือ Arduino UNO เพราะเหตุใดเนื่องจากใช้งานได้ง่าย คุณยังสามารถใช้ Arduino Nano หรือ Arduino mini แต่ขอแนะนำให้คุณติด UNO เนื่องจากเราสามารถตั้งโปรแกรมได้โดยตรงโดยไม่ต้องใช้ฮาร์ดแวร์ภายนอกใด ๆ
มอเตอร์:  ทางเลือกที่ดีที่สุดของมอเตอร์ที่คุณสามารถใช้สำหรับหุ่นยนต์สมดุลตนเองโดยไม่ต้องสงสัยจะเป็นมอเตอร์ Stepper แต่เพื่อให้ง่ายขึ้นฉันใช้มอเตอร์เกียร์แบบ DC ใช่มันไม่จำเป็นต้องมี stepper; บอททำงานได้ดีกับมอเตอร์เกียร์ดีเซลที่มีสีเหลืองราคาถูกทั่วไปเช่นกัน
มอเตอร์ไดร์เวอร์:ถ้าคุณได้เลือกมอเตอร์เกียร์ DC เช่นเหมืองแล้วคุณสามารถใช้โมดูลไดรเวอร์ L298N เช่นฉันหรือแม้กระทั่ง L293D ควรทำงานได้ดี เรียนรู้เพิ่มเติมเกี่ยวกับการควบคุมมอเตอร์ DC ใช้ L293D และ Arduino
ล้อ:อย่าคาดเดาเหล่านี้ ฉันมีเวลาที่ยากลำบากในการหาว่าปัญหาเกิดขึ้นกับล้อของฉัน เพื่อให้แน่ใจว่าล้อของคุณมีด้ามจับที่ดีกว่าพื้นที่คุณใช้อยู่ ดูอย่างใกล้ชิดจับของคุณไม่ควรให้ล้อของคุณที่จะล้มลงบนพื้น 
Accelerometer และ Gyroscope:ตัวเลือก Accelerometer และ Gyroscope ที่ดีที่สุดสำหรับบอทของคุณจะเป็น MPU6050 ดังนั้นอย่าพยายามที่จะสร้าง Accelerometer แบบปกติเช่น ADXL345 หรือสิ่งอื่นที่คล้ายคลึงกันเพียงเท่านี้ก็จะไม่ทำงาน คุณจะรู้ว่าทำไมในตอนท้ายของบทความนี้ นอกจากนี้คุณยังสามารถตรวจสอบบทความที่ทุ่มเทของเราเกี่ยวกับการใช้MPU6050 กับ Arduino
แบตเตอรี่:  เราจำเป็นต้องใช้แบตเตอรี่ที่มีน้ำหนักเบาที่สุดเท่าที่จะเป็นไปได้และแรงดันไฟฟ้าในการทำงานควรมากกว่า 5V เพื่อให้เราสามารถใช้พลังงานของ Arduino ได้โดยตรงโดยไม่ต้องใช้โมดูลเพิ่ม ดังนั้นทางเลือกที่เหมาะจะเป็นแบตเตอรี่ลิเธียมโพลิเมอร์ 7.4V ที่นี่เนื่องจากฉันมีแบตเตอรี่ Li-ion 7.4V พร้อมใช้งานฉันได้ใช้มัน แต่โปรดจำไว้ว่า Li-po เป็นข้อดีกว่า Li-ion
แชสซี:  สถานที่อื่นที่คุณไม่ควรประนีประนอมอยู่กับบอทบอทของคุณ คุณสามารถใช้กระดาษแข็งไม้สิ่งของพลาสติกที่คุณต้องการได้ แต่ให้แน่ใจว่าแชสซีแข็งแรงและไม่ควรกระดิกเมื่อบอทกำลังพยายามปรับสมดุล ฉันได้รับการออกแบบโดยแชสซีของตนเองใน Solidworks ที่อนุมานจากบอทอื่น ๆ และพิมพ์ 3D หากคุณมีเครื่องพิมพ์แล้วคุณยังสามารถพิมพ์การออกแบบไฟล์การออกแบบจะแนบมาในหัวข้อที่กำลังจะมาถึง

การพิมพ์ 3D และการประกอบ Bot

ถ้าคุณได้ตัดสินใจที่จะพิมพ์ 3 มิติตัวถังเดียวกันกับที่ผมใช้ในการสร้างบอทของฉันแล้วไฟล์ STL สามารถดาวน์โหลดได้จาก thingiverse ฉันยังได้เพิ่มไฟล์การออกแบบพร้อมด้วยเพื่อให้คุณสามารถปรับเปลี่ยนได้ตามความต้องการของบุคลากรของคุณ
ชิ้นส่วนไม่มีโครงสร้างที่ยื่นออกเพื่อให้คุณสามารถพิมพ์ได้โดยไม่ต้องรองรับใด ๆ และการใส่ข้อมูล 25% จะทำงานได้ดี การออกแบบเป็นธรรมดาสวยและเครื่องพิมพ์พื้นฐานใดควรจะสามารถจัดการกับได้อย่างง่ายดาย ฉันใช้ซอฟต์แวร์ Cura เพื่อตัดชิ้นงานและพิมพ์โดยใช้ Tevo Tarantula ของฉันการตั้งค่าดังแสดงด้านล่าง
การตั้งค่าด้วยตนเองใน Cura สำหรับหุ่นยนต์ปรับสมดุลด้วยตัวเอง

คุณจะต้องพิมพ์ส่วนของร่างกายเช่นเดียวกับสี่ส่วนติดตั้งมอเตอร์ การชุมนุมจะสวยตรงไปข้างหน้า; ใช้เกลียวและสลักเกลียว 3 มม. เพื่อยึดมอเตอร์และแผงให้เข้าที่ หลังจากประกอบเสร็จแล้วควรมีลักษณะดังนี้ดังรูปข้างล่างนี้
หุ่นยนต์ดัดแปลงด้วยตัวเอง DIY ที่ประกอบด้วย Arduino

การออกแบบที่แท้จริงได้รับการวางแผนด้วยโมดูลไดรฟ์ L298Nในชั้นวางด้านล่างของ Arduino และแบตเตอรี่ด้านบนดังรูปข้างต้น หากคุณทำตามคำสั่งเดียวกันคุณสามารถกรูบอร์ดได้โดยตรงโดยใช้รูที่จัดมาให้และใช้แท็กลวดสำหรับแบตเตอรี่ Li-po การจัดเรียงนี้ควรใช้ยกเว้นล้อธรรมดาที่ต้องเปลี่ยนในภายหลัง
ในบอทของฉันฉันได้เปลี่ยนตำแหน่งของแบตเตอรี่และ Arduino UNO คณะกรรมการเพื่อความสะดวกในการเขียนโปรแกรมและยังมีการแนะนำบอร์ด perf สำหรับการเสร็จสิ้นการเชื่อมต่อ ดังนั้นบอทของฉันจึงไม่ได้ดูตามที่ฉันวางแผนไว้ในระยะแรก หลังจากเสร็จสิ้นการทดสอบการเขียนโปรแกรมสายไฟและทุกอย่าง bot ของฉันก็มีลักษณะเช่นนี้
วงจรฮาร์ดแวร์สำหรับหุ่นยนต์ปรับสมดุลอัตโนมัติ DIY โดยใช้ Arduino

แผนภูมิวงจรรวม

การสร้างการเชื่อมต่อสำหรับหุ่นยนต์สมดุลตนเอง Arduinoนี้ทำได้ง่ายมาก เราต้องเชื่อมต่อ MPU6050 กับ Arduino และเชื่อมต่อมอเตอร์ด้วยโมดูล Motor Motor การตั้งค่าทั้งหมดใช้พลังงานจากแบตเตอรี่ Li-ion 7.4V แผนภาพวงจรสำหรับเดียวกันแสดงด้านล่าง
 หุ่นยนต์ปรับสมดุลของวงจรไฟฟ้าอัตโนมัติสำหรับ DIY โดยใช้ Arduino

โมดูล Arduino และชุดขับ L298N Motor ขับเคลื่อนโดยตรงผ่านขา Vin และขั้วต่อ 12V ตามลำดับ ตัวควบคุมบน board บนบอร์ด Arduino จะแปลงค่า input 7.4V to 5V และ ATmega IC และ MPU6050 จะถูกขับเคลื่อนด้วย มอเตอร์กระแสตรงสามารถทำงานจากแรงดันไฟฟ้า 5V ถึง 12V แต่เราจะเชื่อมต่อสายไฟบวก 7.4V จากแบตเตอรี่เข้ากับขั้วสัญญาณ 12V ของโมดูลควบคุมมอเตอร์ นี้จะทำให้มอเตอร์ทำงานกับ 7.4V ตารางต่อไปนี้จะแสดงวิธีการเชื่อมต่อโมดูลไดรเวอร์มอเตอร์ MPU6050 และ L298N กับ Arduino
Pin ส่วนประกอบ
Pin Arduino
MPU6050
vcc
+ 5V
พื้น
GND
SCL
A5
SDA
A4
INT
D2
L298N
IN1
D6
IN2
D9
IN3
D10
IN4
D11
 
MPU6050 สื่อสารกับ Arduino ผ่านอินเทอร์เฟซ I2C ดังนั้นเราจึงใช้ SPI pins A4 และ A5 ของ Arduino มอเตอร์ DC เชื่อมต่อกับหมุด PWM D6, D9 D10 และ D11 ตามลำดับ เราจำเป็นต้องเชื่อมต่อกับหมุด PWM เนื่องจากเราจะควบคุมความเร็วของมอเตอร์กระแสตรงโดยการเปลี่ยนรอบหน้าที่ของสัญญาณ PWM หากคุณไม่คุ้นเคยกับส่วนประกอบทั้งสองนี้ขอแนะนำให้อ่านคู่มือการใช้งาน MPU6050 Interfacingและ L298N Motor driver

การเขียนโปรแกรมหุ่นยนต์ปรับสมดุลตนเอง

ตอนนี้เราต้องโปรแกรม Arduino UNO board ของเราเพื่อปรับสมดุลหุ่นยนต์ นี่คือที่มายากลทั้งหมดที่เกิดขึ้น; แนวคิดเบื้องหลังมันเป็นเรื่องง่าย เราต้องตรวจสอบว่าบอทเอนตัวไปทางด้านหน้าหรือด้านหลังโดยใช้ MPU6050แล้วถ้าเอนเอียงไปทางด้านหน้าเราต้องหมุนล้อไปข้างหน้าและถ้าหากเอนเอียงไปทางด้านหลังเราก็ต้องหมุนล้อ ในทิศทางตรงกันข้าม
ในเวลาเดียวกันเรายังต้องควบคุมความเร็วที่ล้อหมุนอยู่หากบอทบิดเบี้ยวเล็กน้อยจากตำแหน่งกึ่งกลางล้อหมุนไปอย่างช้าๆและความเร็วเพิ่มขึ้นเมื่อห่างจากตำแหน่งตรงกลางมากขึ้น เพื่อให้บรรลุตรรกะนี้เราใช้อัลกอริทึม PID ซึ่งมีตำแหน่งกึ่งกลางเป็นจุดตั้งและระดับการสับสนเป็นเอาท์พุท
เพื่อทราบตำแหน่งปัจจุบันของบอทเราใช้MPU6050ซึ่งเป็นเครื่องวัดความเร่งแบบ 6 แกนและเซนเซอร์วัดการหมุนวนที่รวมกัน เพื่อให้ได้ค่าตำแหน่งที่เชื่อถือได้จากเซ็นเซอร์เราจำเป็นต้องใช้ค่าของเครื่องวัดความเร่งและเครื่องวัดการหมุนวนเพราะค่าจากเครื่องวัดความเร่งมีปัญหาเรื่องเสียงรบกวนและค่าจากเครื่องวัดการหมุนวนมีแนวโน้มที่จะล่องลอยไปตามเวลา ดังนั้นเราจึงต้องรวมทั้งและรับค่าของการขว้างและขว้างหุ่นของเราซึ่งเราจะใช้เฉพาะค่าของการเบี้ยว
เสียงบิตของหัว reeling ขวา? แต่ต้องกังวลไม่ขอบคุณชุมชน Arduino เรามีห้องสมุดพร้อมที่สามารถดำเนินการคำนวณ PID ​​และยังได้รับค่าของการหันเหจาก MPU6050 ห้องสมุดได้รับการพัฒนาโดยbr3ttbและjrowbergตามลำดับ ก่อนที่จะดำเนินการดาวน์โหลดไลบรารีของตนจะสร้างลิงก์ต่อไปนี้และเพิ่มลงในไดเรกทอรี Arduino lib ของคุณ
https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
ขณะนี้เรามีห้องสมุดที่เพิ่มเข้าไปใน Arduino IDE ของเรา เริ่มต้นการเขียนโปรแกรมสำหรับหุ่นยนต์สมดุลด้วยตนเองของเรา เช่นเคยรหัสที่สมบูรณ์สำหรับโครงการจะได้รับที่ส่วนท้ายของหน้านี้ที่นี่ฉันเพียงแค่การอธิบายตัวอย่างที่สำคัญที่สุดในรหัส A บอกก่อนหน้านี้รหัสถูกสร้างขึ้นด้านบนของรหัสตัวอย่าง MPU6050เราเพียงจะเพิ่มประสิทธิภาพของรหัสสำหรับวัตถุประสงค์ของเราและเพิ่ม PID และเทคนิคการควบคุมสำหรับหุ่นสมดุลของเราเอง
ขั้นแรกเราจะรวมไลบรารีที่จำเป็นสำหรับโปรแกรมนี้ไว้เพื่อใช้งาน รวมถึงไลบรารี I2C ที่สร้างขึ้นห้องสมุด PID และ MPU6050 Library ที่เราเพิ่งดาวน์โหลดมา
# include "I2Cdev.h" 
#include <PID_v1.h> // จาก https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h 
# include "MPU6050_6Axis_MotionApps20.h" // https : //github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

จากนั้นเราประกาศตัวแปรที่จะต้องได้รับข้อมูลจากเซ็นเซอร์ MPU6050 เราอ่านทั้งเวกเตอร์แรงโน้มถ่วงและค่า quaternion แล้วคำนวณค่า pitch และ roll ของบอท YPR ลอยอาร์เรย์ [3]จะถือผลสุดท้าย
/ การควบคุม / สถานะ MPU vars 
bool dmpReady = false; // ตั้งค่า true ถ้า init DMP สำเร็จ
uint8_t mpuIntStatus; / / ถือ byte สถานะ interrupt จริงจาก MPU 
uint8_t devStatus; / / สถานะกลับหลังจากการดำเนินการแต่ละอุปกรณ์ (0 = ความสำเร็จ,! 0 = ข้อผิดพลาด) 
uint16_t packetSize; / / คาดว่าขนาดแพ็คเก็ต DMP (ค่าเริ่มต้นคือ 42 ไบต์) 
uint16_t fifoCount; // นับจำนวนไบต์ทั้งหมดที่มีอยู่ใน FIFO 
uint8_t fifoBuffer [64]; // บัฟเฟอร์การจัดเก็บ FIFO 

// การวางแนว / การเคลื่อนที่ vars 
Quaternion q; // [w, x, y, z] คอนเทนเนอร์ quaternion 
VectorFloat แรงโน้มถ่วง; / / [x, y, z] เวกเตอร์แรงโน้มถ่วง
ลอย ypr [3]; // [ขวาง, ขว้าง, ม้วน] หีบ / ม้วน / ม้วนคอนเทนเนอร์และเวกเตอร์แรงโน้มถ่วง

ถัดมาส่วนที่สำคัญมากของรหัสและนี่คือที่คุณจะใช้จ่ายเป็นเวลานานในการปรับแต่งสำหรับการตั้งค่าที่เหมาะสมของค่า ถ้าหุ่นยนต์ตัวนี้ถูกสร้างขึ้นด้วยแรงโน้มถ่วงที่ดีมากและส่วนประกอบต่างๆจะถูกจัดเรียงโดยสมมาตร (ซึ่งในกรณีส่วนใหญ่ไม่เป็นเช่นนั้น) ค่าของเซตของคุณจะเท่ากับ 180 นอกจากนี้หากคุณเชื่อมต่อบอร์ของคุณเข้ากับจอแสดงผล Arduino serial และเอียงจน คุณจะหาตำแหน่งสมดุลที่ดีอ่านค่าที่แสดงบนจอแสดงผลอนุกรมและนี่คือค่าที่ตั้งไว้ ค่าของ Kp, Kd และ Ki ต้องได้รับการปรับตามบอตของคุณ ไม่มีสองบอทเท่ากันจะมีค่า Kp, Kd และ Ki เท่ากันดังนั้นจึงไม่สามารถหลบหนีได้ ดูวิดีโอที่ท้ายหน้านี้เพื่อรับทราบวิธีปรับค่าเหล่านี้
/ ********* ปรับค่าเหล่านี้ 4 สำหรับ BOOT ของคุณ ********* / 
double pointpoint = 176; // ตั้งค่าเมื่อบอทตั้งฉากกับพื้นโดยใช้จอแสดงผลแบบอนุกรม 
// อ่านเอกสารโครงการใน circuitdigest.com เพื่อเรียนรู้วิธีการตั้งค่าเหล่านี้
double Kp = 21; / / ตั้งครั้งแรกนี้
Kd = 0.8; // ตั้งค่าsec 
กี่ครั้ง
นี้Ki = 140; / / สุดท้ายตั้งค่านี้/ ****** สิ้นสุดการตั้งค่าค่า ********* /

ในบรรทัดถัดไปเราinitialise ขั้นตอนวิธี PID โดยผ่านตัวแปรนำเข้าส่งออกตั้งจุด Kp, Ki และ Kd จากจุดนี้เราได้ตั้งค่า Kp, Ki และ Kd ไว้ในตัวอย่างข้างต้นแล้ว ค่าของอินพุตจะเป็นค่าปัจจุบันของการเบี่ยงเบนที่อ่านได้จากเซ็นเซอร์ MPU6050 และค่าของเอาท์พุทจะเป็นค่าที่คำนวณโดยอัลกอริธึม PID ดังนั้นโดยทั่วไปอัลกอริธึม PID จะให้ค่าการส่งออกซึ่งควรใช้เพื่อแก้ไขค่า Input ให้ใกล้เคียงกับค่าที่ตั้งไว้
PID pid (& อินพุต & เอาท์พุทและ setpoint, Kp, Ki, Kd, ​​DIRECT);

ภายในฟังก์ชั่นการตั้งค่าโมฆะเราจะเริ่มต้น MPU6050 ด้วยการกำหนดค่า DMP (Digital Motion Processor) ซึ่งจะช่วยให้เราสามารถรวมข้อมูล Accelerometer กับข้อมูล Gyroscope และให้ค่า Yaw, Pitch and Roll ได้อย่างน่าเชื่อถือ เราจะไม่ไปลึกมากในเรื่องนี้เพราะมันจะไกลเกินกว่าหัวข้อ อย่างไรก็ตามส่วนหนึ่งส่วนของรหัสที่คุณต้องค้นหาในฟังก์ชันการตั้งค่าคือค่าชดเชยไจโร เซนเซอร์ MPU6050 แต่ละตัวมีค่าชดเชยของตัวเองคุณสามารถใช้ร่าง Arduinoนี้เพื่อคำนวณค่าชดเชยของเซ็นเซอร์และอัพเดตบรรทัดต่อไปนี้ในโปรแกรมของคุณ 
    // จัดหาการลดขนาดไจโรของคุณที่นี่ปรับขนาดความไวต่ำ
    สุด mpu.setXGyroOffset (220); 
    mpu.setYGyroOffset (76); 
    mpu.setZGyroOffset (-85); 
    mpu.setZAccelOffset (1688);

นอกจากนี้เราต้องเริ่มต้นหมุด PWM แบบดิจิทัลที่เราใช้เพื่อเชื่อมต่อมอเตอร์ของเราไปด้วย ในกรณีของเราคือ D6, D9, D10 และ D11 ดังนั้นเราจึงเริ่มต้นหมุดเหล่านี้เป็นพินขาออกทำให้ค่าต่ำสุดตามค่าเริ่มต้น
// Initialise มอเตอร์outpu หมุด
    pinMode (6, OUTPUT); 
    pinMode (9, เอาท์พุท); 
    pinMode (10, เอาท์พุท); 
    pinMode (11, เอาท์พุท); 

/ / โดยค่าเริ่มต้นปิดทั้งสองมอเตอร์
    analogWrite (6, ต่ำ); 
    analogWrite (9, LOW); 
    analogWrite (10, LOW); 
    analogWrite (11 LOW);

ภายในฟังก์ชั่นลูปหลักเราจะตรวจสอบว่าข้อมูลจาก MPU6050 นั้นพร้อมที่จะอ่านหรือไม่ ถ้าใช่แล้วเราจะใช้เพื่อคำนวณค่า PID และแสดงค่าอินพุตและเอาต์พุตของ PID ในจอมอนิเตอร์แบบอนุกรมเพื่อตรวจสอบว่า PID ตอบสนองอย่างไร จากนั้นขึ้นอยู่กับมูลค่าของผลผลิตที่เราตัดสินใจว่าบอทต้องเดินหน้าหรือถอยหลังหรือยืนนิ่ง
เนื่องจากเราคิดว่า MPU6050 จะคืนค่า 180 เมื่อบอทตั้งตรง เราจะได้รับการแก้ไขค่าบวกเมื่อบอทจะลดลงไปทางด้านหน้าและเราจะได้รับค่าในเชิงลบหาก ธ ปทจะลดลงไปทางด้านหลัง ดังนั้นเราจึงตรวจสอบเงื่อนไขนี้และเรียกใช้ฟังก์ชันที่เหมาะสมเพื่อย้าย bot ไปข้างหน้าหรือด้านหลังวอร์ด
    ในขณะที่ (! mpuInterrupt && fifoCount <packetSize) 
    { 
        // ไม่มีข้อมูล
mpu - ทำการคำนวณ PID ​​และส่งออกไปยังมอเตอร์             pid.Compute ();  
        // พิมพ์ค่าอินพุตและเอาต์พุตในจอภาพอนุกรมเพื่อตรวจสอบว่ามันทำงานอย่างไร         Serial.print (input); Serial.print ("=>"); Serial.println (output);         if (input> 150 && input <200) {// ถ้า Bot กำลังตกลง        ถ้า (เอาท์พุท> 0) // ตกไปทางด้านหน้า        Forward (); // หมุนล้อไปข้างหน้า        ถ้า (เอาท์พุท <0) // เลี้ยวไปทางด้านหลัง        ย้อนกลับ (); // หมุนล้อไปข้างหลัง        }         else // ถ้า Bot ไม่ตก        Stop ();

ตัวแปรเอาท์พุท PID ยังตัดสินใจวิธีการที่รวดเร็วมอเตอร์จะต้องมีการหมุน ถ้าบอทเพิ่งจะลดลงเราจะทำการแก้ไขเล็กน้อยด้วยการหมุนวงล้ออย่างช้าๆ หากการแก้ไขเล็กน้อยทำงาน dint และยังคงถ้า bot จะล้มลงเราเพิ่มความเร็วของมอเตอร์ ค่าของความเร็วในการหมุนล้อจะถูกกำหนดโดยอัลกอริทึม PI โปรดทราบว่าสำหรับฟังก์ชันย้อนกลับเราได้คูณค่าของเอาท์พุทด้วย -1 เพื่อให้เราสามารถแปลงค่าลบเป็นบวกได้  
void Forward () // Code เพื่อหมุนล้อไปข้างหน้า
{ 
    analogWrite (6, output); 
    analogWrite (9,0); 
    analogWrite (10, เอาท์พุท); 
    analogWrite (11,0); 
    Serial.print ( "F"); // Debugging information 
) 

void Reverse () // โค้ดเพื่อหมุนล้อย้อนกลับ
{ 
    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

DIY Self Balancing Robot ในการทำงาน

เมื่อคุณพร้อมกับฮาร์ดแวร์แล้วคุณสามารถอัปโหลดโค้ดลงในกระดาน Arduino ได้ ตรวจสอบให้แน่ใจว่าการเชื่อมต่อถูกต้องเนื่องจากเราใช้แบตเตอรี่ Li-ion เป็นสิ่งจำเป็นอย่างยิ่ง ดังนั้นตรวจสอบอีกครั้งสำหรับวงจรสั้นและตรวจสอบว่าขั้วจะไม่สัมผัสแม้บอทของคุณประสบผลกระทบเล็ก ๆ น้อย ๆ เปิดใช้งานโมดูลของคุณและเปิดจอภาพแบบอนุกรมถ้า Arduino ของคุณสามารถสื่อสารกับ MPU6050 ได้สำเร็จและหากทุกอย่างทำงานตามที่คาดไว้คุณจะเห็นหน้าจอต่อไปนี้
การติดต่อสื่อสารระหว่าง Arduino และ MPU6050

ที่นี่เราเห็นอินพุตและเอาต์พุตค่าของขั้นตอนวิธี PID ในการป้อนข้อมูลรูปแบบ => เอาท์พุท หาก ธ ปทที่มีความสมดุลอย่างสมบูรณ์แบบมูลค่าของการส่งออกจะเป็น 0 ค่าอินพุทคือค่าปัจจุบันจากเซ็นเซอร์ MPU6050 ตัวอักษร "F" แสดงให้เห็นว่าบอทกำลังเคลื่อนที่ไปข้างหน้าและ "R" แสดงให้เห็นว่าบอทอยู่ในสถานะย้อนกลับ
ในระหว่างขั้นตอนเริ่มต้นของ PID ผมขอแนะนำให้วางสายเคเบิล Arduino ของคุณเชื่อมต่อกับบอทเพื่อให้คุณสามารถตรวจสอบค่าอินพุตและเอาต์พุตได้ง่ายและยังสามารถแก้ไขและอัปโหลดโปรแกรมของคุณสำหรับค่า Kp, Ki และ Kd ได้อีกด้วย วิดีโอด้านล่างแสดงให้เห็นถึงการทำงานที่สมบูรณ์ของบอทและยังแสดงให้เห็นถึงวิธีการแก้ไขค่า PID ของคุณ
หวังว่านี่จะช่วยในการสร้างหุ่นยนต์สมดุลด้วยตนเองของคุณเองหากคุณมีปัญหาในการทำให้การทำงานแล้วปล่อยให้คำถามของคุณในส่วนความคิดเห็นด้านล่างหรือใช้ฟอรัมสำหรับคำถามทางเทคนิคเพิ่มเติม
รหัส
/ * Arduino หุ่นยนต์สมดุลตนเอง
 * รหัสโดย: B.Aswinth Raj 
 * สร้างด้านบนของ Lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
 * เว็บไซต์: circuitdigest.com  
 * /
# include "I2Cdev.h" 
#include <PID_v1.h> // จากhttps://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
# include "MPU6050_6Axis_MotionApps20.h" // https : //github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
MPU6050 mpu;
/ การควบคุม / สถานะ MPU vars 
bool dmpReady = false; // ตั้งค่า true ถ้า init DMP สำเร็จ
uint8_t mpuIntStatus; / / ถือ byte สถานะ interrupt จริงจาก MPU 
uint8_t devStatus; / / สถานะกลับหลังจากการดำเนินการแต่ละอุปกรณ์ (0 = ความสำเร็จ,! 0 = ข้อผิดพลาด) 
uint16_t packetSize; / / คาดว่าขนาดแพ็คเก็ต DMP (ค่าเริ่มต้นคือ 42 ไบต์) 
uint16_t fifoCount; // นับจำนวนไบต์ทั้งหมดที่มีอยู่ใน FIFO 
uint8_t fifoBuffer [64]; // FIFO storage buffer
/ / ปฐมนิเทศ / การเคลื่อนที่ vars 
Quaternion q; // [w, x, y, z] คอนเทนเนอร์ quaternion 
VectorFloat แรงโน้มถ่วง; / / [x, y, z] เวกเตอร์แรงโน้มถ่วง
ลอย ypr [3]; // [ขวาง, ขว้าง, ม้วน] หีบ / ม้วน / ม้วนคอนเทนเนอร์และเวกเตอร์แรงโน้มถ่วง

/ ********* ปรับค่าเหล่านี้ 4 สำหรับ BOOT ของคุณ ********* / 
double pointpoint = 176; // ตั้งค่าเมื่อบอทตั้งฉากกับพื้นโดยใช้จอแสดงผลแบบอนุกรม 
// อ่านเอกสารโครงการใน circuitdigest.com เพื่อเรียนรู้วิธีการตั้งค่าเหล่านี้
double Kp = 21; / / ตั้งค่านี้
คู่แรกKd = 0.8; // ตั้งค่า sec 
กี่ครั้งนี้Ki = 140; / / สุดท้ายตั้งค่านี้  
/ ****** สิ้นสุดการตั้งค่าค่า ********* /
อินพุตคู่, เอาท์พุท; 
PID pid (& อินพุต & เอาท์พุทและ setpoint, Kp, Ki, Kd, ​​DIRECT);

ระเหย bool mpuInterrupt = false; // แสดงว่าขาขัดจังหวะ MPU ขัดข้องสูง
dmpDataReady void () 
{ 
    mpuInterrupt = true; 
}
การตั้งค่าเป็นโมฆะ () { 
  Serial.begin (115200);
  // เตรียมอุปกรณ์
    Serial.println (F ("Initializing I2C devices ... ")); 
    mpu.initialize ();
     // ตรวจสอบการเชื่อมต่อ
    Serial.println (F ("การทดสอบอุปกรณ์เชื่อมต่อ ... ")); 
    Serial.println (mpu.testConnection ()? F ("การเชื่อมต่อ MPU6050"): F ("การเชื่อมต่อ MPU6050 ล้มเหลว"));
    // โหลดและกำหนดค่า DMP 
    devStatus = mpu.dmpInitialize ();
    
    // จัดหาการลดขนาดไจโรของคุณที่นี่ปรับขนาดความไวต่ำ
    สุด mpu.setXGyroOffset (220); 
    mpu.setYGyroOffset (76); 
    mpu.setZGyroOffset (-85); 
    mpu.setZAccelOffset (1688); 
      // ตรวจสอบให้แน่ใจว่าทำงานได้ (ส่งกลับ 0 ถ้าใช่) 
    if (devStatus == 0) 
    { 
        // เปิด DMP ตอนนี้ว่าพร้อม
        Serial.println (F ("Enable DMP ... ")); 
        mpu.setDMPEnabled (จริง);
        / / เปิดใช้ Arduino ขัดจังหวะการตรวจสอบ
        Serial.println (F ("การเปิดใช้งานการตรวจสอบ interrupt (Arduino ภายนอกขัดจังหวะ 0) ... ")); 
        attachInterrupt (0, dmpDataReady, RISING); 
        mpuIntStatus = mpu.getIntStatus ();
        / / ตั้งค่าสถานะ DMP Ready ของเราเพื่อให้ฟังก์ชันลูปหลัก () รู้ว่าไม่เป็นไรที่จะใช้
        Serial.println (F ("พร้อม DMP! รอการขัดจังหวะครั้งแรก ... ")); 
        dmpReady = true;
        / / รับขนาดแพ็คเก็ต DMP ที่คาดหวังสำหรับการเปรียบเทียบในภายหลัง
        packetSize = mpu.dmpGetFIFOPacketSize (); 
        
        // การตั้งค่า PID 
        pid.SetMode (AUTOMATIC); 
        pid.SetSampleTime (10); 
        pid.SetOutputLimits (-255, 255);  
    } 
    else 
    { 
        // ERROR! 
        // 1 = การโหลดหน่วยความจำเริ่มต้นล้มเหลว
        // 2 = การปรับปรุงการกำหนดค่า DMP ล้มเหลว
        // (ถ้าเป็นไปตามปกติรหัสจะเป็น 1) 
        Serial.print (F ("DMP Initialization failed (code)") 
        Serial พิมพ์ (devStatus); 
        Serial.println (F (")")); 
    }
/ / เริ่มต้นขามอเตอร์
    outpu pinMode (6, เอาท์พุท); 
    pinMode (9, เอาท์พุท); 
    pinMode (10, เอาท์พุท); 
    pinMode (11, เอาท์พุท);
/ / โดยค่าเริ่มต้นปิดทั้งสองมอเตอร์
    analogWrite (6, ต่ำ); 
    analogWrite (9, LOW); 
    analogWrite (10, LOW); 
    analogWrite (11 LOW); 
}

void loop () { 
 
    // ถ้าการเขียนโปรแกรมล้มเหลวอย่าพยายามทำอะไร
    ถ้า (! dmpReady) return;
    / รอ MPU ขัดจังหวะหรือแพ็คเก็ตพิเศษ (s) พร้อมใช้งาน
    ในขณะที่ (! mpuInterrupt && fifoCount <packetSize) 
    { 
        / / ไม่มีข้อมูลmpu - การคำนวณ PID ​​และส่งออกไปยังมอเตอร์      
        pid.Compute ();   
        
        // พิมพ์ค่าอินพุตและเอาต์พุตในจอภาพอนุกรมเพื่อตรวจสอบว่ามันทำงานอย่างไร 
        Serial.print (input); Serial.print ("=>"); Serial.println (output); 
               
        if (input> 150 && input <200) {// ถ้า Bot กำลังตกลง  
          
        ถ้า (เอาท์พุท> 0) // ตกไปทางด้านหน้า  
        Forward (); // หมุนล้อไปข้างหน้า  
        ถ้า (เอาท์พุท <0) // เลี้ยวไปทางด้านหลัง
        ย้อนกลับ (); // หมุนล้อไปข้างหลัง  
        }
        else // ถ้า Bot ไม่ตก
        Stop (); // ถือล้อไว้
        
    }
    / / ตั้งค่าธงขัดจังหวะและได้รับ INT_STATUS ไบต์
    mpuInterrupt = เท็จ; 
    mpuIntStatus = mpu.getIntStatus ();
    / / ได้รับ FIFO ปัจจุบันนับ
    fifoCount = mpu.getFIFOCount ();
    / / ตรวจสอบการล้น (นี้ไม่ควรเกิดขึ้นเว้นแต่รหัสของเราไม่มีประสิทธิภาพมากเกินไป) 
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) 
    { 
        // รีเซ็ตเพื่อให้เราสามารถดำเนินการต่อได้อย่างหมดจด
        mpu.resetFIFO (); 
        Serial.println (F ("FIFO overflow!"));
    // มิฉะนั้นให้ตรวจสอบข้อมูล DMP พร้อมขัดจังหวะ (นี้ควรจะเกิดขึ้นบ่อยครั้ง) 
    } 
    else if (mpuIntStatus & 0x02) 
    { 
        / / รอความถูกต้องของข้อมูลที่มีอยู่ควรเป็นระยะเวลาสั้นมาก
        ในขณะที่ fifoCount <packetSize> fifoCount = mpu getFIFOCount ();
        // อ่านแพคเก็ตจาก FIFO 
        mpu.getFIFOBytes (fifoBuffer, packetSize); 
        
        // track FIFO count ในกรณีที่มี> 1 packet available 
        // (นี้จะช่วยให้เราอ่านได้ทันทีโดยไม่ต้องรอให้ interrupt) 
        fifoCount - = packetSize;
        mpu.dmpGetQuaternion (& q, FIFOBuffer); / / รับค่าสำหรับ q 
        mpu.dmpGetGravity (& gravity, & q); // รับค่าสำหรับแรงโน้มถ่วง
        mpu.dmpGetYawPitchRoll (ypr, & q และแรงโน้มถ่วง); / / รับค่าสำหรับ ypr
        input = ypr [1] * 180 / M_PI + 180;
   } 
}
void Forward () // Code เพื่อหมุนล้อไปข้างหน้า  
{ 
    analogWrite (6, output); 
    analogWrite (9,0); 
    analogWrite (10, เอาท์พุท); 
    analogWrite (11,0); 
    Serial.print ( "F"); // แก้จุดบกพร่องข้อมูล  
}
ถือเป็นโมฆะย้อนกลับ () // รหัสเพื่อหมุนล้อย้อนกลับ   
{ 
    analogWrite (6,0); 
    analogWrite (9, เอาท์พุท * -1); 
    analogWrite (10,0); 
    analogWrite (11 เอาท์พุท * -1); 
    Serial.print ( "R"); 
}
void Stop () // Code เพื่อหยุดทั้งสองล้อ
{ 
    analogWrite (6,0); 
    analogWrite (9,0); 
    analogWrite (10,0); 
    analogWrite (11,0); 
    Serial.print ( "S"); 
}