Magicbit -аас өөрийгөө тэнцүүлэх робот: 6 алхам

2024 Зохиолч: John Day | [email protected]. Хамгийн сүүлд өөрчлөгдсөн: 2024-01-30 11:00

Энэхүү заавар нь Magicbit dev самбар ашиглан өөрийгөө тэнцвэржүүлэх робот хэрхэн хийхийг харуулдаг. Бид ESP32 дээр суурилсан энэхүү төсөлд magicbit -ийг хөгжлийн самбар болгон ашиглаж байна. Тиймээс энэ төсөлд ESP32 хөгжүүлэх самбарыг ашиглаж болно.

Хангамж:

• шидэт бит
• Хос гүүр L298 хос жолооч
• Шугаман зохицуулагч (7805)
• Lipo 7.4V 700mah батерей
• Инерцийн хэмжилтийн нэгж (IMU) (6 градус эрх чөлөө)
• 3V-6V DC арааны мотор

Алхам 1: Түүх

Хөөе залуусаа, өнөөдөр бид энэ хичээлээр бага зэрэг нарийн төвөгтэй зүйлийн талаар олж мэдэх болно. Энэ бол Arduino IDE бүхий Magicbit ашиглан өөрийгөө тэнцвэржүүлэх роботын тухай юм. Тиймээс эхэлцгээе.

Юуны өмнө өөрийгөө тэнцвэржүүлэх робот гэж юу болохыг харцгаая. Өөрөө тэнцвэржүүлэх робот бол хоёр дугуйтай робот юм. Онцлог шинж чанар нь робот гадны ямар ч дэмжлэггүйгээр өөрийгөө тэнцвэржүүлэх боломжтой юм. Эрчим хүч асахад робот босч, хэлбэлзлийн хөдөлгөөнийг ашиглан тэнцвэржүүлнэ. Тиймээс одоо та бүхэнд өөрийгөө тэнцвэржүүлэх роботын талаар бага зэрэг ойлголттой байна.

Алхам 2: Онол, арга зүй

Роботыг тэнцвэржүүлэхийн тулд эхлээд роботын өнцгийг босоо хавтгайд хэмжихийн тулд зарим мэдрэгчээс мэдээлэл авдаг. Үүний тулд бид MPU6050 -ийг ашигласан. Мэдрэгчээс мэдээлэл авсны дараа бид босоо хавтгай руу хазайлтыг тооцоолно. Хэрэв робот шулуун, тэнцвэртэй байрлалд байвал хазайлтын өнцөг тэг болно. Үгүй бол хазайлтын өнцөг нь эерэг эсвэл сөрөг утгатай байна. Хэрэв робот урд тал руугаа хазайсан бол робот урд зүг рүү чиглэх ёстой. Хэрэв робот урвуу тийш хазайсан бол робот урвуу чиглэлд шилжих ёстой. Хэрэв энэ хазайлтын өнцөг өндөр байвал хариу өгөх хурд өндөр байх ёстой. Эсрэг тохиолдолд хазайлтын өнцөг бага, урвалын хурд бага байх ёстой. Энэ үйл явцыг хянахын тулд бид PID хэмээх тусгай теоремыг ашигласан. PID бол олон процессыг хянахад ашигладаг хяналтын систем юм. PID нь 3 процессыг илэрхийлдэг.

• P- пропорциональ
• I- салшгүй
• D- дериватив

Систем бүр оролт, гаралттай байдаг. Үүнтэй адилаар энэ хяналтын систем нь бас тодорхой хэмжээний оролттой байдаг. Энэхүү хяналтын системд энэ нь тогтвортой байдлаас хазайх явдал юм. Бид үүнийг алдаа гэж нэрлэсэн. Манай роботын хувьд алдаа нь босоо хавтгайгаас хазайх өнцөг юм. Хэрэв робот тэнцвэртэй байвал хазайлтын өнцөг тэг болно. Тиймээс алдааны утга тэг болно. Тиймээс PID системийн гаралт тэг байна. Энэ систем нь гурван тусдаа математикийн процессыг агуулдаг.

