Go Down

Topic: Quadcopter Stabilizing (I2C Gyro Acc) (Read 3575 times) previous topic - next topic

Benjamin_Lovelady

Jul 06, 2013, 07:14 am Last Edit: Jul 07, 2013, 11:34 pm by Benjamin_Lovelady Reason: 1
I am using an I2C 10DOF IMU from ebay. (adxl345 and L3G4200d are what I am using on it) I have successfully used the accelerometer to get angles by mapping the values to -90, 90. However, I am having issues with the Gyroscope (L3G4200d). I based the code off the one that worked for the ADXL345. I found an example code that gave some values to send to initialize it, that I couldn't find in the data sheet. I found the multiplier for the numbers and used it to get degrees/second. However, the x and y values come out the same.

I also tried to change it to an angle, but I can't get that to work either. (The previous problem is more important)

Code: [Select]
#include <Wire.h>

#define GYRO (0x69)
#define TO_READ (4)


byte buff_1[4]; //buffer array to recieve data from device


float MDPS = 0.00875; //datasheet said this was the multiplier

void setup()
{
 Serial.begin(9600);
 
 Wire.begin();
 
 //setup Gyro (L3G4200)
 writeTo(GYRO, 0x20, 0b00001111); //couldn't find this in data
 //sheet, but it was in an example code
 writeTo(GYRO, 0x21, 0b00000000);
 writeTo(GYRO, 0x22, 0b00001000);
 writeTo(GYRO, 0x23, 0b00000000);  
}


void loop()
{
  //Gyro values
 
 int x_Gyro, y_Gyro;  
 
 //I send it a buffer to give back with data
 readFrom(GYRO, 0b1101000, TO_READ, buff_1);  
 
 //bitwise operations to shift MSB over 8 and tack on LSB
 x_Gyro = ((((int)buff_1[1]) << 8) | buff_1[0]) * MDPS;

 y_Gyro = ((((int)buff_1[3]) << 8) | buff_1[2]) * MDPS;
   
 Serial.print(" Gyro: x = ");
 Serial.print(x_Gyro);
 Serial.print(" y = ");
 Serial.print(y_Gyro);
 Serial.write(10);
 //delay(200);
}

//basic I2C write function
void writeTo(int device, byte address, byte val)
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.write(val);
 Wire.endTransmission();
}

//this takes the device address, the info address, the number of
//bytes you want, and a buff array (to send the data back with)
// and gives back a buff array
void readFrom(int device, byte address, int num, byte buff[])
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.endTransmission();
 Wire.beginTransmission(device);
 Wire.requestFrom(device, num);
 
 int i = 0;
 while(Wire.available())
 {
   buff[i] = Wire.read();
   i++;
 }
 
 Wire.endTransmission();
}


Moderator edit: quote tags swapped for code tags. AWOL

Benjamin_Lovelady

#1
Jul 06, 2013, 09:05 pm Last Edit: Jul 07, 2013, 11:34 pm by Benjamin_Lovelady Reason: 1
UPDATE:

I was able to get the correct values by grabbing the MSB and LSB separately instead of in a buff array. I don't know why it worked.

I tried to make a simple integration, but the values skyrocket immediately.
Code: [Select]
#include <Wire.h>

#define GYRO (0x69)
#define TO_READ (4)


byte buff_1[4]; //buffer array to recieve data from device


float MDPS = 0.00875; //datasheet said this was the multiplier
float GyroAngleX = 0.0;
float GyroAngleY = 0.0;
float DT = 0.01;
float xFix, yFix;

void setup()
{
 Serial.begin(9600);
 
 Wire.begin();
 
 //setup Gyro (L3G4200)
 writeTo(GYRO, 0x20, 0b00001111); //couldn't find this in data
 //sheet, but it was in an example code
 writeTo(GYRO, 0x21, 0b00000000);
 writeTo(GYRO, 0x22, 0b00001000);
 writeTo(GYRO, 0x23, 0b00000000);
 
 /*
 byte xMSB = readFrom(GYRO, 0x29, 1);
 byte xLSB = readFrom(GYRO, 0x28, 1);
 xFix = ((xMSB << 8) | xLSB) * MDPS;

 byte yMSB = readFrom(GYRO, 0x2B, 1);
 byte yLSB = readFrom(GYRO, 0x2A, 1);
 yFix = ((yMSB << 8) | yLSB) * MDPS;
 
 Serial.print(xFix);
 Serial.println(yFix);
 Serial.println(" ");
  */
 
}


