MPU 6050 Gyro, Ардерино менен акселерометрдин байланышы (Atmega328p): 5 кадам

2024 Автор: John Day | [email protected]. Акыркы өзгөртүү: 2024-01-30 10:36

MPU6050 IMUда бир чипке бириктирилген 3-Ок акселерометр жана 3-Окс гироскопу бар.

Гироскоп айлануу ылдамдыгын же бурчтук абалдын өзгөрүү ылдамдыгын X, Y жана Z огу боюнча өлчөйт.

Гироскоптун чыгышы секундасына градус, ошондуктан бурчтук абалды алуу үчүн биз жөн гана бурчтук ылдамдыкты бириктирүүбүз керек.

Башка жагынан алганда, MPU6050 акселерометр 3 огу боюнча гравитациялык ылдамданууну өлчөө менен ылдамданууну өлчөйт жана кээ бир тригонометрия математикасын колдонуп, сенсор жайгашкан бурчту эсептей алабыз. Ошентип, эгер биз акселерометр менен гироскоптун маалыматтарын бириктирсек же бириктирсек, сенсордун ориентациясы жөнүндө абдан так маалыматты ала алабыз.

MPU-6050 3 октук гироскоптон турат, ал x, y, z огунда микроэлектрикалык механикалык система технологиясы (MEMS) менен айлануу ылдамдыгын аныктай алат. Сенсор кандайдыр бир огу боюнча айланганда, Coriolis эффектинен улам дирилдөө пайда болот, аны MEMS.16-бит ADC ар бир октун үлгүсүндөгү чыңалууну санариптештирүү үчүн колдонот. +/- 250, +/- 500, +/- 1000, +/- 2000- бул өндүрүштүн толук масштабы, бурчтук ылдамдыгы ар бир ог боюнча секунда бирдигинде өлчөнөт.

Пайдалуу шилтеме: …………….

Arduino Board: ……….

MPU6050 IMU ……………

1-кадам: MPU-6050 модулу

MPU-6050 модулунда 8 төөнөгүч бар,

INT: үзгүлтүккө санарип чыгаруу пин.

AD0: I2C кул дареги LSB пин. Бул түзмөктүн 7-бит кул дарегиндеги 0 бит. Эгерде VCCге туташкан болсо, анда ал логикалык жана кулдук даректин өзгөрүшү катары окулат.

XCL: Көмөкчү Serial Clock пин. Бул пин башка I2C интерфейси иштетилген сенсорлор SCL туташуусун MPU-6050 менен туташтыруу үчүн колдонулат.

XDA: Көмөкчү сериялык маалымат пин. Бул пин башка I2C интерфейси иштетилген сенсорлор SDA пинин MPU-6050 менен туташтыруу үчүн колдонулат.

SCL: Сериалдык саат пин. Бул пинди микроконтроллерлерге SCL төөнөгүчкө туташтырыңыз. SDA: Сериялык маалымат пин. Бул төөнөгүчтү микроконтроллер SDA пинине туташтырыңыз.

GND: Жер казыгы. Бул пинди жерге туташууга туташтырыңыз.

VCC: Электр менен камсыздоо пин. Бул төөнөгүчтү +5V туруктуу токко туташтырыңыз. MPU-6050 модулунда Slave дареги бар (AD0 = 0 болгондо, б.а. Vcc менен туташкан эмес), Жазуу дареги (SLA+W): 0xD0

Slave Read дареги (SLA+R): 0xD1

2 -кадам: эсептөөлөр

MPU6050 модулунун гироскопу жана акселерометрдин сенсорунун маалыматтары 2нин толуктоочу формасындагы 16 биттик чийки маалыматтардан турат.

MPU6050 модулунун температура сенсорунун маалыматы 16 биттик маалыматтан турат (2нин толуктоо формасында эмес).

Эми биз тандап алдык дейли,

• - Акселерометрдин толук масштабдуу диапазону +/- 2g 16, 384 LSB (Count)/гр.
• - Гироскоптун толук масштабдуу диапазону +/- 250 °/с, Сезгичтик шкаласы фактору менен 131 LSB (Count)/°/s. анда,

Сенсордук чийки маалыматтарды алуу үчүн, биз биринчи кезекте акселерометр менен гироскоптун сенсордук маалыматтары боюнча 2нин толуктоосун аткарышыбыз керек. Сенсордук чийки маалыматтарды алгандан кийин, сенсордун чийки маалыматтарын алардын сезгичтик шкаласы факторуна бөлүү менен ылдамдатууну жана бурчтук ылдамдыкты эсептей алабыз-

Акселерометрдин мааниси g (g күчү)

• X огу боюнча ылдамдануу = (Акселерометр X огу чийки маалыматтар/16384) ж.
• Y огу боюнча ылдамдануу = (Акселерометр Y огу чийки маалыматтар/16384) ж.
• Z огу боюнча ылдамдануу = (акселерометр Z огунун чийки маалыматы/16384) ж.

Гироскоптун мааниси °/с (секундасына даража)

• X огу боюнча бурчтук ылдамдык = (Гироскоп X огу чийки маалыматтар/131) °/с.
• Y огу боюнча бурчтук ылдамдык = (гироскоп Y огу чийки маалыматтар/131) °/с.
• Z огу боюнча бурчтук ылдамдык = (гироскоп Z огунун чийки маалыматы/131) °/с.

Температуранын мааниси °/с (Цельсий боюнча градус)

Температура градус C = ((температура сенсорунун маалыматы)/340 + 36.53) °/c.

Мисалы, Мисалы, 2’комплементтен кийин биз акселерометр X огунун чийки мааниси = +15454 алабыз дейли

Анда Ax = +15454/16384 = 0.94 гр.

Дагы,

Ошентип, биз +/- 2G жана +/- 250deg/s сезгичтигинде иштеп жатканыбызды билебиз, бирок биздин баалуулуктар бул ылдамдыктарга/бурчтарга кантип туура келет.

Бул экөө тең түз сызыктуу графиктер жана биз алардан иштей алабыз, биз 1G үчүн 16384 окуйбуз жана 1 градус/сек үчүн 131.07 окуйбуз (.07 бинардык себептен этибарга алынбайт), бул баалуулуктар чийүү аркылуу иштелип чыккан. 32767де 2G жана -32768де -2G менен 250/-250 түз сызык графасы бирдей маанилерде.

Ошентип, азыр биз сезимталдык баалуулуктарыбызды билебиз (16384 жана 131.07), биз жөн гана баалуулуктарыбыздын ордун басуу керек, андан кийин сезимталдык менен бөлүшүү керек.

Булар X жана Y баалуулуктары үчүн жакшы иштейт, бирок Z 1Gде жазылгандыктан 0 эмес, биз сезгичтигибизге бөлүүдөн мурун 1G (16384) өчүрүүбүз керек болот.

3-кадам: MPU6050-Atmega328p байланыштары

Баарын диаграммада көрсөтүлгөндөй туташтырыңыз …

Байланыштар төмөнкүчө берилет:-

MPU6050 Arduino Nano

VCC 5V чыккычы

GND Жер казыгы

SDA A4 пин // сериялык маалыматтар

SCL A5 пин // сериялык саат

Pitch and Roll Эсептөөсү: Roll-бул х огунун айланасындагы айлануу жана кадам-y огу боюнча айлануу.

Натыйжа радиандарда. (180ге көбөйтүү жана пи менен бөлүү менен даражаларга которуу)

4 -кадам: Коддор жана түшүндүрмөлөр

