თვითბალანსირებული რობოტი Magicbit– დან: 6 ნაბიჯი

2024 ავტორი: John Day | [email protected]. ბოლოს შეცვლილი: 2024-01-30 10:15

ეს გაკვეთილი გვიჩვენებს, თუ როგორ უნდა გავაკეთოთ თვითბალანსირებული რობოტი Magicbit dev დაფის გამოყენებით. ჩვენ ვიყენებთ magicbit– ს, როგორც განვითარების დაფას ამ პროექტში, რომელიც დაფუძნებულია ESP32– ზე. ამიტომ ნებისმიერი ESP32 განვითარების დაფა შეიძლება გამოყენებულ იქნას ამ პროექტში.

მასალები:

• მაგიბიბიტი
• ორმაგი 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- განუყოფელი
• დ- წარმოებული

თითოეულ სისტემას აქვს შესავალი და გამომავალი. ანალოგიურად ამ საკონტროლო სისტემასაც აქვს გარკვეული შეყვანა. ამ კონტროლის სისტემაში არის გადახრა სტაბილური მდგომარეობიდან. ჩვენ ამას შეცდომას ვუწოდებთ. ჩვენს რობოტში შეცდომა არის დახრის კუთხე ვერტიკალური სიბრტყისაგან. თუ რობოტი დაბალანსებულია, დახრის კუთხე ნულის ტოლია. ასე რომ, შეცდომის მნიშვნელობა იქნება ნული. აქედან გამომდინარე, PID სისტემის გამომუშავება ნულის ტოლია. ეს სისტემა მოიცავს სამ ცალკეულ მათემატიკურ პროცესს.

პირველი არის რიცხვითი მოგებიდან შეცდომის გამრავლება. ამ მოგებას ჩვეულებრივ უწოდებენ Kp

P = შეცდომა*Kp

მეორე არის შეცდომის ინტეგრალის წარმოქმნა დროის დომენში და მისი გამრავლება გარკვეული მოგებიდან. ამ მოგებას ეძახიან როგორც Ki

I = ინტეგრალური (შეცდომა)*კი

მესამე არის შეცდომის წარმოშობა დროის დომენში და გავამრავლოთ იგი გარკვეული რაოდენობის მოგებით. ამ მოგებას ეწოდება ქდ

D = (d (შეცდომა)/dt)*kd

ზემოაღნიშნული ოპერაციების დამატების შემდეგ ჩვენ ვიღებთ ჩვენს საბოლოო გამომავალს

გამოსავალი = P+I+D

P ნაწილის გამო რობოტს შეუძლია მიიღოს სტაბილური პოზიცია, რაც პროპორციულია გადახრისა. I ნაწილი ითვლის შეცდომის არეალს დროის გრაფიკის წინააღმდეგ. ის ცდილობს რობოტი ყოველთვის ზუსტად მიიყვანოს სტაბილურ მდგომარეობაში. D ნაწილი ზომავს ფერდობს დროში შეცდომის გრაფიკის წინააღმდეგ. თუ შეცდომა იზრდება ეს მნიშვნელობა დადებითია. თუ შეცდომა მცირდება, ეს არის უარყოფითი. ამის გამო, როდესაც რობოტი გადადის სტაბილურ პოზიციაში, მაშინ რეაქციის სიჩქარე შემცირდება და ეს ხელს შეუწყობს ზედმეტი გადაღებების ამოღებას. თქვენ შეგიძლიათ გაიგოთ მეტი PID თეორიის შესახებ ქვემოთ მოცემულ ბმულზე.

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

PID ფუნქციის გამომავალი ზღვარია 0-255 დიაპაზონში (8 ბიტიანი PWM გარჩევადობა) და ის მიეწოდება ძრავებს PWM სიგნალის სახით.

ნაბიჯი 3: აპარატურის დაყენება

