Head Motion Tracking for RC Cam, Arduino-Based

Blogged by Mathieu as Electronics — Mathieu Thu 1 Nov 2007 12:52

Mashed together the inclinometer code and the arduino-controlled RC transmitter and I can now control two servos remotely on the model by tilting the arduino forwards/backwards and left/right. Cleaned up a couple rookie mistakes in the original code, too… like defining 5.7 as an integer. (oops)

Here is the new code:

/*
* Remote Inclinometer Interface
* -------------------
*
*
* Created 01 November 2007
* copyleft 2007 Mathieu Glachant
* http://ban-sidhe.com/
*
*/

#include
// Need math.h for the trig in the inclinometer

/*
* Smoothing of the inclination Measurement
* Variable definitions
*/

// Define the number of samples to keep track of. The higher the number,
// the more the readings will be smoothed, but the slower the output will
// respond to the input. Using a #define rather than a normal variable lets
// use this value to determine the size of the readings array.

#define NUMREADINGS 16 // Number of readings to average

int index = 0; // the index of the current reading

int Zreadings[NUMREADINGS]; // the readings from the Z axis
int Ztotal = 0; // the running total
int Zaverage = 0; // the average

int Yreadings[NUMREADINGS]; // the readings from the Y axis
int Ytotal = 0; // the running total
int Yaverage = 0; // the average

int Xreadings[NUMREADINGS]; // the readings from the X axis
int Xtotal = 0; // the running total
int Xaverage = 0; // the average

//int YRreadings[NUMREADINGS]; // the gyro readings from the Y axis
//int YRtotal = 0; // the running total
//int YRaverage = 0; // the average

//int XRreadings[NUMREADINGS]; // the gyro readings from the X axis
//int XRtotal = 0; // the running total
//int XRaverage = 0; // the average

int Pitch = 0;
int Bank = 0;
//int PitchRate = 0;
//int BankRate = 0;

int ZinputPin = 3;
int YinputPin = 4;
int XinputPin = 5;
//int YRinputPin = 2;
//int XRinputPin = 1;

/*
* PPM interface to the radio transmitter
* Variable definitions
*/

#define channelNumber 2 // Number of channels to send (keep below frameLength/pulseMax)

int servoPin = 12; // Control pin for trainer interface

// A pulse starts with a low signal of fixed width (0.3ms),
// followed by a high signal for the remainder of the pulse.
// Total pulse width is proportional to servo position (1 to 2ms)
int pulseStart = 300; // pulse start width in microseconds
int pulseMin = 724; // pulse minimum width minus start in microseconds
int pulseMax = 2048; // pulse maximum width in microseconds
int conversionFactor = (pulseMax - pulseMin - pulseStart)/18; // (will divide by ten, wanted two digits without using a float)

// A frame is a succession of pulses, in order of channels,
// followed by a synchronisation pulse to fill out the frame.
// A frame's total length is fixed (20ms)
int frameLength = 20; // The duration in millisecs of a frame

long lastFrame = 0; // The time in millisecs of the last frame
int servo[channelNumber]; // Values to set for the servos in degrees
int channel[channelNumber]; // Values to send on channels (duration of pulse minus start, in microseconds)
int i; // Counter in for loop

void setup() {
pinMode(servoPin, OUTPUT); // Set servo pin as an output pin
Serial.begin(9600); // connect to the serial port
for ( i = 0; i < channelNumber; i = i++ ) {servo[i] = 0;}
for ( i = 0; i < channelNumber; i = i++ ) {channel[i] = pulseMin;}
for (int i = 0; i < NUMREADINGS; i++)
{
Zreadings[i] = 0; // initialize all the readings to 0
Yreadings[i] = 0;
Xreadings[i] = 0;
//YRreadings[i] = 0;
//XRreadings[i] = 0;
}
Serial.println("Remote Inclinometer ready");
Serial.print("Number of readings averaged:");
Serial.println(NUMREADINGS);
Serial.print("Number of channels sent to radio:");
Serial.println(channelNumber);
Serial.print("Conversion factor (degrees to microseconds):");
Serial.println(conversionFactor);
}

