Kev Ntsuas Tus Kheej Tus Neeg Los Ntawm Magicbit: 6 Kauj Ruam

2024 Tus sau: John Day | [email protected]. Kawg hloov kho: 2024-01-30 09:24

Cov ntawv qhia no qhia yuav ua li cas ua tus kheej ntsuas tus neeg hlau siv Magicbit dev board. Peb tab tom siv khawv koob raws li pawg thawj coj loj hauv txoj haujlwm no uas yog raws ESP32. Yog li ntawd ib qho ESP32 pawg thawj coj loj tuaj yeem siv tau hauv txoj haujlwm no.

Khoom siv:

• khawv koob
• Dual H-choj L298 tsav tsheb
• Tawm Txoj Cai (7805)
• Lipo 7.4V 700mah roj teeb
• Inertial Measurement Unit (IMU) (6 deg ntawm kev ywj pheej)
• iav motors 3V-6V DC

Kauj ruam 1: Zaj dab neeg

Hav guys, hnub no hauv qhov kev qhia no peb yuav kawm txog qhov nyuaj me ntsis. Qhov ntawd yog hais txog tus kheej ntsuas cov neeg hlau siv Magicbit nrog Arduino IDE. Yog li cia pib.

Ua ntej tshaj plaws, cia saib dab tsi yog tus kheej ntsuas tus neeg hlau. Tus kheej ntsuas tus kheej yog ob lub log neeg hlau. Qhov tshwj xeeb yog tus neeg hlau tuaj yeem ntsuas nws tus kheej yam tsis siv lwm qhov kev txhawb nqa sab nraud. Thaum lub hwj chim nyob ntawm tus neeg hlau yuav sawv thiab tom qab ntawd nws sib npaug tsis tu ncua los ntawm kev siv kev txav mus los. Yog li tam sim no txhua yam koj muaj qee lub tswv yim ntxhib txog kev ntsuas tus kheej neeg hlau.

Kauj Ruam 2: Txoj Kev Xav thiab Txoj Kev

Yuav kom sib npaug cov neeg hlau, ua ntej peb tau txais cov ntaub ntawv los ntawm qee qhov ntsuas los ntsuas lub kaum ntse ntse neeg hlau mus rau lub dav hlau ntsug. Rau lub hom phiaj ntawd peb siv MPU6050. Tom qab tau txais cov ntaub ntawv los ntawm lub ntsuas cua peb suav qhov qaij mus rau lub dav hlau ntsug. Yog tias neeg hlau ntawm txoj haujlwm ncaj thiab sib luag, tom qab ntawd lub kaum sab xis qaij yog xoom. Yog tsis yog, ces kaum qaij kaum yog qhov zoo lossis tsis zoo. Yog tias neeg hlau qaij rau sab xub ntiag, tom qab ntawd neeg hlau yuav tsum txav mus rau pem hauv ntej. Tsis tas li yog tias tus neeg hlau qaij rau sab nraub qaum ces tus neeg hlau yuav tsum txav mus rau qhov rov qab. Yog hais tias lub kaum sab xis qaij no siab ces qhov lus teb yuav tsum siab. Hloov pauv lub kaum sab xis qaij qis ces cov tshuaj tiv thaiv nrawm yuav tsum qis. Txhawm rau tswj cov txheej txheem no peb siv cov theorem tshwj xeeb hu ua PID. PID yog kev tswj hwm kev tswj hwm uas siv los tswj hwm ntau txoj haujlwm. PID sawv rau 3 txheej txheem.

• P- sib npaug
• Kuv- ib qho
• D-derivative

Txhua qhov system muaj cov tswv yim thiab tso tawm. Ib yam nkaus qhov kev tswj hwm no tseem muaj qee cov tswv yim. Hauv qhov kev tswj hwm no uas yog qhov sib txawv ntawm lub xeev ruaj khov. Peb hu qhov ntawd ua yuam kev. Hauv peb tus neeg hlau, qhov ua yuam kev yog lub kaum sab xis los ntawm lub dav hlau ntsug. Yog tias neeg hlau sib npaug ces lub qaij kaum yog xoom. Yog li qhov yuam kev tus nqi yuav xoom. Yog li cov txiaj ntsig ntawm PID system yog xoom. Cov kab ke no suav nrog peb cov lej sib cais.

Thawj qhov yog ua yuam kev los ntawm cov lej nce. Qhov kev nce no feem ntau hu ua Kp

P = yuam kev*Kp