ახლა ეს არის აპარატურის დაყენების ნაწილი. რობოტის დიზაინი თქვენზეა დამოკიდებული. რობოტის სხეულის შემუშავებისას თქვენ უნდა გაითვალისწინოთ ის სიმეტრიულად ვერტიკალური ღერძის შესახებ, რომელიც მდებარეობს საავტომობილო ღერძში. ბატარეის პაკეტი მდებარეობს ქვემოთ. ამრიგად, რობოტის დაბალანსება ადვილია. ჩვენს დიზაინში ჩვენ ვამაგრებთ Magicbit დაფას ვერტიკალურად სხეულზე. ჩვენ გამოვიყენეთ ორი 12V სიჩქარის ძრავა. მაგრამ თქვენ შეგიძლიათ გამოიყენოთ ნებისმიერი სახის გადაცემათა კოლოფი. ეს დამოკიდებულია თქვენს რობოტის ზომებზე.

როდესაც ვსაუბრობთ მიკროსქემის შესახებ, ის იკვებება 7.4 ვ ლიპო ბატარეით. Magicbit– მა გამოიყენა 5V კვებისათვის. ამიტომ ჩვენ გამოვიყენეთ 7805 რეგულატორი ბატარეის ძაბვის 5 ვ -მდე რეგულირებისთვის. Magicbit– ის შემდგომ ვერსიებში ეს რეგულატორი არ არის საჭირო. რადგან ის იკვებება 12 ვ -მდე. ჩვენ პირდაპირ ვაძლევთ ძრავის მძღოლს 7.4 ვ.

შეაერთეთ ყველა კომპონენტი ქვემოთ მოცემული დიაგრამის მიხედვით.

ნაბიჯი 4: პროგრამული უზრუნველყოფის დაყენება

კოდში ჩვენ გამოვიყენეთ PID ბიბლიოთეკა PID გამომუშავების გამოსათვლელად.

გადადით შემდეგ ბმულზე, რომ გადმოწეროთ.

www.arduinolibraries.info/libraries/pid

ჩამოტვირთეთ მისი უახლესი ვერსია.

სენსორების უკეთესი კითხვების მისაღებად ჩვენ გამოვიყენეთ DMP ბიბლიოთეკა. DMP ნიშნავს ციფრული მოძრაობის პროცესს. ეს არის MPU6050 ჩაშენებული მახასიათებელი. ამ ჩიპს აქვს ინტეგრირებული მოძრაობის პროცესის ერთეული. ასე რომ საჭიროა კითხვა და ანალიზი. მას შემდეგ რაც ის ქმნის უხმაურო ზუსტ გამოსავალს მიკროკონტროლერზე (ამ შემთხვევაში Magicbit (ESP32)). მაგრამ ბევრი სამუშაოა მიკროკონტროლერის მხრივ, რომ მიიღოს ეს მაჩვენებლები და გამოთვალოს კუთხე. მარტივად რომ ვთქვათ ჩვენ გამოვიყენეთ MPU6050 DMP ბიბლიოთეკა. გადმოწერეთ იგი შემდეგ ბმულზე.

github.com/ElectronicCats/mpu6050

ბიბლიოთეკების დაყენების მიზნით, Arduino მენიუში გადადით ინსტრუმენტებზე-> მოიცავს ბიბლიოთეკა-> add.zip ბიბლიოთეკას და შეარჩიეთ გადმოწერილი ბიბლიოთეკის ფაილი.

კოდში თქვენ უნდა შეცვალოთ მითითებული კუთხე სწორად. PID– ის მუდმივი მნიშვნელობები განსხვავდება რობოტიდან რობოტამდე. ასე რომ, ამის დარეგულირებაში, ჯერ დააყენეთ Ki და Kd ნულოვანი მნიშვნელობები და შემდეგ გაზარდეთ Kp სანამ არ მიიღებთ რეაქციის უკეთეს სიჩქარეს. მეტი Kp იწვევს მეტ გადატვირთვას. შემდეგ გაზარდეთ Kd მნიშვნელობა. გაზარდეთ ის ყოველთვის ძალიან მცირე რაოდენობით. ეს მნიშვნელობა ზოგადად დაბალია ვიდრე სხვა მნიშვნელობები. ახლა გაზარდეთ Ki სანამ არ გექნებათ ძალიან კარგი სტაბილურობა.

