#include <Wire.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; // Расчет угла с помощью акселерометра
double accYangle;
double temp;
double gyroXangle = 180; // Расчет угла с помощью гироскопа
double gyroYangle = 180;
double compAngleX = 180; // Вычислить угол с помощью фильтра Калмана
double compAngleY = 180;
double kalAngleX; //Вычислить угол с помощью фильтра Калмана
double kalAngleY;
uint32_t timer;
void setup() {
Wire.begin();
Serial.begin(9600);
i2cWrite(0x6B,0x00); // Отключить спящий режим
kalmanX.setAngle(180); // Установить начальный угол
kalmanY.setAngle(180);
timer = micros();
}
void loop() {
/* Обновить все значения */
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]);
/* Расчет углов на основе различных датчиков и алгоритмов */
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(" ");
// Максимальная частота дискретизации акселерометра составляет 1 кГц
}
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); // Не выпускайте шину
Wire.requestFrom(IMUAddress, nbytes); //Отправьте повторный запуск, а затем отпустите шину после чтения
for(uint8_t i = 0; i < nbytes; i++)
data [i]= Wire.read();
return data;
}