

使っているマイコンはArduino
nanoです。サンプリング周期を決めて単純に積分しているのですが、どうにもうまくいきません。複雑なプログラムに見えますが、大事なのはここだけです。

[code]

    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);
    
[/code]

そして生じている問題は以下です。  
まずスケッチ全体を実行すると、ang＿xがひたすらに負に増えていきます。なぜならrxが何もしていないのに-1.48~-1.63をウロウロしているからです。故障かなと思い、rxに1.5を足して実行してみますが今度はang_xが90度で2.0や-8.0などいろいろな値を取ります。

コードに間違いがあるのでしょうか

[code]

    #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);
    }
    
[/code]


CC BY-SA 4.0
2021-01-11T15:23:55.097
73223
2021-01-11T15:23:55.097
41334
question
c arduino
ジャイロモジュール MPU-6050で角速度を取得したいがうまくいかない