我目前使用L3G42000陀螺儀 關於他目前輸出的數據與角度

ben3345678

我是剛接觸的新手 還不是很懂程式 所以請問各位大大
針對這個陀螺儀所附的程式碼 讀取出來的數據 到底是甚麼數據 如何換算成角度 有公式嗎??
#include <Wire.h>

#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23

int Addr = 105;                             // I2C address of the L3G4200D gyro
int x, y, z,a;
int ledPinA=2;
int ledPinB=3;

void setup(){
  pinMode(ledPinA,OUTPUT);
  pinMode(ledPinB,OUTPUT);
  Wire.begin();
  Serial.begin(9600);
  writeI2C(CTRL_REG1, 0x1F);                // Turn on all axes, disable power down
  writeI2C(CTRL_REG3, 0x08);                // Enable control ready signal
  writeI2C(CTRL_REG4, 0x80);                // Set scale (500 deg/sec)
  delay(100);                               // Wait to synchronize
}

void loop(){
  getGyroValues();                         // Get new values
  Serial.print("Raw X:");  Serial.print(x /114);  // Divide by 114 reduces noise
  Serial.print(" Raw Y:"); Serial.print(y /114);
  Serial.print(" Raw Z:"); Serial.println(z /114);
  delay(300);                              // Short delay between reads
  if(x>=20520||y>=20520||z>=20520){
    //a=a+1;
    digitalWrite(ledPinA,HIGH);
    delay(400);
    //if(a>30)
      //digitalWrite(ledPinB,HIGH);
  }
  if(x<=-20520||y<=-20520||z<=-20520){
    //a=a+1;
    digitalWrite(ledPinA,HIGH);
    delay(400);
    //if(a>30)
      //digitalWrite(ledPinB,HIGH);
      
  }
   else{
    digitalWrite(ledPinA,LOW);
    delay(400);
    a=a+1;
    if(a>30){
      digitalWrite(ledPinB,HIGH);
    }
  }

}
void getGyroValues () {
  byte MSB, LSB;

  MSB = readI2C(0x29);
  LSB = readI2C(0x28);
  x = ((MSB << 8) | LSB);

  MSB = readI2C(0x2B);
  LSB = readI2C(0x2A);
  y = ((MSB << 8) | LSB);

  MSB = readI2C(0x2D);
  LSB = readI2C(0x2C);
  z = ((MSB << 8) | LSB);
}
int readI2C (byte regAddr) {
    Wire.beginTransmission(Addr);
    Wire.write(regAddr);                     // Register address to read
    Wire.endTransmission();                 // Terminate request
    Wire.requestFrom(Addr, 1);              // Read a byte
    while(!Wire.available()) { };           // Wait for receipt
    return(Wire.read());                 // Get result
}

void writeI2C (byte regAddr, byte val) {
    Wire.beginTransmission(Addr);           // Begin transmission
    Wire.write(regAddr);                     // Open communications
    Wire.write(val);                         //
    Wire.endTransmission();                 // 　
}

vegewell

回復 1# ben3345678
>>針對這個陀螺儀所附的程式碼 讀取出來的數據 到底是甚麼數據 如何換算成角度 有公式嗎?

如果要換算成具體角度： angle = atan2(y, z) * (180/3.14)

陀螺儀的直接輸出值是相對靈敏軸的角速率，角速率對時間積分即可得到圍繞靈敏軸旋轉過的角度值。由於系統採用微控制器循環採樣程序獲取陀螺儀角速率信息，即每隔一段很短的時間採樣一次，所以採用累加的方法實現積分的功能來計算角度值：

    陀螺儀角度計算：

   式中angle(n)為陀螺儀採樣到第n次的角度值；
   angle(n-1)為陀螺儀第n-1次採樣時的角度值；
   gyron為陀螺儀的第n次採樣得到的瞬時角速率值；
   dt為運行一遍所用時​​間；

      angle_n += gyro(n) * dt
//積分計算

ben3345678

回復 2# vegewell


    如果現在 還有另一個計算的程式 可是 他出來的角度 幾乎都不準確 有其他方法可以解決嗎??
    能夠直接在程式裡面做修正 還是需要其他的硬體教準??
    #include <Wire.h>
#include <L3G4200D.h>
#define EMAoffset 0.05
//#define Gyr_Gain 0.076335877862595  // 1/131,250dps
float angleG;
float G_offset;
float gyroSpeed;
unsigned long timer = 0;


L3G4200D gyro;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  gyro.enableDefault();
  delay(500);
}

void loop() {
  long o_timer = timer;
  timer = millis();
  int dt = timer - o_timer;
  gyro.read();
  G_offset = EMAoffset *(gyro.g.y - 55) / 131 + (1-EMAoffset) * G_offset;
  gyroSpeed = (gyro.g.y - 55) / 131 - G_offset;
  angleG = angleG + gyroSpeed * dt / 1000;
  Serial.print("angle_x: ");
  Serial.print(angleG,6);
  Serial.println(";");   
  delay(500);
}