void loop()
{
 
 
 //Gyro values
 
 float x, y;
 
 //time between loops
 DT = micros();
 DT = DT / 1000000; //change to seconds
 
 
 byte xMSB = readFrom(GYRO, 0x29, 1);
 byte xLSB = readFrom(GYRO, 0x28, 1);
 x = ((xMSB << 8) | xLSB) * MDPS; //shift MSB over 8 bits and
 //tack on the LSB, then muliply it by MDPS to get
 // degrees per second

 byte yMSB = readFrom(GYRO, 0x2B, 1);
 byte yLSB = readFrom(GYRO, 0x2A, 1);
 y = ((yMSB << 8) | yLSB) * MDPS;
 
 
 
 //degrees = previous degree plus degrees-per-second times seconds
 GyroAngleX = (GyroAngleX + (x) * DT);
 
 GyroAngleY = (GyroAngleY + (y) * DT);
 
 //x = GyroAngleX;
 //y = GyroAngleY;
 
 
 
 Serial.print(" Gyro: x = ");
 Serial.print(GyroAngleX);
 Serial.print(" y = ");
 Serial.print(GyroAngleY);
 Serial.write(10);
 delay(1);
}



//basic I2C write function
void writeTo(int device, byte address, byte val)
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.write(val);
 Wire.endTransmission();
}


//this takes the device address, the info address, and 1
//, returns a byte
int readFrom(int device, byte address, int num)
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.endTransmission();
 Wire.beginTransmission(device);
 Wire.requestFrom(device, num);
 
 int val;
 val = Wire.read();
 return val;
 
 Wire.endTransmission();
}


Moderator edit: Again, quote tags swapped for code tags.

Benjamin_Lovelady

Update:

I think I figured it out. I went ahead and wrote a new code using functions to try to make it neater. I combined the Gyro and Accelerometer readings to make angles using a basic complementary filter. It doesn't quite zero out, I may try to do a better calibration method, or possibly try to figure out some type of Kalman filter.

@Moderator, sorry about using quote tags before, this time I will use code tags.

Code: [Select]
#include <Wire.h>

#define GYRO (0x69) //gyro address
#define ACC (0x53) //acclerometer address

int xAcc, yAcc; //raw data from sensor
int xAccFix = 0, yAccFix = 0; //calibration data

float xAccAngle, yAccAngle; //angle from accelerometer data

int xGyro, yGyro; //raw data from gyro
int xGyroFix, yGyroFix;

float MDPS = 0.0000875; //change raw to degrees per second
float xGyroAngle, yGyroAngle;
float dt = 0.01; //time between loops

float xAngle = 0.0, yAngle = 0.0;

void setup()
{
  Serial.begin(9600);
  Wire.begin();
 
  AccSetup(); //send setup values to accelerometer
  GyroSetup(); //same for gyro
 
  AccFix(); //calibration values
  GyroFix();
 
   
}


void loop()
{
 
  dt = micros() / 1000000; //time for loop
 
 
  AccGetVals(); //gets values
 
  xAcc = xAcc - xAccFix; //uses calibration values
  yAcc = yAcc - yAccFix;
 
  xAccAngle = map(xAcc, -256, 256, -90, 90); //changes to angle
  yAccAngle = map(yAcc, -256, 256, -90, 90);
   
 
  GyroGetVals();
 
 
  //"integrates" the angular velocities
  xGyroAngle = (xAngle + (xGyro - xGyroFix) * dt);
  yGyroAngle = (yAngle + (yGyro - yGyroFix) * dt);
 
  //combines it with the accelerometer angle
  xAngle = (0.8) * xGyroAngle + (0.2) * xAccAngle;
  yAngle = (0.8) * yGyroAngle + (0.2) * yAccAngle;
 
 
  Serial.print(xAngle);
  Serial.print("     ");
  Serial.print(yAngle);
  Serial.print("\n"); //newline
 
 
  /*
  Serial.print(xAccAngle);
  Serial.print("   ");
  Serial.print(xGyroAngle);
  Serial.print("       ");
  Serial.print(yAccAngle);
  Serial.print("   ");
  Serial.print(yGyroAngle);
  Serial.print("\n");
    */
}


void AccSetup()
{
  i2cWrite(ACC, 0x2D, 0);
  i2cWrite(ACC, 0x2D, 16);
  i2cWrite(ACC, 0x2D, 8);
}

void GyroSetup()
{
  i2cWrite(GYRO, 0x20, 0b00001111);
  i2cWrite(GYRO, 0x21, 0b00000000);
  i2cWrite(GYRO, 0x22, 0b00001000);
  i2cWrite(GYRO, 0x23, 0b00000000);
}

void AccFix()
{
   
  int i = 1;
  while (i <= 3)
  {
    AccGetVals();
    xAccFix = xAccFix + xAcc;
    yAccFix = yAccFix + yAcc;
    i++;
  }
 
  xAccFix = xAccFix / 3;
  yAccFix = yAccFix / 3;
 
 
  Serial.print(xAccFix);
  Serial.print("    ");
  Serial.println(yAccFix);
 
}

void GyroFix()
{
 
  xGyroFix = 0;
  yGyroFix = 0;
 
  int i = 1;
  while (i <= 3)
  {
    GyroGetVals();
    xGyroFix = xGyroFix + xGyro;
    yGyroFix = yGyroFix + yGyro;
    i++;
  }
 
  xGyroFix = xGyroFix / 3;
  yGyroFix = yGyroFix / 3;
 
}

void AccGetVals()
{
  byte MSB = i2cread(ACC, 0x33);
  byte LSB = i2cread(ACC, 0x32);
  xAcc = ((MSB << 8) | LSB);
 
  MSB = i2cread(ACC, 0x35);
  LSB = i2cread(ACC, 0x34);
  yAcc = ((MSB << 8) | LSB);
 
}

void GyroGetVals()
{
  byte MSB = i2cread(GYRO, 0x29);
  byte LSB = i2cread(GYRO, 0x28);
  xGyro = ((MSB << 8) | LSB) * MDPS;
 
  MSB = i2cread(GYRO, 0x2B);
  LSB = i2cread(GYRO, 0x2A);
  yGyro = ((MSB << 8) | LSB) * MDPS;
 
}

 
 

void i2cWrite(int device, byte address, byte value)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(value);
  Wire.endTransmission();
}


int i2cread (int device, byte address)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();
  Wire.beginTransmission(device);
  Wire.requestFrom(device, 1);
 
  int value = Wire.read();
 
  return value;
 
  Wire.endTransmission();
}

Benjamin_Lovelady

#3
Sep 23, 2013, 12:36 am Last Edit: Sep 23, 2013, 12:38 am by Benjamin_Lovelady Reason: 1
I finally made some progress on both the code and the quad. It does not fly yet, but hopefully that is just because I don't have enough propellers (one was destroyed). I was able to change the code around until it would stabilize reasonably well on one axis, so my hope is when I get the new propellers I will only have to do minor tweaks.

For those interested, here is the code (I haven't commented it, so if you have questions about what a certain part is for, just ask):

Code: [Select]
#include <Wire.h>
#include <Servo.h>

#define GYRO (0x69)
#define ACC (0x53)

int currMicros, pastMicros, count = 0;

int xAcc, yAcc;
int xAccFix = 0, yAccFix = 0;


float xAccAngle, yAccAngle;

int xGyro, yGyro;
int xGyroFix, yGyroFix;

float MDPS = 0.0000875;
float xGyroAngle, yGyroAngle;
float dt = 0.01;

Servo myservo, myservo1, myservo2, myservo3;

float xFix = 0, yFix = 0;
float xAngle = 0.0, yAngle = 0.0;

int signalAngleX = 0, signalAngleY = 0;
int throttle = 0;
float xVal_1, xVal_2, yVal_1, yVal_2;

int shutdownCount;


void setup()
{
 Serial3.begin(9600);
 Wire.begin();
 
 rotorStartup();
 
 
 
 AccSetup();
 GyroSetup();
 
 AccFix();
 GyroFix();
 
}

void loop()
{
 
 pastMicros = currMicros;
 currMicros = micros();
 
 dt = (float) (currMicros - pastMicros) / 1000000;
 
 getSignal();
   
 getAngles();

   if (xAngle < (signalAngleX - 1.5) )
 {
   
   if (xFix < 0)
   {      
     xFix = 0;
   }
   else if (xAngle < (signalAngleX - 5) )
   {
     if (xAngle < (signalAngleX - 10) )
     {
       if (xAngle < (signalAngleX - 15) )
       {
         xFix = 5;
       }
       else xFix = xFix + 1.25;
     }
     else xFix = xFix + 0.25;
   }
   else xFix = xFix + 0.0625;
 }
 
 
 else if (xAngle > (signalAngleX + 1.5) )
 {
     
   if (xFix > 0)
   {      
     xFix = 0;
   }
   else if (xAngle > (signalAngleX + 5) )
   {
     if (xAngle > (signalAngleX + 10) )
     {
       if (xAngle > (signalAngleX + 15) )
       {
         xFix = -5;
       }
       else xFix = xFix - 1.25;
     }
     else xFix = xFix - 0.25;
   }
   else xFix = xFix - 0.0625;
 }
 
 else xFix = 0;
 
 
 
 if (yAngle < (signalAngleY - 1.5) )
 {
   
   if (yFix < 0)
   {      
     yFix = 0;
   }
   else if (yAngle < (signalAngleY - 5) )
   {
     if (yAngle < (signalAngleY - 10) )
     {
       if (yAngle < (signalAngleY - 15) )
       {
         yFix = 5;
       }
       else yFix = yFix + 1.25;
     }
     else yFix = yFix + 0.25;
   }
   else yFix = yFix + 0.0625;
 }
 
 
 else if (yAngle > (signalAngleY + 1.5) )
 {
     
   if (yFix > 0)
   {      
     yFix = 0;
   }
   else if (yAngle > (signalAngleY + 5) )
   {
     if (yAngle > (signalAngleY + 10) )
     {
       if (yAngle > (signalAngleY + 15) )
       {
         yFix = -5;
       }
       else yFix = yFix - 1.25;
     }
     else yFix = yFix - 0.25;
   }
   else yFix = yFix - 0.0625;
 }
 
 else yFix = 0;
 
 count = count + 1;
 if (count >= 25)
 {
   yFix = 0;
   xFix = 0;
   
   count = 0;
 }
 
 if ((yFix > 5) or (yFix < -5))
 {
   yFix = 0;
 }
 
 if ((xFix > 5) or (xFix < -5))
 {
   xFix = 0;
 }

 
   xVal_1 = throttle + xFix;
   xVal_2 = throttle - xFix;
     
   yVal_1 = throttle + yFix;
   yVal_2 = throttle - yFix;
   
 myservo.write(xVal_1);
 myservo1.write(xVal_2);
 myservo2.write(yVal_1);
 myservo3.write(yVal_2);

}



void getSignal()
{
 int x, y;
 
 if (Serial3.available())
 {
   throttle =   Serial3.read();

   x = constrain(Serial3.read(), 0, 30);
   signalAngleX = map(x, 0, 30, -15, 15);

   y = constrain(Serial3.read(), 0, 30);

   signalAngleY = map(y, 0, 30, -15, 15);
   
 }
 
 else if (throttle > 50)
 {
   shutdownCount = shutdownCount + 1;
   if (shutdownCount > 25)
   {
     throttle = throttle - 1;
     shutdownCount = 0;
   }
 }
 
}

void getAngles()
{
 AccGetVals();
 
 xAcc = xAcc - xAccFix;
 yAcc = yAcc - yAccFix;
 
 xAccAngle = map(xAcc, -256, 256, -90, 90);
 yAccAngle = map(yAcc, -256, 256, -90, 90);
   
 GyroGetVals();
 
 xGyroAngle = (xAngle + (xGyro - xGyroFix) * dt);
 yGyroAngle = (yAngle + (yGyro - yGyroFix) * dt);
 
 xAngle = (0.9) * xGyroAngle + (0.1) * xAccAngle;
 yAngle = (0.9) * yGyroAngle + (0.1) * yAccAngle;
 
 
}


void AccSetup()
{
 i2cWrite(ACC, 0x2D, 0);
 i2cWrite(ACC, 0x2D, 16);
 i2cWrite(ACC, 0x2D, 8);
}

void GyroSetup()
{
 i2cWrite(GYRO, 0x20, 0b00001111);
 i2cWrite(GYRO, 0x21, 0b00000000);
 i2cWrite(GYRO, 0x22, 0b00001000);
 i2cWrite(GYRO, 0x23, 0b00000000);
}

void AccFix()
{
   
 int i = 1;
 while (i <= 3)
 {
   AccGetVals();
   xAccFix = xAccFix + xAcc;
   yAccFix = yAccFix + yAcc;
   i++;
 }
 
 xAccFix = xAccFix / 3;
 yAccFix = yAccFix / 3;
 
 
}

void GyroFix()
{
 
 xGyroFix = 0;
 yGyroFix = 0;
 
 int i = 1;
 while (i <= 3)
 {
   GyroGetVals();
   xGyroFix = xGyroFix + xGyro;
   yGyroFix = yGyroFix + yGyro;
   i++;
 }
 
 xGyroFix = xGyroFix / 3;
 yGyroFix = yGyroFix / 3;
 
}

void AccGetVals()
{
 byte MSB = i2cread(ACC, 0x33);
 byte LSB = i2cread(ACC, 0x32);
 xAcc = ((MSB << 8) | LSB);
 
 MSB = i2cread(ACC, 0x35);
 LSB = i2cread(ACC, 0x34);
 yAcc = ((MSB << 8) | LSB);
 
}

void GyroGetVals()
{
 byte MSB = i2cread(GYRO, 0x29);
 byte LSB = i2cread(GYRO, 0x28);
 xGyro = ((MSB << 8) | LSB) * MDPS;
 
 MSB = i2cread(GYRO, 0x2B);
 LSB = i2cread(GYRO, 0x2A);
 yGyro = ((MSB << 8) | LSB) * MDPS;
 
}

 
 

void i2cWrite(int device, byte address, byte value)
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.write(value);
 Wire.endTransmission();
}


int i2cread (int device, byte address)
{
 Wire.beginTransmission(device);
 Wire.write(address);
 Wire.endTransmission();
 Wire.beginTransmission(device);
 Wire.requestFrom(device, 1);
 
 int value = Wire.read();
 
 return value;
 
 Wire.endTransmission();
}

void rotorStartup()
{
 myservo.attach(35);
 myservo1.attach(37);
 myservo2.attach(23);
 myservo3.attach(25);
 
 int val = 10;
 myservo.write(val);
 myservo1.write(val);
 myservo2.write(val);
 myservo3.write(val);
 
 delay(1000);
 
 val = 10;
 
 while (val < 65)
 {
   myservo.write(val);
   myservo1.write(val);
   myservo2.write(val);
   myservo3.write(val);
   
   val++;
   delay(50);
 }
 
 
}

silverxxx

My question is why map the accel to -90 , 90, isn't the angle related to cos of gravity vector? Or is it for some other purpose?

Benjamin_Lovelady


My question is why map the accel to -90 , 90, isn't the angle related to cos of gravity vector? Or is it for some other purpose?


Using the map function simplifies it. For angles under ?/6, the trig isn't really necessary (look up small angle approximation). I assume it will never tilt more than 30°, so it shouldn't matter. (I don't plan on doing tricks) I also assume that it is faster for it to do a map function than to do trig.

leleco

you already made ??your first flight with your Quadcopter? Post a video for us!
I want made a quad too, and I follow you for this!
8)

Go Up