|
本帖最後由 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(); //
} |
|