hi, im need to build a self stabilizing platform for my project, but im totally new to arduino.
i was given a arduino pro mini and SparkFun IMU Fusion Board - ADXL345 & IMU3000 for this project
when i mount the sensor on the servo motors, it start to swing around unable to stabilize, it can only perform well when i hold the sensor with my hands
anyone know how to solve this problem? 
#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
#define DATA_FORMAT 0x31
#define PI 3.14159
byte buffer[12]; // Array to store ADC values
int raw_gyro_x;
int raw_gyro_y;
int raw_gyro_z;
int raw_accel_x;
int raw_accel_y;
int raw_accel_z;
int i;
int angleX;
int angleY;
float gyro_y;
float gyro_x;
float gyro_z;
float accel_x;
float accel_y;
float accel_z;
float roll;
float pitch;
int compAngleX;
int compAngleY;
const int numReadings =10;
int xreadings[numReadings]; // the readings from the analog input
int xindex = 0; // the index of the current reading
int xtotal = 0; // the running total
int yreadings[numReadings]; // the readings from the analog input
int yindex = 0; // the index of the current reading
int ytotal = 0; // the running total
int averageX;
int averageY;
unsigned long time;
long interval = 10;
long previousMillis = 0;
uint32_t timer;
#include <Wire.h>
#include <math.h>
#include <Servo.h>
Servo pitchservo;
Servo rollservo;
int pos = 0;
void Transmission()
{
// 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.write(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.read();
i++;
}
Wire.endTransmission();
}
void ReadGyro() //Combine bytes into integers
{
// Gyro format is MSB first
raw_gyro_x = buffer[0] << 8 | buffer[1];
raw_gyro_y = buffer[2] << 8 | buffer[3];
raw_gyro_z = buffer[4] << 8 | buffer[5];
// Accel is LSB first.
raw_accel_x = buffer[7] << 8 | buffer[6];
raw_accel_y = buffer[9] << 8 | buffer[8];
raw_accel_z = buffer[11] << 8 | buffer[10];
}
void Readaccel() //Convert the accelerometer value to G's.
{
accel_x = raw_accel_x * 0.0078;
accel_y = raw_accel_y * 0.0078;
accel_z = raw_accel_z * 0.0078;
}
void convertRawData() //Convert raw Gyro data to deg/s
{
//Sensitivity information (from Invensense's website):
// Range (deg/s) | Sensitivity (LSB/deg/s)
// +/-250 | 131
// +/-500 | 65.5
// +/-1000 | 32.8
// +/-2000 | 16.4
//This is where the sensitivity values come from:
//Gyro sensitivity: 16 bit range (65535) at +/- 500 deg/s (total range of 1000)
// 65535 / 1000 = 65.535
gyro_x = raw_gyro_x / 65.5;
gyro_y = raw_gyro_y / 65.5;
gyro_z = raw_gyro_z / 65.5;
}
void RollandPitchEquation() // Roll & Pitch Equations
{
// Roll rotation about X
roll = atan2(accel_y, sqrt( pow(accel_x,2) + pow(accel_z,2) )); // phi
// pitch rotation about Y
// to correct the direction of rotation about Y , add negative sign on ptich
pitch = - atan2(accel_x , sqrt( pow(accel_y,2) + pow(accel_z,2) )); // rho
}
void RadianToDegree() // convert the values from radians to degree
{
pitch = pitch * (180.0 / PI);
roll = roll * (180.0 / PI);
// using complementary filter, see http://www.pieter-jan.com/node/11
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
compAngleX = 0.93 * (compAngleX + gyro_x * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyro_y * dt) + 0.07 * pitch;
timer = micros();
}
void averageValue()
{
xtotal= xtotal - xreadings[xindex]; // read from the sensor:
xreadings[xindex] = compAngleX; // add the reading to the total:
xtotal= xtotal + xreadings[xindex]; // advance to the next position in the array:
xindex = xindex + 1;
if (xindex >= numReadings) // if we're at the end of the array...
xindex = 0; // ...wrap around to the beginning:
averageX = xtotal / numReadings; // calculate the average:
ytotal = ytotal - yreadings[yindex]; // read from the sensor
yreadings[yindex] = compAngleY; // add the reading to the total
ytotal = ytotal + yreadings[yindex]; // advance to the next position in the array
yindex = yindex + 1;
if (yindex >= numReadings)// if we're at the end of the array...
yindex = 0;// ...wrap around to the beginning
averageY = ytotal / numReadings;// calculate the average
}
void setup()
{
Serial.begin(9600);
pitchservo.attach(9);
rollservo.attach(6);
for (int thisReading = 0; thisReading < numReadings; thisReading++)
{
xreadings[thisReading] = 0;
yreadings[thisReading] = 0;
}
Wire.begin(); // Set Gyro settings
writeTo(GYRO, 0x16, 0x0B);
writeTo(GYRO, 0x18, 0x32); //set accel register data address
writeTo(GYRO, 0x14, ACCEL); // set accel i2c slave address
writeTo(GYRO, 0x3D, 0x08); // Set passthrough mode to Accel so we can turn it on
writeTo(ACCEL, ADXL345_POWER_CTL, 8); // set accel power control to 'measure'
writeTo(ACCEL, DATA_FORMAT, 0x01); //Write 0x00 for 2G, 0x01 for 4G, 0x0A for 8G,0x0B for 16G
writeTo(GYRO, 0x3D, 0x28); //cancel pass through to accel, gyro will now read accel for us
}
void writeTo(int device, byte address, byte val)// Write a value to address register on device
{
Wire.beginTransmission(device); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
void loop()
{
angleX = 90 + averageX;
pitchservo.write(angleX);
angleY = 90 + averageY ;
rollservo.write(angleY);
led();
Transmission();
ReadGyro();
Readaccel();
convertRawData();
RollandPitchEquation();
RadianToDegree();
averageValue();
}
}
}