Qhov thib ob yog tsim qhov tseem ceeb ntawm qhov ua yuam kev hauv lub sijhawm sau thiab sib npaug nws los ntawm qee qhov tau txais. Qhov kev nce no hu ua Ki

Kuv = Integral (yuam kev)*Ki

Ib qho thib peb yog qhov ua txhaum ntawm qhov ua yuam kev hauv lub sijhawm thiab muab nws los ntawm qee qhov nyiaj tau los. Qhov nce no hu ua Kd

D = (d (yuam kev)/dt)*kd

Tom qab ntxiv cov haujlwm saum toj no peb tau txais peb cov txiaj ntsig kawg

OUTPUT = P+I+D

Vim tias P ib feem neeg hlau tuaj yeem tau txais txoj haujlwm ruaj khov uas raws li qhov sib txawv. Kuv ib feem suav thaj tsam ntawm kev ua yuam kev vs lub sijhawm teeb duab. Yog li nws sim kom tus neeg hlau mus rau txoj haujlwm ruaj khov ib txwm raug. Ib feem ntsuas qhov nqes hav hauv lub sijhawm vs teeb meem yuam kev. Yog qhov ua yuam kev nce tus nqi no yog qhov zoo. Yog qhov ua yuam kev tau txo tus nqi no yog qhov tsis zoo. Vim li ntawd, thaum tus neeg hlau txav mus rau txoj haujlwm ruaj khov tom qab ntawd cov tshuaj tiv thaiv kab mob yuav qis dua thiab qhov no yuav pab tshem tawm qhov tsis tseem ceeb. Koj tuaj yeem kawm paub ntau ntxiv txog PID txoj kev xav los ntawm qhov txuas hauv qab no.

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

Qhov tso tawm ntawm PID muaj nuj nqi yog txwv rau 0-255 ntau yam (8 ntsis PWM daws teeb meem) thiab uas yuav pub rau cov motors ua PWM teeb liab.

Kauj Ruam 3: Teeb Kho Vajtse

Tam sim no qhov no yog teeb tsa kho vajtse ib feem. Kev tsim tus neeg hlau yog nyob ntawm koj. Thaum koj tsim tus neeg hlau lub cev koj yuav tsum txiav txim siab sib npaug nws txog ntsug axis uas nyob hauv lub cev muaj zog. Cov roj teeb nyob hauv qab no. Yog li ntawd cov neeg hlau yooj yim kom sib npaug. Hauv peb qhov kev tsim kho peb kho Magicbit pawg thawj coj saib lub cev. Peb tau siv ob lub cav iav 12V. Tab sis koj tuaj yeem siv txhua hom iav iav. uas yog nyob ntawm koj tus neeg hlau qhov ntev.

Thaum peb tham txog Circuit Court nws tau siv los ntawm 7.4V Lipo roj teeb. Magicbit siv 5V rau lub zog. Yog li peb siv 7805 tus tswj hwm los tswj cov roj teeb hluav taws xob rau 5V. Hauv cov ntawv tom qab ntawm Magicbit uas tus tswj hwm tsis xav tau. Vim tias nws muaj zog txog 12V. Peb ncaj qha muab 7.4V rau tus tsav tsheb.

Txuas tag nrho cov khoom siv raws li daim duab hauv qab no.

Kauj ruam 4: Teeb Software

Hauv txoj cai peb siv PID lub tsev qiv ntawv los xam PID cov zis.

Mus rau qhov txuas hauv qab no txhawm rau rub nws.

www.arduinolibraries.info/libraries/pid

Rub tawm qhov tseeb version ntawm nws.

Txhawm rau kom tau txais kev nyeem ntawv zoo dua peb siv DMP lub tsev qiv ntawv. DMP sawv rau cov txheej txheem ua suab. Qhov no yog qhov tsim tshwj xeeb ntawm MPU6050. Cov nti no muaj cov txheej txheem txheej txheem txav. Yog li nws yuav siv sijhawm nyeem thiab txheeb xyuas. Tom qab nws tsim cov lus tso tawm tsis raug rau lub microcontroller (qhov no yog Magicbit (ESP32)). Tab sis muaj ntau txoj haujlwm hauv microcontroller sab los nyeem qhov ntawd thiab suav lub kaum ntse ntse. Yog li kom yooj yim uas peb siv MPU6050 DMP lub tsev qiv ntawv. Rub nws los ntawm goint mus rau qhov txuas hauv qab no.

github.com/ElectronicCats/mpu6050