აირჩიეთ სწორი COM პორტი და დაფის ტიპი. ატვირთეთ კოდი. ახლა თქვენ შეგიძლიათ ითამაშოთ თქვენი წვრილმანი რობოტით.

ნაბიჯი 5: სქემა

ნაბიჯი 6: კოდი

#ჩართეთ

#მოიცავს "I2Cdev.h" #მოიცავს "MPU6050_6Axis_MotionApps20.h" #თუ I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #მოიცავს "Wire.h" #endif MPU6050 mpu; bool dmpReady = ყალბი; // დააყენეთ true თუ DMP init წარმატებული იყო uint8_t mpuIntStatus; // ფლობს ფაქტობრივ შეწყვეტის სტატუსს byte 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 gravity; // [x, y, z] გრავიტაციის ვექტორი float ypr [3]; // [yaw, pitch, roll] yaw/pitch/roll კონტეინერი და სიმძიმის ვექტორი ორმაგი ორიგინალური მითითებული = 172.5; ორმაგი setpoint = originalSetpoint; ორმაგი მოძრავი AngleOffset = 0.1; ორმაგი შეყვანა, გამომავალი; int moveState = 0; ორმაგი Kp = 23; // მითითებული P პირველი ორმაგი Kd = 0.8; // ეს მნიშვნელობა ზოგადად მცირე ორმაგი Ki = 300; // ეს მნიშვნელობა უნდა იყოს მაღალი სტაბილურობისთვის PID pid (& შეყვანა, & გამომავალი, & setpoint, Kp, Ki, Kd, DIRECT); // pid ინიციალიზაცია int motL1 = 26; // 4 ქინძისთავი საავტომობილო დისკზე int motL2 = 2; int motR1 = 27; int motR2 = 4; არასტაბილური ბოლი mpuInterrupt = ყალბი; // მიუთითებს თუ არა MPU შეწყვეტის პინზე მაღალი void dmpDataReady () {mpuInterrupt = true; } void setup () {ledcSetup (0, 20000, 8); // pwm setup ledcSetup (1, 20000, 8); ledcSetup (2, 20000, 8); ledcSetup (3, 20000, 8); ledcAttachPin (motL1, 0); // ძრავების pinmode ledcAttachPin (motL2, 1); ledcAttachPin (motR1, 2); ledcAttachPin (motR2, 3); // შეუერთდით I2C ავტობუსს (I2Cdev ბიბლიოთეკა ამას ავტომატურად არ აკეთებს) #თუ 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 მოწყობილობების ინიციალიზაცია …")); pinMode (14, INPUT); // სერიული კომუნიკაციის ინიციალიზაცია // (115200 არჩეულია იმიტომ, რომ ეს საჭიროა Teapot Demo გამოსაყვანად, მაგრამ ეს // ნამდვილად თქვენია თქვენი პროექტის მიხედვით) Serial.begin (9600); ხოლო (! სერიული); // დაელოდეთ ლეონარდოს ჩამოთვლას, სხვები დაუყოვნებლივ აგრძელებენ // მოწყობილობის ინიციალიზაცია Serial.println (F ("I2C მოწყობილობების ინიციალიზაცია …")); mpu. ინიციალიზაცია (); // კავშირის გადამოწმება Serial.println (F ("მოწყობილობის კავშირების ტესტირება …")); Serial.println (mpu.testConnection ()? F ("MPU6050 კავშირი წარმატებულია"): F ("MPU6050 კავშირი ვერ მოხერხდა")); // ჩატვირთეთ და დააკონფიგურირეთ DMP Serial.println (F ("DMP ინიციალიზაცია …")); devStatus = mpu.dmpInitialize (); // მიაწოდეთ თქვენი საკუთარი გიროსოფსეტები აქ, შემცირებული მინიმალური მგრძნობელობით mpu.setXGyroOffset (220); mpu.setYGyroOffset (76); mpu.setZGyroOffset (-85); mpu.setZAccelOffset (1788); // 1688 ქარხნული ნაგულისხმევი ჩემი საცდელი ჩიპისთვის // დარწმუნდით, რომ ის მუშაობდა (აბრუნებს 0 -ს) თუ (devStatus == 0) {// ჩართეთ DMP, ახლა უკვე მზად არის Serial.println (F ("ჩართვა DMP … ")); mpu.setDMP ჩართულია (ჭეშმარიტი); // ჩართეთ Arduino შეწყვეტის გამოვლენა Serial.println (F ("შეწყვეტის გამოვლენის ჩართვა (Arduino გარე შეფერხება 0) …")); attachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // დააყენეთ ჩვენი DMP Ready დროშა, რათა მთავარმა მარყუჟის () ფუნქციამ იცოდეს, რომ ნორმალურია მისი გამოყენება Serial.println (F ("DMP მზად არის! ველოდები პირველ შეწყვეტას …")); dmpReady = ჭეშმარიტი; // მიიღეთ მოსალოდნელი DMP პაკეტის ზომა შემდგომი შედარებისთვის packetSize = mpu.dmpGetFIFOPacketSize (); // PID pid. SetMode (AUTOMATIC) დაყენება; pid. SetSampleTime (10); pid. SetOutputLimits (-255, 255); } სხვა {// შეცდომა! // 1 = მეხსიერების საწყისი ჩატვირთვა ვერ მოხერხდა // 2 = DMP კონფიგურაციის განახლება ვერ მოხერხდა // (თუ ის გატეხავს, ჩვეულებრივ კოდი იქნება 1) Serial.print (F ("DMP ინიციალიზაცია ვერ მოხერხდა (კოდი")); სერიული ბეჭდვა (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 (); // გადამოწმება (ეს არ უნდა მოხდეს, თუ ჩვენი კოდი ძალიან არაეფექტურია) თუ ((mpuIntStatus & 0x10) || fifoCount == 1024) {// გადატვირთვა, ასე რომ ჩვენ შეგვიძლია გავაგრძელოთ სუფთა mpu.resetFIFO (); Serial.println (F ("FIFO overflow!")); // წინააღმდეგ შემთხვევაში, შეამოწმეთ DMP მონაცემების მზადაა შეწყვეტა (ეს ხშირად უნდა ხდებოდეს)} სხვა შემთხვევაში თუ (mpuIntStatus & 0x02) {// დაელოდეთ მონაცემების სწორ ხელმისაწვდომ სიგრძეს, უნდა იყოს ძალიან მოკლე ლოდინი სანამ (fifoCount 1 პაკეტი ხელმისაწვდომია // (ეს საშუალებას მოგვცემს დაუყოვნებლივ წავიკითხოთ მეტი შეფერხების ლოდინის გარეშე) ბეჭდვა ("ypr / t"); Serial.print (ypr [0] * 180/M_PI); // euler angles 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;}} void motorSpeed (int PWM) {float L1, L2, R1, R2; თუ (PWM> = 0) {// წინ მიმართულება L2 = 0; L1 = abs (PWM); R2 = 0; R1 = abs (PWM); თუ (L1> = 255) { L1 = R1 = 255;}} სხვა {// უკან მიმართულება L1 = 0; L2 = აბს (PWM); R1 = 0; R2 = აბს (PWM); თუ (L2> = 255) {L2 = R2 = 255; }} // საავტომობილო დისკი ledcWrite (0, L1); ledcWrite (1, L2); ledcWrite (2, R1*0.97); // 0.97 არის სიჩქარის ფაქტი ან, რადგან მარჯვენა ძრავას აქვს უფრო მაღალი სიჩქარე ვიდრე მარცხენა ძრავა, ამიტომ ჩვენ ვამცირებთ მას სანამ ძრავის სიჩქარე არ იქნება თანაბარი ledcWrite (3, R2*0.97);

}