Ինքնահավասարակշռող ռոբոտ Magicbit- ից ՝ 6 քայլ

2024 Հեղինակ: John Day | [email protected]. Վերջին փոփոխված: 2024-01-30 09:45

Այս ձեռնարկը ցույց է տալիս, թե ինչպես պատրաստել ինքնահավասարակշռող ռոբոտ ՝ օգտագործելով 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- ինտեգրալ
• D- ածանցյալ

Յուրաքանչյուր համակարգ ունի մուտք և ելք: Նույն կերպ այս կառավարման համակարգը նույնպես որոշակի մուտք ունի: Այս կառավարման համակարգում դա կայուն վիճակից շեղում է: Մենք դա անվանեցինք որպես սխալ: Մեր ռոբոտում սխալը ուղղահայաց հարթությունից թեքության անկյունն է: Եթե ռոբոտը հավասարակշռված է, ապա թեքության անկյունը զրո է: Այսպիսով, սխալի արժեքը կլինի զրո: Այսպիսով, PID համակարգի ելքը զրո է: Այս համակարգը ներառում է երեք առանձին մաթեմատիկական գործընթացներ:

Առաջինը `բազմապատկել սխալը թվային շահույթից: Այս շահույթը սովորաբար կոչվում է որպես Kp:

P = սխալ*Kp

Երկրորդը `ժամանակի տիրույթում առաջացնում է սխալի ինտեգրալ և բազմապատկում այն որոշ օգուտներից: Այս շահույթը կոչվում է Ki

I = Ինտեգրալ (սխալ)*Ki

Երրորդը ժամանակի տիրույթի սխալի ածանցյալն է և այն բազմապատկել որոշակի օգուտով: Այս շահույթը կոչվում է որպես Kd

D = (d (սխալ)/dt)*kd

Վերոնշյալ գործողությունները ավելացնելուց հետո մենք ստանում ենք մեր վերջնական արդյունքը

Ելք = P+I+D

P մասի պատճառով ռոբոտը կարող է ստանալ կայուն դիրք, որը համաչափ է շեղմանը: I մասը հաշվարկում է սխալի մակերեսը ընդդեմ ժամանակի գրաֆիկի: Այսպիսով, այն փորձում է ռոբոտին կայուն ճշգրիտ հասցնել: D մասը չափում է թեքությունը ժամանակի և սխալի գրաֆիկի նկատմամբ: Եթե սխալը մեծանում է, ապա այդ արժեքը դրական է: Եթե սխալը նվազում է, ապա այդ արժեքը բացասական է: Դրա պատճառով, երբ ռոբոտը տեղափոխվում է կայուն դիրքի, ապա ռեակցիայի արագությունը կնվազի, և դա կօգնի հեռացնել ավելորդ անցումները: PID տեսության մասին ավելին կարող եք իմանալ ստորև ներկայացված այս հղումից:

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

PID ֆունկցիայի ելքը սահմանափակված է 0-255 տիրույթով (8 բիթանոց PWM լուծաչափ) և այն սնուցվում է շարժիչներին որպես PWM ազդանշան:

Քայլ 3: Սարքավորման կարգավորում

Այժմ սա ապարատային տեղադրման մասն է: Ռոբոտի դիզայնը կախված է ձեզանից: Երբ նախագծում եք ռոբոտի մարմինը, դուք պետք է համարեք սիմետրիկ այն ուղղահայաց առանցքի շուրջ, որը գտնվում է շարժիչի առանցքում: Ստորև տեղադրված մարտկոցի փաթեթը: Այսպիսով, ռոբոտին հեշտ է հավասարակշռել: Մեր դիզայնով մենք Magicbit տախտակն ուղղահայաց ամրացնում ենք մարմնին: Մենք օգտագործեցինք երկու 12 Վ լարման շարժիչ: Բայց դուք կարող եք օգտագործել ցանկացած տեսակի փոխանցման շարժիչներ: դա կախված է ձեր ռոբոտի չափերից:

