The gimbal developed, like the pool table seen below, has 2 degrees of freedom (DOF) so that it can rotate about 2 axes. As seen below in Dongil Choi's (KAIST) high speed coffee delivery robot and a cruise ship pool table, if a controller ensures that the resultant 3d acceleration vector is always pointing normal to the platform, a liquid can be transported without spilling and balls can be stabilized without rolling. This was the inspiration to create a basic controller (PID) and perform a bit of IMU data filtering.

Note: If the videos do not load, just refresh the page.

The gimbal has 2DOF - it can rotate about two axes, controlling a platform's orientation by minimizing any acceleration in the two direction perpendicular to the platform's normal vector. This means that the acceleration vector is always pointing down with respect to anything on the platform.

Using a LEGO NXT Mindstorms kit and a Sparkfun IMU via the HiTechnic Proto-board, an acceleration controlled gimbal was developed. The servos on two axes are controlled to minimize acceleration in the X- and Y-directions. The only component of acceleration is therefore pointing down through the Z-axis of the IMU. On a larger scale, and with a more robust control scheme, this type of mechanism could be used to keep a fluid in a cup when unexpected motion occurs. Imagine a waiter holding a tray of drinks and tilting it to compensate as he turns quickly. This is not the same as an angular velocity controller gimbal used for camera stabilization.

Note: If the videos do not load, just refresh the page.

AccelBimbal.nxc

// Program: AccelGimbal.nxc
// Author:  Alex Alspach ([email protected])
// Date:    March 2009
// Status:  Working
 
#include "NXCDefs.h"
 
#define PROTO_PORT IN_1
 
#define   px 0.65
#define   py 0.65
 
#define   dx 0.06
#define   dy 0.06
 
#define   ix 0.01//0.01
#define   iy 0.01//0.01
 
#define   alphaX 0.5
#define   alphaY 0.5
 
int inputdata;
float A0;
float A1;
float A2;
float X;
float Y;
float Z;
 
float errorX;
float errorY;
float errorX_old;
float errorY_old;
 
float DerX;
float DerY;
float IntX = 0.0;
float IntY = 0.0;
 
float X_old;
float Y_old;
 
long motorX;
long motorY;
 
float tCalcStart;
float dT = 1.0;
 
int outputdata;
int count;
byte cmndbuf[];                 // buffer for outbound I2C command
byte respbuf[];                 // buffer for inbound I2C response
 
/* protoboard I/O map
   42,43 - A0 input
   44,45 - A1 input
   46,47 - A2 input
   48,49 - A3 input
   4A,4B - A4 input
   4C    - B inputs
   4D    - B outputs
   4E    - B controls
*/
 
void readA0()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x42;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A0=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
 
void readA1()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x44;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A1=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
 
void readA2()
  {
  ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x46;            // to set read address
  count=2;                      // 2 bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  A2=respbuf[0]*4+respbuf[1];              // create input value by reading the high order byte,
                                                  // shift it left 3 bits and add the low order byt to
                                                  //create a full 10 bit value
  }
 
void writedata()
  {
  ArrayInit(cmndbuf, 0, 3);     // set the buffer to hold 3 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x4D;            // to set write address
  cmndbuf[2] = outputdata;      // to set write data
  count=0;                      // no bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
  }
 
task main()
  {
 
  byte b0 = 1;
  byte b1 = 2;
  byte b2 = 4;
  byte b3 = 8;
  byte b4 = 16;
  byte b5 = 32;
 
  SetSensorLowspeed(PROTO_PORT); // set sensor port 1 to low speed serial (I2C)
  Wait(100);
 
  ArrayInit(cmndbuf, 0, 3);     // set the buffer to hold 3 values
  cmndbuf[0] = 0x02;            // set write to channel
  cmndbuf[1] = 0x4E;            // to set write address
  cmndbuf[2] = 0x3F;            // to write 111111
  count=0;                      // no bytes to read
  I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command
  Wait(100);
 
 
 
  while (TRUE)
    {
 
    tCalcStart = CurrentTick();
 
    readA0();                   // read the analog port 0
    readA1();
    readA2();
    ClearScreen();
 
    // low pass filter
    // y[i] = y[i-1] + a * (x[i] - y[i-1])
    X = X_old + alphaX*(A2 - X_old) - 512/2;
    Y = Y_old + alphaY*(A1 - Y_old) - 512/2;
 
    Z = (A0 - 512);
 
    errorX = X;
    errorY = Y;
 
    DerX = (errorX-errorX_old)/dT;
    IntX = IntX + errorX*dT;
 
    DerY = (errorY-errorY_old)/dT;
    IntY = IntY + errorY*dT;
 
    motorX = px*X + dx*DerX + ix*IntX;
    motorY = py*Y + dy*DerY + iy*IntY;
 
    NumOut(20, LCD_LINE1, X);
    NumOut(20, LCD_LINE2, Y);
    NumOut(20, LCD_LINE3, Z);
 
    NumOut(20, LCD_LINE5, dT);
 
    // Limit the power to motor power range -100 to 100
    if (motorY > 100)   motorY = 100;
    if (motorY < -100)  motorY = -100;
 
    // Limit the power to motor power range -100 to 100
    if (motorX > 100)  motorX = 100;
    if (motorX < -100) motorX = -100;
 
 
    OnFwd(OUT_A, motorY);
    OnFwd(OUT_B, motorX);
 
    X_old = X;
    Y_old = Y;
 
    errorX_old = errorX;
    errorY_old = errorY;
 
 
    dT = (CurrentTick() - tCalcStart)/1000;
    //writedata();
 
 
    }
 
 }

accelgimbal.nxc.zip

When wiring the IMU, consider matching the orientation in the photos above. If this is match, less changes will need to be made for thie following code to work with your system. The X-direction acceleraometer (yellow) is wired to analog input A2 and the Y-direction accelerometer (green) is wired to analogu input A1.

A low-pass filter can be extablished by running the raw data through the following function:

y_filtered[i] = y_filtered[i-1] + a * ( (y_raw[i] - offset) - y_filtered[i-1])

where i is the iteration number and a is a value chosen to provide a level of data smoothing.

You can see in the graphs below the effect of this filter on noisy accelerometer data.

Using a LEGO NXT Mindstorms kit and a Sparkfun IMU via the HiTechnic Proto-board, an acceleration controlled gimbal was developed. The servos on two axes are controlled to minimize acceleration in the X- and Y-directions. The only component of acceleration is therefore pointing down through the Z-axis of the IMU. On a larger scale, and with a more robust control scheme, this type of mechanism could be used to keep a fluid in a cup when unexpected motion occurs. Imagine a waiter holding a tray of drinks and tilting it to compensate as he turns quickly. This is not the same as an angular velocity controller gimbal used for camera stabilization.