"9 grader av frihet" IMU (11 / 12 steg)

Steg 11: Prata med Accelerometer/Magnetometer med I2C

Följande kod skriver rå acceleration och magnetiska data för X, Y, och Z-axeln (tre första siffrorna är X, Y, Z acceleration, senaste tre är X, Y Z magnetfält). utdata från min ombord visas ovanför.

#include < Wire.h >

#define ACC_MAG_ADDRESS ((int) 0x1E) / / 0x1E = 0x3C / 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 acc [3].
float mag [3].
int num_mag_errors = 0;
int num_acc_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
Acc_Mag_Init();

Läs sensorer, init DCM algoritm
Delay(20); Ge sensorer tillräckligt med tid att samla in data
}

void Acc_Mag_Init()
{

som acceleration datahastigheten till 50Hz, aktiverar alla acc axlar
Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND (0x20); //CTRL1 registrera
WIRE_SEND (0X57); //01010111
Wire.endTransmission();
Delay(5);

Ange acceleration skala +/-16G
Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND (0x21); //CTRL2 registrera
WIRE_SEND (0X20); //0010000
Wire.endTransmission();
Delay(5);

magnetiska upplösningen till hög och datahastighet inställd på 50Hz
Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND (0x24); //CTRL5 registrera
WIRE_SEND (0X70); //01110000
Wire.endTransmission();
Delay(5);

Ange magnetiska skala till +/-8 Gauss
Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND (0x25); //CTRL6 registrera
WIRE_SEND (0X40); //01000000
Wire.endTransmission();
Delay(5);

Ställ in magnetometer att kontinuerligt läge
Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND (0x26); //CTRL7 registrera
WIRE_SEND(0x00); / /
Wire.endTransmission();
Delay(5);

}

Läsningar x, y och z acc/mag register
void Read_Mag()
{
int jag = 0;
byte buff [6].

Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND(0X08 | 0X80); Skickar adressen att läsa från
Wire.endTransmission();

Wire.beginTransmission(ACC_MAG_ADDRESS);
Wire.requestFrom (ACC_MAG_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?
{
mag [0] = -1 * (((int) buff[1]) << 8) | buff[0]); X-axeln
mag [1] = -1 * (((int) buff[3]) << 8) | buff[2]); Y-axeln
mag [2] = -1 * (((int) buff[5]) << 8) | buff[4]); Z-axeln
}
annat
{
num_mag_errors ++;
om (output_errors) Serial.println("! ERR: läsa acc/mag ");
}
}

void Read_Acc() {
int jag = 0;
byte buff [6].

Wire.beginTransmission(ACC_MAG_ADDRESS);
WIRE_SEND(0X28 | 0X80); Skickar adressen att läsa från
Wire.endTransmission();

Wire.beginTransmission(ACC_MAG_ADDRESS);
Wire.requestFrom (ACC_MAG_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?
{
ACC [0] = -1 * (((int) buff[1]) << 8) | buff[0]); X-axeln
ACC [1] = -1 * (((int) buff[3]) << 8) | buff[2]); Y-axeln
ACC [2] = -1 * (((int) buff[5]) << 8) | buff[4]); Z-axeln
}
annat
{
num_acc_errors ++;
om (output_errors) Serial.println("! ERR: läsa 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]);
}