Нэгдүгээрт, тоон олзны алдааг үржүүлэх. Энэ ололтыг ихэвчлэн Kp гэж нэрлэдэг

P = алдаа*Kp

Хоёрдугаарт, цаг хугацааны домэйнд гарсан алдааны интегралийг бий болгож, түүнийг зарим ашгаас үржүүлнэ. Энэ ашиг нь Ки гэж нэрлэгддэг

I = Интеграл (алдаа)*Ки

Гуравдугаарт, цаг хугацааны домэйны алдаанаас үүсэлтэй бөгөөд үүнийг тодорхой хэмжээний ашиг орлогоор үржүүлнэ. Энэ ололтыг Kd гэж нэрлэдэг

D = (d (алдаа)/dt)*kd

Дээрх үйлдлүүдийг нэмсний дараа бид эцсийн үр дүнг авна

OUTPUT = 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 хүртэлх хязгаартай (8 битийн ХОУХШ) бөгөөд энэ нь хөдөлгүүрт ХОУХД-ийн дохиогоор тэжээгддэг.

Алхам 3: Тоног төхөөрөмжийн тохиргоо

Одоо энэ бол тоног төхөөрөмжийн тохиргооны хэсэг юм. Роботын загвар нь танаас хамаарна. Роботын биеийг зохион бүтээхдээ хөдөлгүүрийн тэнхлэгт байрлах босоо тэнхлэгийн хувьд тэгш хэмтэй байх ёстой. Батерейны багцыг доор байрлуулна. Тиймээс роботыг тэнцвэржүүлэхэд хялбар байдаг. Манай дизайны хувьд бид Magicbit хавтанг биедээ босоо байдлаар засдаг. Бид 12V хүчдэлийн хоёр хөдөлгүүрийг ашигласан. Гэхдээ та ямар ч төрлийн арааны мотор ашиглаж болно. Энэ нь таны роботын хэмжээнээс хамаарна.

Бид хэлхээний талаар ярилцахдаа 7.4V Lipo батерейгаар тэжээгддэг. Magicbit нь 5V хүчдэлийг ашигладаг. Тиймээс бид батерейны хүчдэлийг 5V хүртэл тохируулахын тулд 7805 зохицуулагчийг ашигласан. Magicbit -ийн дараагийн хувилбаруудад энэ зохицуулагч шаардлагагүй болно. Учир нь энэ нь 12 В хүртэл хүчдэлтэй. Бид мотор жолоочийн хувьд 7.4V шууд нийлүүлдэг.

Доорх диаграмын дагуу бүх бүрэлдэхүүн хэсгүүдийг холбоно уу.

Алхам 4: Програм хангамжийн тохиргоо

Код дээр бид PID гаралтыг тооцоолохын тулд PID номын санг ашигласан.

Дараах линкээр орж татаж авна уу.

www.arduinolibraries.info/libraries/pid

Үүний хамгийн сүүлийн хувилбарыг татаж аваарай.

Мэдрэгчийг илүү сайн уншихын тулд бид DMP номын санг ашигласан. DMP нь дижитал хөдөлгөөний үйл явцыг илэрхийлдэг. Энэ бол MPU6050 -ийн суулгагдсан онцлог юм. Энэхүү чип нь хөдөлгөөнт процессын нэгдсэн нэгжтэй. Тиймээс үүнийг уншиж, дүн шинжилгээ хийх шаардлагатай болно. Энэ нь микроконтроллерт дуу чимээгүй үнэн зөв гаралт үүсгэдэг (энэ тохиолдолд Magicbit (ESP32)). Гэхдээ микроконтроллерийн тал дээр уг уншилтыг авах, өнцгийг тооцоолох олон ажил бий. Тиймээс бид MPU6050 DMP номын санг ашигласан болно. Үүнийг goint -ээр дараах линк рүү татаж аваарай.

github.com/ElectronicCats/mpu6050