Txhawm rau teeb tsa lub tsev qiv ntawv, hauv Arduino ntawv qhia zaub mov mus rau cov cuab yeej-> suav nrog tsev qiv ntawv-> ntxiv.zip lub tsev qiv ntawv thiab xaiv cov ntawv qiv ntawv uas koj rub tawm.

Hauv txoj cai koj yuav tsum hloov lub kaum sab xis teeb tsa kom raug. PID qhov tseem ceeb sib txawv los ntawm neeg hlau rau neeg hlau. Yog li hauv kev kho qhov ntawd, ua ntej teeb tsa Ki thiab Kd tus nqi xoom thiab tom qab ntawd nce Kp kom txog thaum koj tau txais cov tshuaj tiv thaiv zoo dua. Ntau Kp ua rau ntau dhau hwv. Tom qab ntawd nce Kd tus nqi. Ua kom nws los ntawm ib txwm muaj tsawg heev. Tus nqi no feem ntau qis dua lwm qhov txiaj ntsig. Tam sim no nce Ki kom txog thaum koj muaj kev ruaj ntseg zoo heev.

Xaiv qhov chaw nres nkoj COM raug thiab ntaus lub hom. upload cov cai. Tam sim no koj tuaj yeem ua si nrog koj tus neeg hlau DIY.

Kauj ruam 5: Schematics

Kauj ruam 6: Code

#suav nrog

