0

使っているマイコンは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);
}
8

0

この質問に回答するには、ログインする必要があります。

のタグが付いた他の質問を参照する。