Երբ մենք խոսում ենք միացման մասին, այն սնուցվում է 7.4 Վ լիպո մարտկոցով: Magicbit- ն օգտագործել է 5 Վ լարման համար: Այսպիսով, մենք օգտագործեցինք 7805 կարգավորիչ `մարտկոցի լարումը մինչև 5 Վ կարգավորելու համար: Magicbit- ի հետագա տարբերակներում այդ կարգավորիչը կարիք չունի: Քանի որ այն աշխատում է մինչև 12 Վ լարման: Մենք ուղղակիորեն մատակարարում ենք 7.4 Վ շարժիչի վարորդի համար:

Միացրեք բոլոր բաղադրիչները ստորև ներկայացված սխեմայի համաձայն:

Քայլ 4: Softwareրագրաշարի տեղադրում

Կոդում մենք օգտագործում էինք 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 նավահանգիստը և տախտակը մուտքագրեք. վերբեռնեք կոդը: Այժմ կարող եք խաղալ ձեր DIY ռոբոտի հետ:

Քայլ 5: Սխեմաներ

Քայլ 6: Կոդ

#ներառում

#ներառել «I2Cdev.h» #ներառել «MPU6050_6Axis_MotionApps20.h» #եթե I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #Include «Wire.h» #endif MPU6050 mpu; bool dmpReady = կեղծ; // սահմանել true, եթե 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 gravity; // [x, y, z] ինքնահոս վեկտորը float ypr [3]; // [yaw, pitch, roll] yaw/pitch/roll բեռնարկղ և ինքնահոս վեկտոր կրկնակի բնօրինակըSetpoint = 172.5; կրկնակի սահմանման կետ = originalSetpoint; կրկնակի շարժվողAngleOffset = 0.1; կրկնակի մուտք, ելք; int moveState = 0; կրկնապատկել Kp = 23; // սահմանել P առաջին կրկնապատկել Kd = 0.8; // այս արժեքը, ընդհանուր առմամբ, փոքր կրկնակի Ki = 300; // այս արժեքը պետք է բարձր լինի PID- ի ավելի լավ կայունության համար (& մուտք, & ելք, & սահմանման կետ, Kp, Ki, Kd Ուղղակի int motR1 = 27; int motR2 = 4; անկայուն bool mpuInterrupt = կեղծ; // ցույց է տալիս, արդյոք MPU- ի ընդհատման քորոցը բարձր է անվավեր 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, Մուտք); // սկզբնավորել սերիական հաղորդակցությունը // (115200 ընտրված է, քանի որ դա անհրաժեշտ է Teapot Demo ելքի համար, բայց դա // իսկապես կախված է ձեր նախագծից) 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.setDMP Միացված է (ճշմարիտ); // միացնել Arduino- ի ընդհատումների հայտնաբերումը Serial.println (F ("Ընդհատման հայտնաբերման միացում (Arduino արտաքին ընդհատում 0) …")); attachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // սահմանեք մեր DMP Ready դրոշը, որպեսզի հիմնական loop () գործառույթը իմանա, որ դա նորմալ է օգտագործել այն Serial.println (F ("DMP պատրաստ է: Սպասում ենք առաջին ընդհատմանը …")); dmpReady = ճշմարիտ; // ստանալ սպասվող DMP փաթեթի չափը հետագայում համեմատվող packetSize = mpu.dmpGetFIFOPacketSize (); // setup PID pid. SetMode (AUTOMATIC); pid. SetSampleTime (10); pid. SetOutputLimits (-255, 255); } այլ {// ՍԽԱԼ! // 1 = հիշողության սկզբնական բեռը ձախողվեց // 2 = DMP կոնֆիգուրացիայի թարմացումները ձախողվեցին // (եթե այն կոտրվի, սովորաբար կոդը կլինի 1) Serial.print (F ("DMP Initialization failed (code")); Serial. տպել (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) {// սպասեք տվյալների հասանելի ճիշտ երկարությանը, պետք է լինի շատ կարճ սպասելիս (հասանելի է FiveoCount 1 փաթեթ // (սա թույլ է տալիս մեզ անմիջապես կարդալ ավելին ՝ առանց սպասելու ընդհատման) fifoCount -= packetSize; տպել ("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; եթե (PWM> = 0) {// առաջի ուղղություն L2 = 0; L1 = abs (PWM); R2 = 0; R1 = abs (PWM); եթե (L1> = 255) { L1 = R1 = 255;}} այլ {// հետընթաց L1 = 0; L2 = abs (PWM); R1 = 0; R2 = abs (PWM); եթե (L2> = 255) {L2 = R2 = 255; }} // շարժիչ ledcWrite (0, L1); ledcWrite (1, L2); ledcWrite (2, R1*0.97); // 0.97 արագության փաստ է կամ, քանի որ աջ շարժիչն ունի ավելի մեծ արագություն, քան ձախ շարժիչը, այնպես որ մենք այն նվազեցնում ենք մինչև շարժիչի արագությունները հավասար լինեն ledcWrite (3, R2*0.97);

}