#include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #suav nrog "Wire.h" #endif MPU6050 mpu; bool dmpReady = tsis tseeb; // teeb tsa qhov tseeb yog tias DMP pib ua tiav uint8_t mpuIntStatus; // tuav qhov xwm txheej cuam tshuam los ntawm MPU uint8_t devStatus; // rov qab xwm txheej tom qab txhua lub cuab yeej ua haujlwm (0 = ua tiav,! 0 = yuam kev) uint16_t packetSize; // xav tias DMP pob ntawv loj (lub neej ntawd yog 42 bytes) uint16_t fifoCount; // suav ntawm txhua bytes tam sim no hauv FIFO uint8_t fifoBuffer [64]; // FIFO cia tsis Quaternion q; // [w, x, y, z] quaternion ntim VectorFloat lub ntiajteb txawj nqus; // [x, y, z] lub ntiajteb txawj nqus vector ntab ypr [3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector double originalSetpoint = 172.5; setpoint ob npaug = originalSetpoint; ob npaug txavAngleOffset = 0.1; ob chav tawm tswv yim, tso tawm; int moveState = 0; ob npaug Kp = 23; // teeb tsa P thawj zaug ob zaug Kd = 0.8; // tus nqi no feem ntau me me ob npaug Ki = 300; // tus nqi no yuav tsum siab rau kev ruaj ntseg zoo PID pid (& input, & output, & setpoint, Kp, Ki, Kd, DIRECT); // pid initialize int motL1 = 26; // 4 pins for motor drive int motL2 = 2; int motR1 = 27; int motR2 = 4; volatile bool mpuInterrupt = cuav; // qhia seb MPU cuam tshuam tus pin tau ploj mus tsis muaj dab tsi dmpDataReady () {mpuInterrupt = muaj tseeb; } void setup () {ledcSetup (0, 20000, 8); // pwm setup ledcSetup (1, 20000, 8); ledcSetup (2, 20000, 8); ledcSetup (3, 20000, 8); ledcAttachPin (motL1, 0); // pinmode ntawm motors ledcAttachPin (motL2, 1); ledcAttachPin (motR1, 2); ledcAttachPin (motR2, 3); // koom nrog I2C npav (I2Cdev tsev qiv ntawv tsis ua qhov no tau) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin (); Wire.setClock (400000); // 400kHz I2C moos. Tawm tswv yim kab ntawv no yog tias muaj teeb meem sau ua ke #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire:: teeb tsa (400, muaj tseeb); #endif Serial.println (F ("Initializing I2C devices …")); pinMode (14, INPUT); // initialize serial communication // (115200 xaiv vim nws yuav tsum tau rau Teapot Demo tso tawm, tab sis nws yog // tiag tiag rau koj nyob ntawm koj qhov project) Serial.begin (9600); thaum (! Serial); // tos Leonardo suav sau, lwm tus txuas ntxiv tam sim ntawd // pib ntaus ntawv Serial.println (F ("Initializing I2C devices …")); mpu.initialize (); // txheeb xyuas kev sib txuas Serial.println (F ("Kuaj cov cuab yeej sib txuas …")); Serial.println (mpu.testConnection ()? F ("MPU6050 kev sib txuas ua tiav"): F ("MPU6050 kev sib txuas tsis tau tiav")); // thauj khoom thiab teeb tsa DMP Serial.println (F ("Initializing DMP …")); devStatus = mpu.dmpInitialize (); // muab koj tus kheej gyro offsets ntawm no, ntsuas rau min rhiab heev mpu.setXGyroOffset (220); mpu.setYGyroOffset (76); mpu.setZGyroOffset (-85); mpu.setZAccelOffset (1788); // 1688 lub hauv paus tsis raug rau kuv qhov kev xeem nti // xyuas kom nws ua haujlwm (rov qab 0 yog li) yog (devStatus == 0) {// qhib DMP, tam sim no nws tau npaj txhij Serial.println (F ("Pab DMP … ")); mpu.setDMPEnabled (muaj tseeb); // pab kom Arduino cuam tshuam kev txheeb xyuas Serial.println (F ("Pab kom cuam tshuam kev cuam tshuam (Arduino sab nraud cuam tshuam 0) …")); attachInterrupt (14, dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus (); // teeb tsa peb DMP Npaj tus chij kom lub voj tseem ceeb () ua haujlwm paub nws tsis siv nws li Serial.println (F ("DMP npaj! Tos thawj qhov cuam tshuam …")); dmpReady = muaj tseeb; // tau txais kev cia siab DMP pob ntawv loj rau tom qab sib piv packetSize = mpu.dmpGetFIFOPacketSize (); // teeb tsa PID pid. SetMode (AUTOMATIC); pid. SetSampleTime (10); pid. SetOutputLimits (-255, 255); } lwm {{ERROR! // 1 = thawj lub cim xeeb thauj khoom ua tsis tiav // 2 = DMP teeb tsa hloov tshiab tsis ua tiav // (yog tias nws yuav tawg, feem ntau cov cai yuav yog 1) Serial.print (F ("DMP Initialization Failed (code")); Serial. luam (devStatus); Serial.println (F (")")); }} void loop () {// yog tias lub program tsis ua tiav, tsis txhob sim ua dab tsi yog (! dmpReady) rov qab; // tos MPU cuam tshuam lossis pob ntawv ntxiv (s) muaj thaum (! cov zis); } // rov pib cuam tshuam tus chij thiab tau INT_STATUS byte mpuInterrupt = cuav; mpuIntStatus = mpu.getIntStatus (); // tau txais FIFO suav tam sim no fifoCount = mpu.getFIFOCount (); // kos rau cov dej ntws (qhov no yuav tsum tsis txhob tshwm sim tshwj tsis yog tias peb cov cai tsis ua haujlwm zoo) yog ((mpuIntStatus & 0x10) || fifoCount == 1024) {// rov pib dua yog li peb tuaj yeem txuas ntxiv kom huv mpu.resetFIFO (); Serial.println (F ("FIFO txhoj puab heev!")); // txwv tsis pub, kos rau DMP cov ntaub ntawv npaj cuam tshuam (qhov no yuav tshwm sim ntau zaus)} lwm qhov yog (mpuIntStatus & 0x02) {// tos kom muaj cov ntaub ntawv muaj qhov ntev, yuav tsum yog VERY luv tos thaum (fifoCount 1 pob ntawv muaj // (qhov no cia peb nyeem tam sim ntawd yam tsis tau tos kev cuam tshuam) fifoCount -= packetSize; mpu.dmpGetQuaternion (& q, fifoBuffer); mpu.dmpGetGravity (& gravity, & q); mpu.dmpGetYawPitchRoll (ypr, & q, & gravityIN); #UT LOG. print ("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) {ntab L1, L2, R1, R2; if (PWM> = 0) {// forward direction L2 = 0; L1 = abs (PWM); R2 = 0; R1 = abs (PWM); if (L1> = 255) { L1 = R1 = 255;}} else {// backward direction L1 = 0; L2 = abs (PWM); R1 = 0; R2 = abs (PWM); if (L2> = 255) {L2 = R2 = 255; }} // tsav tsav ledcWrite (0, L1); ledcWrite (1, L2); ledcWrite (2, R1*0.97); // 0.97 yog qhov tseeb lossis, vim tias txoj cai lub cev muaj qhov nrawm dua li sab laug lub cav, yog li peb txo nws kom txog thaum lub cev muaj zog nrawm sib npaug ledcWrite (3, R2*0.97);

}