Номын санг суулгахын тулд Arduino цэсэн дэх tools-> library-> add.zip номын санг оруулаад татаж авсан номын сангийн файлыг сонгоно уу.

Кодод та тогтоосон цэгийн өнцгийг зөв өөрчлөх ёстой. PID тогтмол утга нь роботоос роботоос өөр байдаг. Үүнийг тохируулахдаа эхлээд Ki ба Kd утгыг тэг болгож, дараа нь хариу урвалын хурдыг сайжруулах хүртэл Kp -ийг нэмэгдүүлнэ. Илүү их Kp нь хэт их цохилт өгөх шалтгаан болдог. Дараа нь Kd утгыг нэмэгдүүлэх. Үүнийг үргэлж маш бага хэмжээгээр нэмэгдүүлэх. Энэ утга нь бусад утгаас ерөнхийдөө бага байна. Одоо маш сайн тогтвортой байдалд хүрэх хүртэл Ki -ийг нэмэгдүүлээрэй.

Зөв COM портыг сонгоод самбар дээр a гэж бичнэ үү. кодыг байршуулах. Одоо та DIY роботтой тоглох боломжтой.

Алхам 5: Схем

Алхам 6: Код

#оруулах

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include "Wire.h" #endif MPU6050 mpu; bool 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] quaternion контейнер VectorFloat таталцал; // [x, y, z] таталцлын вектор float ypr [3]; // [yaw, pitch, roll] yaw/pitch/roll контейнер ба таталцлын вектор давхар originalSetpoint = 172.5; давхар тохируулах цэг = анхны тохируулах цэг; double moveAngleOffset = 0.1; давхар оролт, гаралт; int moveState = 0; Давхар Kp = 23; // P -г эхний давхар Kd = 0.8; // энэ утга нь ерөнхийдөө жижиг давхар Ki = 300; // илүү сайн тогтвортой байхын тулд энэ утга өндөр байх ёстой PID pid (& оролт, & гаралт, & тогтоосон цэг, Kp, Ki, Kd, Шууд); // pid эхлүүлэх int motL1 = 26; int motR1 = 27; int motR2 = 4; дэгдэмхий bool mpuInterrupt = false; // нь MPU тасалдлын зүү өндөр хүчингүй болсон эсэхийг заана dmpDataReady () {mpuInterrupt = true; } void setup () {ledcSetup (0, 20000, 8); // pwm тохиргоо 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 номын сан үүнийг автоматаар хийдэггүй) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin (); Wire.setClock (400000); // 400 кГц I2C цаг. Эмхэтгэхэд хүндрэлтэй байгаа бол энэ мөрийг бичнэ үү #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire:: setup (400, үнэн); #endif Serial.println (F ("I2C төхөөрөмжийг эхлүүлж байна …")); pinMode (14, INPUT); // цуваа холболтыг эхлүүлэх // (энэ нь Teapot Demo гаралтанд шаардлагатай учир 115200 сонгосон, гэхдээ энэ нь таны төслөөс хамаарч танд хамаарна) Serial.begin (9600); байхад (! Цуваа); // Леонардогийн тооллогыг хүлээх, бусад нь шууд үргэлжлүүлэх // Serial.println төхөөрөмжийг эхлүүлэх (F ("I2C төхөөрөмжийг эхлүүлэх …")); mpu.initialize (); // холболтыг шалгах 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.setDMPEnabled (үнэн); // Arduino тасалдлын илрүүлэлтийг идэвхжүүлэх Serial.println (F ("Тасалдлын илрүүлэлтийг идэвхжүүлэх (Arduino гадаад тасалдал 0) …")); attachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // манай DMP Бэлэн тугийг тохируулах тул үндсэн давталт () функц үүнийг ашиглах нь зөв гэдгийг мэддэг 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) буцах тохиолдолд юу ч битгий оролдоорой; // (! 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, & grif) хэвлэх ("ypr / t"); Serial.print (ypr [0] * 180/M_PI); // euler өнцөг 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; if (PWM> = 0) {// урагшлах чиглэл 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);

}