Авто дифферент модели подводной лодки на MPU-6050

Данное устройство предназначено для удержания радиоуправляемой модели подводной лодки в горизонтальном положении. В основе лежит гироскоп MPU-6050 и Arduino. Для минимизации размеров платы и энегропотребления вместо Arduino используется плата Digispark (на Attiny85).

Гироскоп MPU-6050 доступен по протоколу I2C. Для полноразмерной Arduino используется библиотека Wire. Мы же воспльзуемся библиотекой TinyWireM. Для установки скачайте zip-архив с сайта TinyWireM.

Данные от гироскопа приходят со значительными шумами. Чтобы сгладить значения с сенсора, используется фильтр Калмана. Для этого скачайте zip-архив из репозитория KalmanFilter.

Подключение Digispark отличается от ардуино. О нюансах подключения можно почитать на сайте Digispark, там же большой раздел по решению проблем.

Подключение

Пины Digispark

• 0 data
• 2 clck
• 4 pwm

Пины MPU-6050:

• cldt
• clck
• vcc
• gnd

Код

#include <TinyWireM.h>   
#include "Kalman.h"

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 kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;

uint32_t timer;

int led = 1;

void setup() {  
  pinMode(led, OUTPUT);

  TinyWireM.begin();
  i2cWrite(0x6B,0x00); // Disable sleep mode  
  if(i2cRead(0x75,1)[0] != 0x68) { // Read "WHO_AM_I" register
    while(1);
  }      
  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);

  // Calculate the angle using a Kalman filter
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();

  digitalWrite(led, HIGH);
  delay(abs(kalAngleX - 180) * 10);
  digitalWrite(led, LOW);

  delay(100); // The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
  TinyWireM.beginTransmission(IMUAddress);
  TinyWireM.send(registerAddress);
  TinyWireM.send(data);
  TinyWireM.endTransmission();
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
  uint8_t data[nbytes];
  TinyWireM.beginTransmission(IMUAddress);
  TinyWireM.send(registerAddress);
  TinyWireM.endTransmission(false);
  TinyWireM.requestFrom(IMUAddress, nbytes);
  for(uint8_t i = 0; i < nbytes; i++) {
      data[i] = TinyWireM.receive();
  }
  return data;
}