Head Motion Tracking for RC Cam, Arduino-Based
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
/*
* 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 averageint Yreadings[NUMREADINGS]; // the readings from the Y axis
int Ytotal = 0; // the running total
int Yaverage = 0; // the averageint 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 averageint 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 framelong 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 loopvoid 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 totalYtotal -= Yreadings[index]; // subtract the last reading
Yreadings[index] = analogRead(YinputPin); // read from the sensor
Ytotal += Yreadings[index]; // add the reading to the totalXtotal -= 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 totalindex = (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 beginningZaverage = 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 averagePitch = 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);
}
}
Comment by Ria — November 21, 2007 at 5:06
Um … *cough* That raisin walnut bread post looks interesting
Comment by Mathieu — November 21, 2007 at 9:07
lol
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!
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!