Гироскоп может быть очень полезен в ваших проектах. Например, его можно использовать как устройство для управления вашими роботами. Сейчас мы рассмотрим один из простых примеров, который Вы с легкостью сможете адаптировать для своих задач.
В этом примере мы научимся управлять двумя серво приводами с помощью акселерометра, когда мы будем отклонять акселерометр Gy-521 (MPU6050) по координате X и Y сервоприводы будут поворачиваться на отклоненный угол.
В данном уроке нам понадобится:
- Arduino
- Провода Папа-Папа или Набор проводов для макетирования 65 шт.
- Провода Папа-Мама
- Breadboard
- 3-осевой гироскоп акселерометр GY-521 (MPU-6050)
- Два любых сервопривода
Для реализации проекта нам необходимо установить библиотеки:
- Библиотека Kalman (Gy-521, mpu6050)
- Библиотека wire
Сборка:
1) Подключаем Акселерометр Gy-521
Gy-521 (mpu6050) | Arduino (Uno) |
---|---|
VCC | 3.3 V |
GND | GND |
SCL | A5 |
SDA | A4 |
Для питания модуля необходимо использовать строго 3.3V! Для этого можно использовать преобразователь напряжения на 3.3V.
2) Сервоприводы подключаем следующим образом:
Arduino (uno) | Servo 1 | Servo 2 |
---|---|---|
5V | Красный (Центральный) | Красный (Центральный) |
GND | Черный или Коричневый (Левый) | Черный или Коричневый (Левый) |
Pin8 - для servo 1 Pin9 - для servo 2 |
Белый или Оранжевый (Правый) | Белый или Оранжевый (Правый) |
Сервопривод рекомендуется питать от внешнего источника питания, если запитать сервопривод от ардуины, то могут возникнуть помехи и перебои в работе arduino. Организовать это можно с помощью источника питания 9V и комбинированного стабилизатора 5V ,3.3V.
Теперь, когда все подключено, приступим к загрузке скетча.
Скетч:
#include <Wire.h> #include "Kalman.h" #include <Servo.h> Servo myservoX; Servo myservoY; Kalman kalmanX; Kalman kalmanY; uint8_t IMUAddress = 0x68; /* IMU Data */ int16_t accX; int16_t accY; int16_t accZ; int16_t tempRaw; int16_t gyroX; int16_t gyroY; int16_t gyroZ; double accXangle; // Angle calculate using the accelerometer double accYangle; double temp; double gyroXangle = 180; // Angle calculate using the gyro double gyroYangle = 180; double compAngleX = 180; // Calculate the angle using a Kalman filter double compAngleY = 180; double kalAngleX; // Calculate the angle using a Kalman filter double kalAngleY; uint32_t timer; void setup() { Wire.begin(); Serial.begin(9600); myservoX.attach(8); myservoY.attach(9); i2cWrite(0x6B,0x00); // Disable sleep mode kalmanX.setAngle(180); // Set starting angle kalmanY.setAngle(180); timer = micros(); } void loop() { /* Update all the values */ uint8_t* data = i2cRead(0x3B,14); accX = ((data[0] << 8) | data[1]); accY = ((data[2] << 8) | data[3]); accZ = ((data[4] << 8) | data[5]); tempRaw = ((data[6] << 8) | data[7]); gyroX = ((data[8] << 8) | data[9]); gyroY = ((data[10] << 8) | data[11]); gyroZ = ((data[12] << 8) | data[13]); /* Calculate the angls based on the different sensors and algorithm */ accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; double gyroXrate = (double)gyroX/131.0; double gyroYrate = -((double)gyroY/131.0); gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000); kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000); timer = micros(); Serial.println(); Serial.print("X:"); Serial.print(kalAngleX,0); Serial.print(" "); Serial.print("Y:"); Serial.print(kalAngleY,0); Serial.println(" "); myservoX.write((int)kalAngleX-90); myservoY.write((int)kalAngleY-90); // The accelerometer's maximum samples rate is 1kHz } void i2cWrite(uint8_t registerAddress, uint8_t data){ Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.write(data); Wire.endTransmission(); // Send stop } uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) { uint8_t data[nbytes]; Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.endTransmission(false); // Don't release the bus Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading for(uint8_t i = 0; i < nbytes; i++) data [i]= Wire.read(); return data; }
Скачать скетч можно по этой ссылке
Видео:
Обсуждение