Stap 11: Praat met versnellingsmeter/Magnetometer met I2C
De volgende code wordt afgedrukt ruwe versnelling en magnetische gegevens voor X, Y, en Z-as (eerst drie nummers zijn X, Y, Z versnelling, laatste drie zijn X, Y Z magnetisch veld). de output van mijn bord is hierboven weergegeven.
#include <Wire.h> #define ACC_MAG_ADDRESS ((int) 0x1E) // 0x1E = 0x3C / 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 acc[3]; float mag[3]; int num_mag_errors = 0; int num_acc_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 Acc_Mag_Init(); // Read sensors, init DCM algorithm delay(20); // Give sensors enough time to collect data } void Acc_Mag_Init() { //set acceleration data rate to 50Hz, enable all acc axes Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x20);//CTRL1 register WIRE_SEND(0x57);//01010111 Wire.endTransmission(); delay(5); //set acceleration scale +/-16G Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x21);//CTRL2 register WIRE_SEND(0x20);//0010000 Wire.endTransmission(); delay(5); //set magnetic resolution to high and data rate to 50Hz Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x24);//CTRL5 register WIRE_SEND(0x70);//01110000 Wire.endTransmission(); delay(5); //set magnetic scale to +/-8 Gauss Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x25);//CTRL6 register WIRE_SEND(0x40);//01000000 Wire.endTransmission(); delay(5); //set magnetometer to continuous mode Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x26);//CTRL7 register WIRE_SEND(0x00);// Wire.endTransmission(); delay(5); } // Reads x, y and z acc/mag registers void Read_Mag() { int i = 0; byte buff[6]; Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x08 | 0x80); // Sends address to read from Wire.endTransmission(); Wire.beginTransmission(ACC_MAG_ADDRESS); Wire.requestFrom(ACC_MAG_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? { mag[0] = -1 * ((((int) buff[1]) << 8) | buff[0]); // X axis mag[1] = -1 * ((((int) buff[3]) << 8) | buff[2]); // Y axis mag[2] = -1 * ((((int) buff[5]) << 8) | buff[4]); // Z axis } else { num_mag_errors++; if (output_errors) Serial.println("!ERR: reading acc/mag"); } } void Read_Acc(){ int i = 0; byte buff[6]; Wire.beginTransmission(ACC_MAG_ADDRESS); WIRE_SEND(0x28 | 0x80); // Sends address to read from Wire.endTransmission(); Wire.beginTransmission(ACC_MAG_ADDRESS); Wire.requestFrom(ACC_MAG_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? { acc[0] = -1 * ((((int) buff[1]) << 8) | buff[0]); // X axis acc[1] = -1 * ((((int) buff[3]) << 8) | buff[2]); // Y axis acc[2] = -1 * ((((int) buff[5]) << 8) | buff[4]); // Z axis } else { num_acc_errors++; if (output_errors) Serial.println("!ERR: reading acc/mag"); } } void loop() { delay(100); Read_Mag(); Read_Acc(); Serial.print(acc[0]); Serial.print(" "); Serial.print(acc[1]); Serial.print(" "); Serial.print(acc[2]); Serial.print(" "); Serial.print(mag[0]); Serial.print(" "); Serial.print(mag[1]); Serial.print(" "); Serial.println(mag[2]); }