1.  
  2. /* IMU Fusion Board - ADXL345 & IMU3000
  3. Example Arduino Sketch to read the Gyro and Accelerometer Data
  4. Written by www.hobbytronics.co.uk
  5. See the latest version at www.hobbytronics.co.uk/arduino-adxl345-imu3000
  6. 08-Apr-2011
  7. */
  8. #define GYRO 0x68 // gyro I2C address
  9. #define REG_GYRO_X 0x1D // IMU-3000 Register address for GYRO_XOUT_H
  10. #define ACCEL 0x53 // Accel I2c Address
  11. #define ADXL345_POWER_CTL 0x2D
  12. byte buffer[12]; // Array to store ADC values
  13. int gyro_x;
  14. int gyro_y;
  15. int gyro_z;
  16. int accel_x;
  17. int accel_y;
  18. int accel_z;
  19. int i;
  20. #include <Wire.h>
  21. float m_inT = 0.0f;
  22. float m_gx = 0.0f;
  23. void setup()
  24. {
  25. Serial.begin(115200);
  26. Wire.begin();
  27. // Set Gyro settings
  28. // Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s
  29. writeTo(GYRO, 0x16, 0x0B);
  30. //set accel register data address
  31. writeTo(GYRO, 0x18, 0x32);
  32. // set accel i2c slave address
  33. writeTo(GYRO, 0x14, ACCEL);
  34. // Set passthrough mode to Accel so we can turn it on
  35. writeTo(GYRO, 0x3D, 0x08);
  36. // set accel power control to 'measure'
  37. writeTo(ACCEL, ADXL345_POWER_CTL, 8);
  38. //cancel pass through to accel, gyro will now read accel for us
  39. writeTo(GYRO, 0x3D, 0x28);
  40. }
  41. // Write a value to address register on device
  42. void writeTo(int device, byte address, byte val) {
  43. Wire.beginTransmission(device); // start transmission to device
  44. Wire.send(address); // send register address
  45. Wire.send(val); // send value to write
  46. Wire.endTransmission(); // end transmission
  47. }
  48. void loop()
  49. {
  50. // Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
  51. // First set the register start address for X on Gyro
  52. Wire.beginTransmission(GYRO);
  53. Wire.send(REG_GYRO_X); //Register Address GYRO_XOUT_H
  54. Wire.endTransmission();
  55. // New read the 12 data bytes
  56. Wire.beginTransmission(GYRO);
  57. Wire.requestFrom(GYRO,12); // Read 12 bytes
  58. i = 0;
  59. while(Wire.available())
  60. {
  61. buffer[i] = Wire.receive();
  62. i++;
  63. }
  64. Wire.endTransmission();
  65. //Combine bytes into integers
  66. // Gyro format is MSB first
  67. gyro_x = buffer[0] << 8 | buffer[1];
  68. gyro_y = buffer[2] << 8 | buffer[3];
  69. gyro_z = buffer[4] << 8 | buffer[5];
  70. // Accel is LSB first. Also because of orientation of chips
  71. // accel y output is in same orientation as gyro x
  72. // and accel x is gyro -y
  73. accel_y = buffer[7] << 8 | buffer[6];
  74. accel_x = buffer[9] << 8 | buffer[8];
  75. accel_z = buffer[11] << 8 | buffer[10];
  76. double norm = sqrt(sq((float)accel_x) + sq((float)accel_y) + sq((float)accel_z));
  77. float accelRoll = -(((degrees( acos( (float)accel_x / norm)) - 90.0f)));
  78. m_gx = m_gx * 0.9f + 0.1f * (float)gyro_x;
  79. // Print out what we have
  80. Serial.print(m_inT ); // echo the number received to screen
  81. Serial.print(",");
  82. Serial.print(accelRoll); // echo the number received to screen
  83. Serial.print(gyro_x); // echo the number received to screen
  84. Serial.print(",");
  85. Serial.print(m_gx); // echo the number received to screen
  86. Serial.print(",");
  87. // I manually set m_gx after one run hardcoded (51 this time)
  88. m_inT += (float)(gyro_x / 65.0f - 51.5f / 65.0f) * 0.040;
  89. // Force accelerometer and gyro sync
  90. if (accelRoll > -1 && accelRoll < 1)
  91. {
  92. m_inT = 0;
  93. }
  94. Serial.print(",");
  95. Serial.print(m_inT / accelRoll); // echo the number received to screen
  96. Serial.println(""); // prints carriage return
  97. delay(40); // wait for a second
  98. }