使っているマイコンはArduino nanoです。サンプリング周期を決めて単純に積分しているのですが、どうにもうまくいきません。複雑なプログラムに見えますが、大事なのはここだけです。
rx= (float)((data[8] <<8 ) | data[9])/GYRO_CONV ;
ry= (float)((data[10] <<8 ) | data[11])/GYRO_CONV;
rz= (float)((data[12] <<8 ) | data[13])/GYRO_CONV;
ang_x += rx*Ts;
ang_y += ry*Ts;
ang_z += rz*Ts;
delay(100);
そして生じている問題は以下です。
まずスケッチ全体を実行すると、ang_xがひたすらに負に増えていきます。なぜならrxが何もしていないのに-1.48~-1.63をウロウロしているからです。故障かなと思い、rxに1.5を足して実行してみますが今度はang_xが90度で2.0や-8.0などいろいろな値を取ります。
コードに間違いがあるのでしょうか
#include <Wire.h>
#define ACC_CONV 1671.8 //acceleration converter
#define GYRO_CONV 131.1 //deg/s converter
volatile uint8_t data[14]; //to accomodate data
const float Ts=0.1; //[s]
volatile float ang_x=0; // ang x data
volatile float ang_y=0; // ang y data
volatile float ang_z=0; //ang z data
volatile float rx=0; //gyro x data
volatile float ry=0; //gyro y data
volatile float rz=0; //gyro z data
//reg_setter
void i2cWriteReg(uint8_t ad, uint8_t reg, volatile uint8_t data ){
Wire.beginTransmission(ad); //starts transmission
Wire.write(reg); //select reg
Wire.write(data); //write data
Wire.endTransmission(); //ends transmission
}
void setup() {
Serial.begin(9600); //Starts serial
Wire.begin(); //Starts I2C
i2cWriteReg(0x68,0x6b,0x00); //Turn on a sensor
i2cWriteReg(0x68,0x1b,0x00); //-250 < deg/s <250
i2cWriteReg(0x68,0x1c,0x00); // -2g < acc <2g
}
void loop() {
Wire.beginTransmission(0x68); //Starts transmission
Wire.write(0x3b); //select data top
Wire.endTransmission(); //ends transmission
Wire.requestFrom(0x68,14); //get data from top to 14byte
uint8_t i =0;
while (Wire.available())
{
data[i++]=Wire.read(); //accomodate data
}
rx= (float)((data[8] <<8 ) | data[9])/GYRO_CONV ;
ry= (float)((data[10] <<8 ) | data[11])/GYRO_CONV;
rz= (float)((data[12] <<8 ) | data[13])/GYRO_CONV;
ang_x += rx*Ts;
ang_y += ry*Ts;
ang_z += rz*Ts;
OUTPUT_SERIAL();
delay(100);
}
void OUTPUT_SERIAL(){
Serial.print("ang_x: ");
Serial.print(ang_x);
Serial.print("\tang_y: ");
Serial.print(ang_y);
Serial.print("\tang_z: ");
Serial.print(ang_z);
Serial.print("\trx: ");
Serial.print(rx);
Serial.print("\try: ");
Serial.print(ry);
Serial.print("\trz: ");
Serial.println(rz);
}