Главная » Форумы » Программирование

Код слетает после отключения питания.

Нет ответов
Сб, 21/03/2015 - 12:55
qpaQ
Offline
Зарегистрирован: 21.03.2015

Есть гироскоп MPU-6050 и Arduino Leonardo

Скетч - выдаёт угол наклона гироскопа. Работает хорошо. НО. Проблема такого рода - этот скетч не работает, после отключения и включения обратно питания. Точнее работает, но результат который он выдаёт остаётся не изменным. Как не крути датчик, угол не меняется.
С другими скетчами такой проблемы нету, а вот с этим(где библиотека Калман). Вот такая беда.
И главное не лечится, если просто загрузить этот же код. Лечится только, если загружаешь другой код под гироскоп и потом обратно этот.

Вопрос почему так? И как решить эту проблему.

p.s. другой код не беру, поскольку "библиотека Калман упрощает жизнь".

Сообственно Код:

#include <Wire.h>
#include "Kalman.h"
 
 Kalman kalmanX;
 Kalman kalmanY;
 
 uint8_t IMUAddress = 0x68;
 
 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);
   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); 
   gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
   kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); 
   kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
   timer = micros();
   
   Serial.print ("x=");
   Serial.print (kalAngleX,0);
   Serial.print (" gr   |\n");
   /*Serial.print ("y=");
   Serial.print (kalAngleY,0);
   Serial.print (" gr   |\n");*/
   delay(2000);
 }
 void i2cWrite(uint8_t registerAddress, uint8_t data)
 {
   Wire.beginTransmission(IMUAddress);
   Wire.write(registerAddress);
   Wire.write(data);
   Wire.endTransmission();
 }
 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;
 }
Верх