"9 grader av frihet" IMU (10 / 12 steg)
Steg 10: Prata med gyroskop med I2C
Följande kod bör skriva ut X, Y och Z raw gyro data till seriell bildskärmen. Utgången för min ombord visas ovanför.
#include < Wire.h >
#define GYRO_ADDRESS ((int) 0x68) / / 0x68 = 0xD0 / 2
#define OUTPUT__BAUD_RATE 57600
#define STATUS_LED_PIN 13 / / Pin-kod för status-LED
Arduino bakåtkompatibilitet makron
#if ARDUINO > = 100
#define WIRE_SEND(b) Wire.write((byte) b)
#define WIRE_RECEIVE() Wire.read()
#else
#define WIRE_SEND(b) Wire.send(b)
#define WIRE_RECEIVE() Wire.receive()
#endif
float gyro [3].
int num_gyro_errors = 0;
booleska output_errors = false;
void setup()
{
Wire.BEGIN(); gå med i2c bussen (adress valfritt för master)
Serial.BEGIN(OUTPUT__BAUD_RATE); Starta följetong för utdata
Init status-LED
pinMode (STATUS_LED_PIN, OUTPUT);
digitalWrite (STATUS_LED_PIN, låg);
Init sensorer
Delay(50); Ge sensorer tillräckligt med tid att starta
Gyro_Init();
Läs sensorer, init DCM algoritm
Delay(20); Ge sensorer tillräckligt med tid att samla in data
}
void Gyro_Init()
{
Power upp Återställ standardvärden
Wire.beginTransmission(GYRO_ADDRESS);
WIRE_SEND(0x3E);
WIRE_SEND(0x80);
Wire.endTransmission();
Delay(5);
Välj fullskaligt utbud av gyro-sensorer
Ställa in LP filtret bandbredd till 42Hz
Wire.beginTransmission(GYRO_ADDRESS);
WIRE_SEND(0x16);
WIRE_SEND(0x1B); DLPF_CFG = 3, FS_SEL = 3
Wire.endTransmission();
Delay(5);
Provet rato inställd på 50Hz
Wire.beginTransmission(GYRO_ADDRESS);
WIRE_SEND(0x15);
WIRE_SEND(0x0A); SMPLRT_DIV = 10 (50Hz)
Wire.endTransmission();
Delay(5);
Ställa in klockan till PLL med z gyro referens
Wire.beginTransmission(GYRO_ADDRESS);
WIRE_SEND(0x3E);
WIRE_SEND(0x00);
Wire.endTransmission();
Delay(5);
}
Läsningar x, y och z gyroskop register
void Read_Gyro()
{
int jag = 0;
byte buff [6].
Wire.beginTransmission(GYRO_ADDRESS);
WIRE_SEND(0x1D); Skickar adressen att läsa från
Wire.endTransmission();
Wire.beginTransmission(GYRO_ADDRESS);
Wire.requestFrom (GYRO_ADDRESS, 6); Begäran 6 byte
While(Wire.available()) / / ((Wire.available()) & &(i<6))
{
buff [i] = WIRE_RECEIVE(); Läs en byte
i ++;
}
Wire.endTransmission();
om (jag == 6) / / alla byte fick?
{
gyro [0] = -1 * (((int) buff[2]) << 8) | buff[3]); X-axeln (intern sensor -y axel)
gyro [1] = -1 * (((int) buff[0]) << 8) | buff[1]); Y-axeln (intern sensor - x-axeln)
gyro [2] = -1 * (((int) buff[4]) << 8) | buff[5]); Z-axeln (intern sensor - z-axel)
}
annat
{
num_gyro_errors ++;
om (output_errors) Serial.println("! ERR: läsa gyroskop ");
}
}
void loop() {
Delay(200);
Read_Gyro();
Serial.Print(gyro[0]);
Serial.Print("");
Serial.Print(gyro[1]);
Serial.Print("");
Serial.println(gyro[2]);
}