void loop() {

// Save the time of frame start
lastFrame = millis();

// This for loop generates the pulse train, one per channel
for ( i = 0; i < channelNumber; i = i + 1 ) {
digitalWrite(servoPin, LOW); // Initiate pulse start
delayMicroseconds(pulseStart); // Duration of pulse start
digitalWrite(servoPin, HIGH); // Stop pulse start
delayMicroseconds(channel[i]); // Finish off pulse
}
digitalWrite(servoPin, LOW); // Initiate synchronisation pulse
delayMicroseconds(pulseStart); // Duration of start of synchronisation pulse
digitalWrite(servoPin, HIGH); // Stop synchronisation pulse start

// We're done generating pulses and using delayMicroseconds()
// Time to do some other processing before the next frame
// How much time depends on how many channels you are running

// Let's update our accelerometer readings and pitch/bank angles

Ztotal -= Zreadings[index]; // subtract the last reading
Zreadings[index] = analogRead(ZinputPin); // read from the sensor
Ztotal += Zreadings[index]; // add the reading to the total

Ytotal -= Yreadings[index]; // subtract the last reading
Yreadings[index] = analogRead(YinputPin); // read from the sensor
Ytotal += Yreadings[index]; // add the reading to the total

Xtotal -= Xreadings[index]; // subtract the last reading
Xreadings[index] = analogRead(XinputPin); // read from the sensor
Xtotal += Xreadings[index]; // add the reading to the total

//YRtotal -= YRreadings[index]; // subtract the last reading
//YRreadings[index] = analogRead(YRinputPin); // read from the sensor
//YRtotal += YRreadings[index]; // add the reading to the total

//XRtotal -= XRreadings[index]; // subtract the last reading
//XRreadings[index] = analogRead(XRinputPin); // read from the sensor
//XRtotal += XRreadings[index]; // add the reading to the total

index = (index + 1); // advance to the next index

if (index >= NUMREADINGS) // if we're at the end of the array...
index = 0; // ...wrap around to the beginning

Zaverage = Ztotal / NUMREADINGS - 342; // calculate the average
Yaverage = Ytotal / NUMREADINGS - 353; // calculate the average
Xaverage = Xtotal / NUMREADINGS - 350; // calculate the average
//YRaverage = YRtotal / NUMREADINGS - 330; // calculate the average
//XRaverage = XRtotal / NUMREADINGS - 332; // calculate the average

Pitch = atan2(-Xaverage,Zaverage)*57;
Bank = atan2(-Yaverage,Zaverage)*57;
//PitchRate = YRaverage;
//BankRate = XRaverage;

// Let's change the servo positions
// Used a for and a switch to prevent breakage if channelNumber is changed, despite the speed loss
for ( i = 0; i < channelNumber; i = i + 1 ) {
switch ( i ) {
case 0:
servo[i]=Pitch+90;
break;
case 1:
servo[i]=Bank+90;
break;
default:
servo[i]=90;
}
}

// Calculate pulse durations from servo positions
for ( i = 0; i < channelNumber; i = i + 1 ) {
channel[i] = abs(servo[i]-180);
channel[i] = int(channel[i]*conversionFactor/10)+pulseMin; // Wanted to avoid floats but have two digits on conversion Factor
}

// We're ready to wait for the next frame
// Some jitter is allowed, so to the closest ms
while (millis() - lastFrame < frameLength) {
delay(1);
}
}

4 Comments »

  1. Comment by Ria — November 21, 2007 at 5:06

    Um … *cough* That raisin walnut bread post looks interesting

  2. Comment by Mathieu — November 21, 2007 at 9:07

    lol

  3. Comment by Mitch — April 14, 2008 at 3:26

    Great work interfacing Arduino with the accelerometer on the Sparkfun 5DOF. You may be interested in trying to filter the readings using a Kalman filter and the Gyro readings. The hardware is there, you may as well, plus it returns much nicer results. Watch the video here, you’ll see what i mean:

    http://tom.pycke.be/mav/92/kalman-demo-application

    GL!

  4. Comment by Mathieu — April 14, 2008 at 8:27

    Yes, that would be the next step for this project, but I’m a bit tied up on other stuff at the moment… soon, I hope!

RSS feed for comments on this post. TrackBack URI

Leave a comment

XHTML: You can use these tags: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>

Click on smiley to insert!

) (w) (u) p (y) (n) d (*) o) 8) ( (f) (g) (t) o (8) (l) (i) x (~) (e) $ (&amp) (c) ( s (d) (o) (@) (p) (^) (b) [

Proudly powered by wordpress - Theme Back in Black 2 by neuro