Go Down

Topic: how to build a self stabilizing platform (motors unable to stable) (Read 1 time) previous topic - next topic

Arduinolearner9

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?  :'( 

Code: [Select]

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

MarkT

Hard to say from your description, but are you seeing a lot of vibration?
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

Southpark

The lines:
       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;

Looks like you're always putting in 7 percent influence from the second terms.... eg. 0.07*roll.

I have seen different implementations... such as:

K = 0.98;
A = K/(K+dt);
compAngleX = A * (compAngleX + gyro_x * dt) + (1-A) * roll;

.....which makes BOTH components dependent on time.

jremington

What is your theory on why the following should work?
Code: [Select]

    angleX = 90 + averageX;
    pitchservo.write(angleX);
    angleY = 90 + averageY ;
    rollservo.write(angleY);
    led();

Go Up