/*

Arduino жана MPU6050 акселерометр жана гироскоп сенсорунун үйрөткүчү, Дежан, https://howtomechatronics.com */#include const int MPU = 0x68; // MPU6050 I2C дареги калкыма AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; калкып жүрүүчү түрмөк, чайыр, жаа; сүзүү AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; void setup () {Serial.begin (19200); Wire.begin (); // Wire.beginTransmission (MPU) байланышын баштоо; // MPU6050 менен байланышты баштаңыз // MPU = 0x68 Wire.write (0x6B); // 6B Wire.write (0x00) реестри менен сүйлөшүңүз; // Баштапкы абалга келтирүү - 6B Wire.endTransmission регистрине 0 коюңуз (чыныгы); // берүүнү токтотуу/* // Акселерометрдин сезимталдыгын конфигурациялоо - Толук масштабдуу диапазон (демейки +/- 2g) Wire.beginTransmission (MPU); Wire.write (0x1C); // ACCEL_CONFIG реестри менен сүйлөшүңүз (1C hex) Wire.write (0x10); // Регистр биттерин 00010000 (+/- 8g толук масштабдуу диапазон) катары коюңуз Wire.endTransmission (чыныгы); // Gyro сезимталдыгын конфигурациялоо - Толук масштабдуу диапазон (демейки +/- 250deg/s) Wire.beginTransmission (MPU); Wire.write (0x1B); // GYRO_CONFIG реестри менен сүйлөшүңүз (1B hex) Wire.write (0x10); // Реестрдин биттерин 00010000 (1000deg/s толук масштабдуу) деп коюңуз Wire.endTransmission (true); кечигүү (20); */ // Эгерде сиз модулуңуз үчүн IMU ката баалуулуктарын алышыңыз керек болсо, бул функцияга чалыңыз calc_IMU_error (); кечигүү (20); } void loop () {// === acceleromter data окуу === // Wire.beginTransmission (MPU); Wire.write (0x3B); // 0x3B регистринен баштаңыз (ACCEL_XOUT_H) Wire.endTransmission (жалган); Wire.requestFrom (MPU, 6, чыныгы); // Бардыгы болуп 6 регистрди окуңуз, ар бир огунун мааниси 2 реестрде сакталат // +-2g диапазону үчүн биз AccX = (Wire.read () << 8 маалымат барагына ылайык, чийки баалуулуктарды 16384кө бөлүшүбүз керек. | Wire.read ()) / 16384.0; // X огунун мааниси AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Y огунун мааниси AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Z огунун мааниси // AccAngleX акселерометринин маалыматынан Roll жана Pitchти эсептөө = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (0.58) Көбүрөөк маалымат алуу үчүн calc_IMU_error () бажы функциясын караңыз accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58) // === Гироскоптун маалыматын окуу === // previousTime = currentTime; // Мурунку убакыт иш убактысынан мурун сакталат, currentTime = millis (); // Учурдагы убакыт чыныгы убакыт окулган elapsedTime = (currentTime - previousTime) / 1000; // Секунддарды алуу үчүн 1000ге бөлүңүз Wire.beginTransmission (MPU); Wire.write (0x43); // Gyro маалыматтарынын биринчи регистр дареги 0x43 Wire.endTransmission (жалган); Wire.requestFrom (MPU, 6, чындык); // Жалпы 4 регистрди окуңуз, ар бир огунун мааниси 2 регистрде сакталат GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0; // 250deg/ s диапазону үчүн GyroY = (Wire.read () << 8 | Wire.read ())/ 131.0 маалыматтык баракчасына ылайык, биз алгач чийки маанини 131.0га бөлүшүбүз керек; GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0; // Чыгууларды GyroX = GyroX + 0.56 эсептелген ката баалуулуктары менен оңдоңуз; // GyroErrorX ~ (-0.56) GyroY = GyroY - 2; // GyroErrorY ~ (2) GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8) // Учурда чийки баалуулуктар секундасына градус, сек/с, андыктан бурчту градуска алуу үчүн gondroAngleX = gyroAngleX + gyroX * elapsedTime; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * elapsedTime; yaw = yaw + GyroZ * elapsedTime; // Кошумча чыпка - акселеромтер менен гиро бурчунун маанилерин бириктирүү = 0.96 * gyroAngleX + 0.04 * accAngleX; кадам = 0.96 * gyroAngleY + 0.04 * accAngleY; // Сериялык мониторго баалуулуктарды басып чыгаруу Serial.print (түрмөк); Serial.print ("/"); Serial.print (кадам); Serial.print ("/"); Serial.println (yaw); } void calc_IMU_error () {// Акселерометрди жана гироскоптун катасын эсептөө үчүн бул функцияны орнотуу бөлүмүнө чала алабыз. Бул жерден биз Serial Monitorдо басылган жогорудагы теңдемелерде колдонулган ката маанилерин алабыз. // Белгилей кетчү нерсе, биз туура маанилерди алуу үчүн IMUну тегиз жайгаштырышыбыз керек, ошондо туура баалуулуктарды алабыз // Акселерометрдин маанилерин 200 жолу окуп жатканда (c <200) {Wire.beginTransmission (MPU); Wire.write (0x3B); Wire.endTransmission (жалган); Wire.requestFrom (MPU, 6, чыныгы); AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Бардык окууларды жыйындысы AccErrorX = AccErrorX + ((atan ((AccY) / sqrt (pow ((AccX), 2) + pow ((AccZ), 2)))) * 180 / PI)); AccErrorY = AccErrorY + ((атан (-1 * (AccX) / sqrt (pow ((AccY), 2) + pow ((AccZ), 2))) * 180 / PI)); c ++; } // Сумманы 200гө бөлүү AccErrorX = AccErrorX /200 ката маанисин алуу үчүн; AccErrorY = AccErrorY / 200; c = 0; // gyro маанилерин 200 жолу окуу (c <200) {Wire.beginTransmission (MPU); Wire.write (0x43); Wire.endTransmission (жалган); Wire.requestFrom (MPU, 6, чыныгы); GyroX = Wire.read () << 8 | Wire.read (); GyroY = Wire.read () << 8 | Wire.read (); GyroZ = Wire.read () << 8 | Wire.read (); // Бардык окууларды жыйындысы GyroErrorX = GyroErrorX + (GyroX / 131.0); GyroErrorY = GyroErrorY + (GyroY / 131.0); GyroErrorZ = GyroErrorZ + (GyroZ / 131.0); c ++; } // GyroErrorX = GyroErrorX /200 ката маанисин алуу үчүн сумманы 200гө бөлүңүз. GyroErrorY = GyroErrorY / 200; GyroErrorZ = GyroErrorZ / 200; // Serial Monitor Serial.print ката баалуулуктарын басып чыгаруу ("AccErrorX:"); Serial.println (AccErrorX); Serial.print ("AccErrorY:"); Serial.println (AccErrorY); Serial.print ("GyroErrorX:"); Serial.println (GyroErrorX); Serial.print ("GyroErrorY:"); Serial.println (GyroErrorY); Serial.print ("GyroErrorZ:"); Serial.println (GyroErrorZ); } ------------------------------------------------- ---------------------------------------------- Жыйынтыктар:-X = Y = Z = --------------------------------------------- ----------------------------------------------- Маанилүү эскертүү: -----------------

