Robofun 機器人論壇

標題: 我目前使用L3G42000陀螺儀關於他目前輸出的數據與角度 [打印本頁]

作者: ben3345678 時間: 2013-7-24 23:12
標題: 我目前使用L3G42000陀螺儀關於他目前輸出的數據與角度
本帖最後由 ben3345678 於 2013-11-12 18:56 編輯

我是剛接觸的新手還不是很懂程式所以請問各位大大
針對這個陀螺儀所附的程式碼讀取出來的數據到底是甚麼數據如何換算成角度有公式嗎??
#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 時間: 2013-7-27 02:43
回復 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 時間: 2013-10-9 19:45
回復 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);
}

歡迎光臨 Robofun 機器人論壇 (https://robofun.net/forum/)