"9 graden van vrijheid" IMU (10 / 12 stap)

Stap 10: Praat met gyroscoop met I2C

De volgende code moet uitprinten van de X-, Y- en Z rauwe gyro gegevens naar de seriële Monitor. De uitvoer voor mijn board is hierboven weergegeven.

#include < Wire.h > #define GYRO_ADDRESS ((int) 0x68) / / 0x68 = 0xD0 / 2 #define OUTPUT__BAUD_RATE 57600 #define STATUS_LED_PIN 13 / / pincode van status-LED / / Arduino achterwaartse compatibiliteit macro's #if ARDUINO > = 100 #define WIRE_SEND(b) Wire.write((byte) b) #define WIRE_RECEIVE() Wire.read() #else #define Wire.receive() #endif WIRE_RECEIVE() WIRE_SEND(b) Wire.send(b) #define float gyro [3]; int num_gyro_errors = 0; Boole output_errors = false; void setup {Wire.begin(); / / i2c bus (adres facultatief voor master) join Serial.begin(OUTPUT__BAUD_RATE); / / start seriële voor uitvoer / / Init status-LED pinMode (STATUS_LED_PIN, OUTPUT); digitalWrite (STATUS_LED_PIN, laag); / / Init sensoren delay(50); / / sensoren geven genoeg tijd om te beginnen Gyro_Init(); / / Lees sensoren, init DCM algoritme vertraging(20); / / sensoren genoeg tijd te geven om gegevens te verzamelen} void Gyro_Init() {/ / Power up resetten standaard Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x3E); WIRE_SEND(0x80); Wire.endTransmission(); delay(5); Selecteer volledige bereik van de sensoren gyro / / Set LP filter bandbreedte tot 42 Hz Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x16); WIRE_SEND(0x1B); DLPF_CFG = 3, FS_SEL = 3 Wire.endTransmission(); delay(5); Monster rato ingesteld op 50 Hz Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x15); WIRE_SEND(0x0A); SMPLRT_DIV = 10 (50Hz) Wire.endTransmission(); delay(5); PLL klok instellen met z gyro referentie Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x3E); WIRE_SEND(0x00); Wire.endTransmission(); delay(5); } / / Leest x, y en z gyroscoop registreert nietig Read_Gyro() {int i = 0; byte buff [6]; #include <Wire.h> #define GYRO_ADDRESS ((int) 0x68) // 0x68 = 0xD0 / 2 #define OUTPUT__BAUD_RATE 57600 #define STATUS_LED_PIN 13 // Pin number of status LED // Arduino backward compatibility macros #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; boolean output_errors = false; void setup() { Wire.begin(); // join i2c bus (address optional for master) Serial.begin(OUTPUT__BAUD_RATE); // start serial for output // Init status LED pinMode (STATUS_LED_PIN, OUTPUT); digitalWrite(STATUS_LED_PIN, LOW); // Init sensors delay(50); // Give sensors enough time to start Gyro_Init(); // Read sensors, init DCM algorithm delay(20); // Give sensors enough time to collect data } void Gyro_Init() { // Power up reset defaults Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x3E); WIRE_SEND(0x80); Wire.endTransmission(); delay(5); // Select full-scale range of the gyro sensors // Set LP filter bandwidth to 42Hz Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x16); WIRE_SEND(0x1B); // DLPF_CFG = 3, FS_SEL = 3 Wire.endTransmission(); delay(5); // Set sample rato to 50Hz Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x15); WIRE_SEND(0x0A); // SMPLRT_DIV = 10 (50Hz) Wire.endTransmission(); delay(5); // Set clock to PLL with z gyro reference Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x3E); WIRE_SEND(0x00); Wire.endTransmission(); delay(5); } // Reads x, y and z gyroscope registers void Read_Gyro() { int i = 0; byte buff[6]; Wire.beginTransmission(GYRO_ADDRESS); WIRE_SEND(0x1D); // Sends address to read from Wire.endTransmission(); Wire.beginTransmission(GYRO_ADDRESS); Wire.requestFrom(GYRO_ADDRESS, 6); // Request 6 bytes while(Wire.available()) // ((Wire.available())&&(i<6)) { buff[i] = WIRE_RECEIVE(); // Read one byte i++; } Wire.endTransmission(); if (i == 6) // All bytes received? { gyro[0] = -1 * ((((int) buff[2]) << 8) | buff[3]); // X axis (internal sensor -y axis) gyro[1] = -1 * ((((int) buff[0]) << 8) | buff[1]); // Y axis (internal sensor -x axis) gyro[2] = -1 * ((((int) buff[4]) << 8) | buff[5]); // Z axis (internal sensor -z axis) } else { num_gyro_errors++; if (output_errors) Serial.println("!ERR: reading gyroscope"); } } void loop() { delay(200); Read_Gyro(); Serial.print(gyro[0]); Serial.print(" "); Serial.print(gyro[1]); Serial.print(" "); Serial.println(gyro[2]); }