Укурук бөлүмүндө акселерометрдин маалыматын окуудан баштайбыз. Ар бир октун маалыматы 2 байт же регистрде сакталат жана бул реестрлердин даректерин сенсордун маалымат барагынан көрө алабыз.

Алардын бардыгын окуу үчүн, биз биринчи реестрден баштайбыз жана RequiestFrom () функциясын колдонуп, X, Y жана Z огу үчүн бардык 6 регистрди окууну суранабыз. Андан кийин биз ар бир реестрдеги маалыматтарды окуйбуз жана чыгымдар эки толуктоочу болгондуктан, туура баалуулуктарды алуу үчүн аларды ылайыктуу түрдө бириктиребиз.

5 -кадам: Tilt бурчун түшүнүү

Акселерометр

Жердин тартылуу күчү дайыма Жердин борборун көрсөтүп турган туруктуу ылдамдануу.

Акселерометр тартылуу күчү менен параллель болгондо, өлчөнүүчү ылдамдануу 1G болот, акселерометр тартылуу күчү менен перпендикуляр болгондо 0G өлчөйт.

Каптал бурчу бул теңдеменин жардамы менен өлчөнүүчү ылдамдануудан эсептелинет:

θ = sin-1 (өлчөнүүчү ылдамдануу / тартылуу ылдамдыгы)

GyroGyro (ака ылдамдык сенсору) бурчтук ылдамдыкты (ω) өлчөө үчүн колдонулат.

Роботтун эңкейиш бурчун алуу үчүн, биз төмөндөгү теңдемеде көрсөтүлгөндөй гиродон алынган маалыматтарды интеграциялашыбыз керек:

ω = dθ / dt, θ = ∫ ω dt

Директордун жана акселерометрдин өзгөчөлүктөрүн изилдеп чыккандан кийин, биз алардын күчтүү жана алсыз жактары бар экенин билебиз. Акселерометрдин маалыматынан эсептелген ийилүү бурчу жай жооп берүү убактысына ээ, ал эми гиро маалыматтарынын интегралдык кыйшайуу бурчу белгилүү бир убакыт ичинде дрейфке дуушар болот. Башкача айтканда, акселерометрдин маалыматтары узак мөөнөттүү, гиро маалыматтары кыска мөөнөттүү үчүн пайдалуу деп айта алабыз.

Жакшыраак түшүнүү үчүн шилтеме: Бул жерди басыңыз