Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /* IMU Fusion Board - ADXL345 & IMU3000
- Example Arduino Sketch to read the Gyro and Accelerometer Data
- Written by www.hobbytronics.co.uk
- See the latest version at www.hobbytronics.co.uk/arduino-adxl345-imu3000
- 08-Apr-2011
- */
- #define GYRO 0x68 // gyro I2C address
- #define REG_GYRO_X 0x1D // IMU-3000 Register address for GYRO_XOUT_H
- #define ACCEL 0x53 // Accel I2c Address
- #define ADXL345_POWER_CTL 0x2D
- byte buffer[12]; // Array to store ADC values
- int gyro_x;
- int gyro_y;
- int gyro_z;
- int accel_x;
- int accel_y;
- int accel_z;
- int i;
- #include <Wire.h>
- float m_inT = 0.0f;
- float m_gx = 0.0f;
- void setup()
- {
- Serial.begin(115200);
- Wire.begin();
- // Set Gyro settings
- // Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s
- writeTo(GYRO, 0x16, 0x0B);
- //set accel register data address
- writeTo(GYRO, 0x18, 0x32);
- // set accel i2c slave address
- writeTo(GYRO, 0x14, ACCEL);
- // Set passthrough mode to Accel so we can turn it on
- writeTo(GYRO, 0x3D, 0x08);
- // set accel power control to 'measure'
- writeTo(ACCEL, ADXL345_POWER_CTL, 8);
- //cancel pass through to accel, gyro will now read accel for us
- writeTo(GYRO, 0x3D, 0x28);
- }
- // Write a value to address register on device
- void writeTo(int device, byte address, byte val) {
- Wire.beginTransmission(device); // start transmission to device
- Wire.send(address); // send register address
- Wire.send(val); // send value to write
- Wire.endTransmission(); // end transmission
- }
- void loop()
- {
- // Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
- // First set the register start address for X on Gyro
- Wire.beginTransmission(GYRO);
- Wire.send(REG_GYRO_X); //Register Address GYRO_XOUT_H
- Wire.endTransmission();
- // New read the 12 data bytes
- Wire.beginTransmission(GYRO);
- Wire.requestFrom(GYRO,12); // Read 12 bytes
- i = 0;
- while(Wire.available())
- {
- buffer[i] = Wire.receive();
- i++;
- }
- Wire.endTransmission();
- //Combine bytes into integers
- // Gyro format is MSB first
- gyro_x = buffer[0] << 8 | buffer[1];
- gyro_y = buffer[2] << 8 | buffer[3];
- gyro_z = buffer[4] << 8 | buffer[5];
- // Accel is LSB first. Also because of orientation of chips
- // accel y output is in same orientation as gyro x
- // and accel x is gyro -y
- accel_y = buffer[7] << 8 | buffer[6];
- accel_x = buffer[9] << 8 | buffer[8];
- accel_z = buffer[11] << 8 | buffer[10];
- double norm = sqrt(sq((float)accel_x) + sq((float)accel_y) + sq((float)accel_z));
- float accelRoll = -(((degrees( acos( (float)accel_x / norm)) - 90.0f)));
- m_gx = m_gx * 0.9f + 0.1f * (float)gyro_x;
- // Print out what we have
- Serial.print(m_inT ); // echo the number received to screen
- Serial.print(",");
- Serial.print(accelRoll); // echo the number received to screen
- Serial.print(gyro_x); // echo the number received to screen
- Serial.print(",");
- Serial.print(m_gx); // echo the number received to screen
- Serial.print(",");
- // I manually set m_gx after one run hardcoded (51 this time)
- m_inT += (float)(gyro_x / 65.0f - 51.5f / 65.0f) * 0.040;
- // Force accelerometer and gyro sync
- if (accelRoll > -1 && accelRoll < 1)
- {
- m_inT = 0;
- }
- Serial.print(",");
- Serial.print(m_inT / accelRoll); // echo the number received to screen
- Serial.println(""); // prints carriage return
- delay(40); // wait for a second
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement