Hello!
I'm trying to to make a self adjusting pan and tilt setup. Idea is that the camera will always point towards one position. Even if the set-up was to be rotated along XYZ axis.
I'm using the Sparkfun 6DOF IMU http://www.sparkfun.com/products/10121 and an Arduino Nano.
I can get the Euler Angles from the IMU to display in the serial monitor using the example code from http://bildr.org/2012/03/stable-orientation-digital-imu-6dof-arduino/, what I'm stuck on is how to use this data and make it work with another script of code that controls a servo.
This is my code so far;
It's not particularly elegant, to compensate for 'jitter' of the measured angles, I've created bands that the measured angles fall in, and these bands determine the angle the servo should move to.
Any tips or pointers would be greatly appreciated!
Thanks!
#include <FreeSixIMU.h>
#include <FIMU_ADXL345.h>
#include <FIMU_ITG3200.h>
#include <Wire.h>
#include <Servo.h>
//global declarations
#define SERVO 4 //define a D pin for servo
Servo myservo; // initialize servo
float angles[3]; // yaw pitch roll
int pw0;
int pw1;
int pw2;
int res;
// These values need to be set for each servo!!!
int forward = 2400;//2342 when no load
int neutral = 1500;
int backward = 600;//622 when no load
// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();
void setup() {
pinMode (SERVO, OUTPUT); // want servo pin to be an output
myservo.attach(SERVO); // attach pin to the servo
myservo.writeMicroseconds(1500); // set servo to mid-point
Serial.begin(4800);
Wire.begin();
delay(5);
sixDOF.init(); //begin the IMU
delay(5);
Serial.print("eulerX\teulerY\teulerZ\n");
}
void loop() {
sixDOF.getEuler(angles);
res=10;
if (angles[0]>0 && angles[0]<=res) {
pw0=1500; // do nothing
delay(50);
}
else if (angles[0]>res && angles[0]<=(2*res)) {
pw0=-(1500+ (res/90)*900);
delay(50);
}
else if (angles[0]>(2*res) && angles[0]<=(3*res)) {
pw0=-(1500+( 2*res/90)*900);
delay(50);
}
else if (angles[0]>(3*res) && angles[0]<=(4*res)) {
pw0=-(1500+( 3*res/90)*900);
delay(50);
}
else if (angles[0]>(4*res) && angles[0]<=(5*res)) {
pw0=-(1500+( 4*res/90)*900);
delay(50);
}
else if (angles[0]>(5*res) && angles[0]<=(6*res)) {
pw0=-(1500+( 5*res/90)*900);
delay(50);
}
else if (angles[0]>(6*res) && angles[0]<=(7*res)) {
pw0=-(1500+( 6*res/90)*900);
delay(50);
}
else if (angles[0]>(7*res) && angles[0]<=(8*res)) {
pw0=-(1500+( 7*res/90)*900);
delay(50);
}
else if (angles[0]>(8*res) && angles[0]<=(9*res)) {
pw0=-(1500+( 8*res/90)*900);
delay(50);
}
else if (angles[0]>=(9*res)) {
pw0=-(1500+( 9*res/90)*900);
delay(50);
}
else {
pw0=1500;
delay(50);
}
myservo.writeMicroseconds(pw0);
delay(500);
Serial.print(angles[0]);
Serial.print("\t");
Serial.print(angles[1]);
Serial.print("\t");
Serial.println(angles[2]);
//Serial.print("\t");
//Serial.print(servoanglet0);
Serial.print("\n");
delay(50);
}