Mindboards

Code: Select all

/*
* Example of calibrating and reading the dIMU Accelerometer.
*
* Matthew Richardson
* http://mattallen37.wordpress.com/
* October 8 2011
*/

#define dIMU_Accel_Addr 0x3A

void dIMU_Accel_Init(byte port, byte G_Mode = 2){
  byte I2Csnd[3];
  I2Csnd[0] = dIMU_Accel_Addr;  // I2C Address of Accelerometer.

  I2Csnd[1] = 0x16;                   // Register address of Mode Control
  I2Csnd[2] = 0x05;                   // 0x05 For 2G
  if(G_Mode == 0x01) I2Csnd[2] = 0x09; // 0x09 For 4G
  if(G_Mode == 0x02) I2Csnd[2] = 0x01; // 0x01 For 8G

  byte I2Crec[1];           // We are looking for a single byte returned.
  byte cnt = 1;             // cnt must be a variable, not a constant
  I2CBytes(port, I2Csnd, cnt, I2Crec);
}

void dIMU_Accel_Read(byte port, float & Accel_X, float & Accel_Y, float & Accel_Z){

  byte I2Csnd[2];              // Sending buffer
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accl.
  I2Csnd[1] = 0x00;            // First Register of the data we're requesting.
  byte cnt = 6;                // Number of bytes to request
  byte I2Crec[6];              // Receiving buffer

  I2CBytes(port, I2Csnd, cnt, I2Crec);    //Send the address and the register, and receive the 6 bytes of data.

  Accel_X = I2Crec[1] * 256 + I2Crec[0];  // Convert the returned array into floating point G values
  Accel_Y = I2Crec[3] * 256 + I2Crec[2];  //                ''
  Accel_Z = I2Crec[5] * 256 + I2Crec[4];  //                ''

  if(Accel_X > 512)Accel_X -= 1023;       // Convert into a signed value
  if(Accel_Y > 512)Accel_Y -= 1023;       //                ''
  if(Accel_Z > 512)Accel_Z -= 1023;       //                ''

  Accel_X /= 64;
  Accel_Y /= 64;
  Accel_Z /= 64;

  return;
}

void dIMU_Accel_Write_Calibration(byte port, int x_offset, int y_offset, int z_offset){

  byte I2Csnd[8];
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accel.
  I2Csnd[1] = 0x10;            // Register of the data we're writing.
  I2Csnd[2] = x_offset;         // The drift value to write for calibration.
  I2Csnd[3] = x_offset>>8;      //                 ''
  I2Csnd[4] = y_offset;         //                 ''
  I2Csnd[5] = y_offset>>8;      //                 ''
  I2Csnd[6] = z_offset;         //                 ''
  I2Csnd[7] = z_offset>>8;      //                 ''

  byte I2Crec[1];              // We are looking for a single byte returned.
  byte cnt = 1;                // cnt must be a variable, not a constant
  I2CBytes(port, I2Csnd, cnt, I2Crec);   //Send the address, register, and 6 bytes of calibration data.
}

void dIMU_Accel_Calibrate(byte port, int gravity_direction = 3){

  dIMU_Accel_Write_Calibration(port, 0, 0, 0);    // Clear all of the calibration registers.

  int x_offset;
  int y_offset;
  int z_offset;

  byte I2Csnd[2];              // Sending buffer
  I2Csnd[0] = dIMU_Accel_Addr; // I2C Address of accel.
  I2Csnd[1] = 0x00;            // First Register of the data we're requesting.
  byte cnt = 6;                // Number of bytes to request
  byte I2Crec[6];              // Receiving buffer

  I2CBytes(port, I2Csnd, cnt, I2Crec);     //Send the address and the register, and receive the 6 bytes of data.

  x_offset = I2Crec[1] * 256 + I2Crec[0];  // Convert the returned array into floating point G values
  y_offset = I2Crec[3] * 256 + I2Crec[2];  //                ''
  z_offset = I2Crec[5] * 256 + I2Crec[4];  //                ''
  if(x_offset > 512)x_offset -= 1023;      // Convert into a signed value
  if(y_offset > 512)y_offset -= 1023;      //                ''
  if(z_offset > 512)z_offset -= 1023;      //                ''

  // We will calibrate depending on the axis that's inline with the gravity field.
  // Positive 1G is 64 decimal, or 0x40 Hex, so we need to subtract that from the proper axis.
  switch(gravity_direction){

   case 1: x_offset -= 0x40;   // Subtract out the 1-g that the x-axis is experiencing.
    break;

   case 2: y_offset -= 0x40;   // Subtract out the 1-g that the y-axis is experiencing.
    break;

   case 3: z_offset -= 0x40;   // Subtract out the 1-g that the z-axis is experiencing.
    break;
  }

  x_offset *= (-2);    // Treated normally here and just zero it out.
  y_offset *= (-2);    // Treated normally here and just zero it out.
  z_offset *= (-2);    // Treated normally here and just zero it out.

  dIMU_Accel_Write_Calibration(port, x_offset, y_offset, z_offset);
}

float Accel_X, Accel_Y, Accel_Z;

task main(){
  SetSensorLowspeed(S1);

  // Initialize the accelerometer. Only needs to be done once per power cycle.
  dIMU_Accel_Init(S1);              // Here we define the G-Mode we're going to use. 0x00 = +/- 2g, 0x01 = +/- 4g, 0x02 = +/- 8g.
  
  // Calibrate the values.
  dIMU_Accel_Calibrate(S1);         // Calibrated z-axis. 1 x-axis, 2 y-axis, 3 z-axis.  Default is z-axis.

  while (true){
      dIMU_Accel_Read(S1, Accel_X, Accel_Y, Accel_Z);      // Get accel data.

      ClearScreen();
      NumOut(0, LCD_LINE1, Accel_X);
      NumOut(0, LCD_LINE2, Accel_Y);
      NumOut(0, LCD_LINE3, Accel_Z);
      
      Wait(50);
  }
}

Code: Select all

void CalibrateDIAccl(const byte port, int iter)
{
  TextOut(0, LCD_LINE1, "Calibrating...");
  Wait(SEC_1);
  SetSensorDIAcclDrift(port, 0, 0, 0);
  int x = 0, y = 0, z = 0;
  VectorType raw;
  repeat(iter)
  {
    ReadSensorDIAcclRaw(S1, raw);
    x += raw.X;
    y += raw.Y;
    z += raw.Z;
    Wait(MS_10);
  }
  x = (0-(x/iter))*2;
  y = (0-(y/iter))*2;
  z = (60-(z/iter))*2;
  NumOut(0, LCD_LINE2, x);
  NumOut(0, LCD_LINE3, y);
  NumOut(0, LCD_LINE4, z);
  Wait(SEC_1);
  SetSensorDIAcclDrift(port, x, y, z);
  TextOut(0, LCD_LINE5, "Completed!");
  Wait(SEC_1);
  ClearScreen();
}

Code: Select all

x = (0-(x/iter))*2;
y = (0-(y/iter))*2;
z = (60-(z/iter))*2;