Акселерометр, гироскоп MPU6050
http://arduino.ru/forum/apparatnye-voprosy/giroskop-gy-521-na-osnove-mpu-6050
Библиотека MPU6050.zip
Использование датчика GY-521 - ФОРУМ ДВ Робот dvrobot.ru
http://forumdvrobot.ru/forum/3-41-1
Калибровка http://www.avislab.com/blog/mpu-6050_ru/ Вопросы по гироскопу MPU 6050 http://forum.amperka.ru/threads/Вопросы-по-гироскопу-mpu-6050.1467/ Модуль 3-х осевого гироскопа и акселерометра GY-521 MPU-6050 https://arduino-kit.ru/catalog/id/modul-3-h-osevogo-giroskopa-i-akselerometra-gy-521-mpu-6050 http://www.geekmomprojects.com/mpu-6050-redux-dmp-data-fusion-vs-complementary-filter/ https://diyhacking.com/arduino-mpu-6050-imu-sensor-tutorial/ http://mfurkanbahat.blogspot.ru/2014/11/artificial-horizon-and-compass-using.html z-axe https://lesson.iarduino.ru/page/urok-11-podklyuchenie-giroskopa-gy-521-mpu-6050-k-arduio/ #include const int MPU_addr = 0x68; // I2C address of the MPU-6050 int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ; void setup() { Serial.begin(115200); Wire.begin(); Wire.beginTransmission(MPU_addr); Wire.write(0x6B); // PWR_MGMT_1 register Wire.write(0); // set to zero (wakes up the MPU-6050) Wire.endTransmission(true); } void loop() { Wire.beginTransmission(MPU_addr); Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) Serial.print("AcX = "); Serial.print(AcX); Serial.print(" | AcY = "); Serial.print(AcY); Serial.print(" | AcZ = "); Serial.print(AcZ); Serial.print(" | Tmp = "); Serial.print(Tmp / 340.00 + 36.53); // temperature in degrees C from datasheet Serial.print(" | GyX = "); Serial.print(GyX); Serial.print(" | GyY = "); Serial.print(GyY); Serial.print(" | GyZ = "); Serial.println(GyZ); delay(500); } //work program #include #include "Kalman.h" Kalman kalmanX; Kalman kalmanY; uint8_t IMUAddress = 0x68; /* IMU Data */ int var; 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); i2cWrite(0x6B,0x00); // Disable sleep mode kalmanX.setAngle(180); // Set starting angle kalmanY.setAngle(180); timer = micros(); var=0; } void loop() { while (var<100){ /* 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)/10000); // Calculate gyro angle using the unbiased rate gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/10000); kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/10000); // Calculate the angle using a Kalman filter kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/10000); timer = micros(); var++;} 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(" "); // The accelerometer's maximum samples rate is 1kHz delay (1000);} 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; }