Stabilized Plateform with IMU 6DoF

Dear all,

I developed a small sketch adapted to an IMU 6 DoF connected via I2C to an Arduino card (Duemilnove).

Using just two servos for the X and Y axis, it permit to show the concept of that stabilized plateform.

You can see the video showing the system working here: Stabilized Platform through IMU 6 DoF and the Quaternions Algorithm - YouTube

The code is divided in two parts:

The main code and the “printData” which show over the serial terminal the values of th axis and the servo’s values (To set properly the servos angles).

The main code:

#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>
#include <Wire.h>
#include <Servo.h>

float angles[3]; 

Servo servo_x;  
Servo servo_y; 
Servo servo_z;

int pos_x = 0;  
int pos_y = 0;  
int pos_z = 0;

FreeSixIMU sixDOF = FreeSixIMU();

void setup() 
{ 
  Serial.begin(9600);
  
  Wire.begin();
  
  servo_z.attach(10); 
  servo_z.write(90);
  
  servo_x.attach(11); 
  servo_y.attach(12);
  
  sixDOF.init(); 
  
  delay(5);
}

void loop() 
{   
  sixDOF.getYawPitchRoll(angles);
  
  pos_x = (1570-angles[1]*9.55);
  servo_x.writeMicroseconds(pos_x);
 
  pos_y = (1400+angles[2]*9.55);
  servo_y.writeMicroseconds(pos_y); 
 
  //pos_z = (1500+angles[0]*9.55);
  //servo_z.writeMicroseconds(pos_z); 
  
  //printData();
}

and the “printData”:

void printData()
{
  Serial.print("X:");
  Serial.print(angles[1]);
  Serial.print("   ");
  
  Serial.print("Pos_X:");
  Serial.print(pos_x);
  Serial.print(" ||  ");
  
  Serial.print("Y:");
  Serial.print(angles[2]);
  Serial.print("   ");
  
  Serial.print("Pos_Y:");
  Serial.print(pos_y);
  Serial.print(" ||  ");
  
  Serial.print("Z:");
  Serial.print(angles[0]);
  Serial.print("   ");
  
  Serial.print("Pos_Z:");
  Serial.print(pos_z);
  Serial.print("   ");
  
  Serial.println("   ");
  delay(5);
}

To have a fluent movements of the servos, please comment the “printData”.

Cheers,

PS: I apologize my poor English…i’m french

Christophe

To have a fluent movements of the servos, please comment the "printData".

Or bump up the data rate to 115200.

Do you want me to move this to the exhibition section?

Hello,

Ok, no problem.

Thanks for the information.

Regards,