Go Down

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

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

#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.

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

#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?


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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy