# DASL Wiki

### Site Tools

acceleration_controlled_gimbal

# Acceleration Controlled Gimbal

## Motivation

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.

## Gimbal

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.

## Video

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

## Code

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
*/

{
ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
cmndbuf = 0x02;            // set write to channel
count=2;                      // 2 bytes to read
I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
A0=respbuf*4+respbuf;              // 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
}

{
ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
cmndbuf = 0x02;            // set write to channel
count=2;                      // 2 bytes to read
I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
A1=respbuf*4+respbuf;              // 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
}

{
ArrayInit(cmndbuf, 0, 2);     // set the buffer to hold 2 values
cmndbuf = 0x02;            // set write to channel
count=2;                      // 2 bytes to read
I2CBytes(PROTO_PORT, cmndbuf, count, respbuf);  // issue I2C write command and read the byte back
A2=respbuf*4+respbuf;              // 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 = 0x02;            // set write to channel
cmndbuf = 0x4D;            // to set write address
cmndbuf = 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
}

{

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 = 0x02;            // set write to channel
cmndbuf = 0x4E;            // to set write address
cmndbuf = 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();

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();

}

}```

### Wiring

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.

## Low-Pass Filtering

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. 