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