Revision 0:7440a03255a7, committed 2010-09-10

Comitter:

aberk

Date:

Fri Sep 10 14:03:00 2010 +0000

Commit message:

Version 1.0

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ADXL345/#bd8f0f20f433

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.cpp	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,338 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+
+/**
+ * Includes
+ */
+#include "IMU.h"
+
+IMU::IMU(float imuRate,
+         double gyroscopeMeasurementError,
+         float accelerometerRate,
+         float gyroscopeRate) : accelerometer(p5, p6, p7, p8),
+        gyroscope(p9, p10), imuFilter(imuRate, gyroscopeMeasurementError) {
+
+    imuRate_ = imuRate;
+    accelerometerRate_ = accelerometerRate;
+    gyroscopeRate_ = gyroscopeRate;
+
+    //Initialize sampling variables.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    accelerometerSamples = 0;
+    gyroscopeSamples = 0;
+
+    //Initialize and calibrate sensors.
+    initializeAccelerometer();
+    calibrateAccelerometer();
+
+    initializeGyroscope();
+    calibrateGyroscope();
+
+    //To reduce the number of interrupts we'll remove the separate tickers for
+    //the accelerometer, gyro and filter update and combine them all into one.
+
+    //accelerometerTicker.attach(this, &IMU::sampleAccelerometer, accelerometerRate_);
+    //gyroscopeTicker.attach(this, &IMU::sampleGyroscope, gyroscopeRate_);
+    sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
+    //filterTicker.attach(this, &IMU::filter, imuRate_);
+
+}
+
+double IMU::getRoll(void) {
+
+    return toDegrees(imuFilter.getRoll());
+
+}
+
+double IMU::getPitch(void) {
+
+    return toDegrees(imuFilter.getPitch());
+
+}
+
+double IMU::getYaw(void) {
+
+    return toDegrees(imuFilter.getYaw());
+
+}
+
+void IMU::initializeAccelerometer(void) {
+
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+    //200Hz data rate.
+    accelerometer.setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+
+}
+
+void IMU::sampleAccelerometer(void) {
+
+    //If we've taken a certain number of samples,
+    //average them, remove the bias and convert the units.
+    if (accelerometerSamples == SAMPLES) {
+
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+        accelerometerSamples = 0;
+
+    }
+    //Otherwise, accumulate another reading. 
+    else {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void IMU::calibrateAccelerometer(void) {
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+    //Accumulate a certain number of samples.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(accelerometerRate_);
+
+    }
+
+    //Average the samples.
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //These are our zero g offsets.
+    //250 = 9.81m/s/s @ 4mg/LSB.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    //Reset accumulators.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+}
+
+void IMU::initializeGyroscope(void) {
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz.
+    gyroscope.setSampleRateDivider(4);
+
+}
+
+void IMU::calibrateGyroscope(void) {
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Accumulate a certain number of samples.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+        wait(gyroscopeRate_);
+
+    }
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //Set the null bias.
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    //Reset the accumulators.
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void IMU::sampleGyroscope(void) {
+
+    //If we've taken the required number of samples then,
+    //average the samples, removed the null bias and convert the units
+    //to rad/s.
+    if (gyroscopeSamples == SAMPLES) {
+
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+    }
+    //Accumulate another sample. 
+    else {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        gyroscopeSamples++;
+
+    }
+
+}
+
+void IMU::sample(void) {
+
+    //If we've taken enough samples then,
+    //average the samples, remove the offsets and convert to appropriate units.
+    //Feed this information into the filter to calculate the new Euler angles.
+    if (accelerometerSamples == SAMPLES) {
+
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+
+        accelerometerSamples = 0;
+
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+        //Update the filter variables.
+        imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+        //Calculate the new Euler angles.
+        imuFilter.computeEuler();
+
+    }
+    //Accumulate another sample. 
+    else {
+
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void IMU::filter(void) {
+
+    //Update the filter variables.
+    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+    //Calculate the new Euler angles.
+    imuFilter.computeEuler();
+
+}
+
+void IMU::reset(void) {
+
+    //Disable interrupts.
+    sampleTicker.detach();
+    
+    //Recalibrate sensors.
+    calibrateAccelerometer();
+    calibrateGyroscope();
+    
+    //Reset the IMU filter.
+    //imuFilter.reset();
+    
+    //Reset the working variables.
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+    accelerometerSamples = 0;
+    gyroscopeSamples = 0;
+    
+    //Enable interrupts.
+    sampleTicker.attach(this, &IMU::sample, accelerometerRate_);
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU.h	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,210 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope using
+ * orientation filter developed by Sebastian Madgwick.
+ *
+ * Find more details about his paper here:
+ *
+ * http://code.google.com/p/imumargalgorithm30042010sohm/
+ */
+
+#ifndef MBED_IMU_H
+#define MBED_IMU_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+#include "IMUfilter.h"
+
+/**
+ * Defines
+ */
+#define IMU_RATE           0.025
+#define ACCELEROMETER_RATE 0.005
+#define GYROSCOPE_RATE     0.005
+#define GYRO_MEAS_ERROR    0.3 //IMUfilter tuning parameter.
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average
+#define SAMPLES 4
+#define CALIBRATION_SAMPLES 128
+//Multiply radians to get degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Multiply degrees to get radians.
+#define toRadians(x) (x * 0.01745329252)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+//Multiply ADC count readings from ADXL345 to get acceleration in m/s/s.
+#define toAcceleration(x) (x * (4 * g0 * 0.001))
+//14.375 LSB/(degrees/sec)
+#define GYROSCOPE_GAIN (1 / 14.375)
+#define ACCELEROMETER_GAIN (0.004 * g0)
+
+/**
+ * IMU consisting of ADXL345 accelerometer and ITG-3200 gyroscope to calculate
+ * roll, pitch and yaw angles.
+ */
+class IMU {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * @param imuRate Rate which IMUfilter update and Euler angle calculation
+     *                occurs.
+     * @param gyroscopeMeasurementError IMUfilter tuning parameter.
+     * @param accelerometerRate Rate at which accelerometer data is sampled.
+     * @param gyroscopeRate Rate at which gyroscope data is sampled.
+     */
+    IMU(float imuRate,
+        double gyroscopeMeasurementError,
+        float accelerometerRate,
+        float gyroscopeRate);
+
+    /**
+     * Get the current roll angle.
+     *
+     * @return The current roll angle in degrees.
+     */
+    double getRoll(void);
+
+    /**
+     * Get the current pitch angle.
+     *
+     * @return The current pitch angle in degrees.
+     */
+    double getPitch(void);
+
+    /**
+     * Get the current yaw angle.
+     *
+     * @return The current yaw angle in degrees.
+     */
+    double getYaw(void);
+
+    /**
+     * Sample the sensors, and if enough samples have been taken,
+     * take an average, and compute the new Euler angles.
+     */
+    void sample(void);
+    
+    /**
+     * Recalibrate the sensors and reset the IMU filter.
+     */
+    void reset(void);
+
+private:
+
+    /**
+     * Set up the ADXL345 appropriately.
+     */
+    void initializeAccelerometer(void);
+
+    /**
+     * Calculate the zero g offset.
+     */
+    void calibrateAccelerometer(void);
+
+    /**
+     * Take a set of samples and average them.
+     */
+    void sampleAccelerometer(void);
+
+    /**
+     * Set up the ITG-3200 appropriately.
+     */
+    void initializeGyroscope(void);
+
+    /**
+     * Calculate the bias offset.
+     */
+    void calibrateGyroscope(void);
+
+    /**
+     * Take a set of samples and average them.
+     */
+    void sampleGyroscope(void);
+
+    /**
+     * Update the filter and calculate the Euler angles.
+     */
+    void filter(void);
+
+    ADXL345   accelerometer;
+    ITG3200   gyroscope;
+    IMUfilter imuFilter;
+
+    Ticker accelerometerTicker;
+    Ticker gyroscopeTicker;
+    Ticker sampleTicker;
+    Ticker filterTicker;
+
+    float accelerometerRate_;
+    float gyroscopeRate_;
+    float imuRate_;
+
+    //Offsets for the gyroscope.
+    //The readings we take when the gyroscope is stationary won't be 0, so we'll
+    //average a set of readings we do get when the gyroscope is stationary and
+    //take those away from subsequent readings to ensure the gyroscope is offset
+    //or biased to 0.
+    double w_xBias;
+    double w_yBias;
+    double w_zBias;
+
+    double a_xBias;
+    double a_yBias;
+    double a_zBias;
+
+    volatile double a_xAccumulator;
+    volatile double a_yAccumulator;
+    volatile double a_zAccumulator;
+    volatile double w_xAccumulator;
+    volatile double w_yAccumulator;
+    volatile double w_zAccumulator;
+
+    //Accelerometer and gyroscope readings for x, y, z axes.
+    volatile double a_x;
+    volatile double a_y;
+    volatile double a_z;
+    volatile double w_x;
+    volatile double w_y;
+    volatile double w_z;
+
+    //Buffer for accelerometer readings.
+    int readings[3];
+    int accelerometerSamples;
+    int gyroscopeSamples;
+
+};
+
+#endif /* MBED_IMU_H */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ITG3200/#b098d99dd81e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/Motor/#c75b234558af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,345 @@
+/**
+ * Includes.
+ */
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+#include "IMU.h"
+
+/**
+ * Defines.
+ */
+#define BUFFER_SIZE 1024 //Used for data logging arrays
+#define RATE 0.01        //PID loop timing
+
+//PID tuning constants.
+#define L_KC 0.4312 //Forward left motor Kc
+#define L_TI 0.1    //Forward left motor Ti
+#define L_TD 0.0    //Forward left motor Td
+#define R_KC 0.4620 //Forward right motor Kc
+#define R_TI 0.1    //Forward right motor Ti
+#define R_TD 0.0    //Forward right motor Td
+
+//PID input/output limits.
+#define L_INPUT_MIN 0     //Forward left motor minimum input
+#define L_INPUT_MAX 3000  //Forward left motor maximum input
+#define L_OUTPUT_MIN 0.0  //Forward left motor minimum output
+#define L_OUTPUT_MAX 0.9  //Forward left motor maximum output
+
+#define R_INPUT_MIN 0     //Forward right motor input minimum
+#define R_INPUT_MAX 3200  //Forward right motor input maximum
+#define R_OUTPUT_MIN 0.0  //Forward right motor output minimum
+#define R_OUTPUT_MAX 0.9  //Forward right motor output maximum
+
+//Physical dimensions.
+#define PULSES_PER_REV        624
+#define WHEEL_DIAMETER        58.928 //mm
+#define ROTATION_DISTANCE     220.0  //mm
+#define REVS_PER_ROTATION     (ROTATION_DISTANCE / WHEEL_DIAMETER)
+#define PULSES_PER_ROTATION   (REVS_PER_ROTATION * PULSES_PER_REV)
+#define PULSES_PER_MM         (PULSES_PER_REV / WHEEL_DIAMETER)
+#define DISTANCE_PER_PULSE    (WHEEL_DIAMETER / PULSES_PER_REV)
+#define ENCODING              2 //Use X2 encoding
+#define WHEEL_DISTANCE        (ROTATION_DISTANCE / DISTANCE_PER_PULSE)
+
+/**
+ * Function Prototypes
+ */
+void initializeMotors(void);
+void initializePid(void);
+//cmd = "move", "turn"
+//direction = "forward", "backward", "left", "right"
+//length = distance in metres or angle in degrees
+void processCommand(char* cmd, char* direction, float length);
+
+/**
+ * Globals
+ */
+
+//Debug.
+Serial pc(USBTX, USBRX);
+
+//Motor control.
+Motor leftMotor(p21, p20, p19);  //pwm, inB, inA
+Motor rightMotor(p22, p17, p18); //pwm, inA, inB
+QEI leftQei(p29, p30, NC, 624);  //chanA, chanB, index, ppr
+QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
+PID leftPid(L_KC, L_TI, L_TD, RATE);  //Kc, Ti, Td
+PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td
+//IMU and peripherals run at a different rate to the main PID loop.
+IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE);
+
+//Filesystem.
+LocalFileSystem local("local");
+
+//Left motor working variables.
+int leftPulses     = 0;
+int leftPrevPulses = 0;
+float leftVelocity = 0.0;
+float leftVelocityBuffer[BUFFER_SIZE];
+
+//Right motor working variables.
+int rightPulses     = 0;
+int rightPrevPulses = 0;
+float rightVelocity = 0.0;
+float rightVelocityBuffer[BUFFER_SIZE];
+
+//General working variables.
+float heading       = 0.0;
+float prevHeading   = 0.0;
+float degreesTurned = 0.0;
+float positionSetPoint = 0.0;
+float headingSetPoint  = 0.0;
+
+//Store the initial and end heading during a straight line section
+//in order to be able to correct turns.
+float startHeading = 0.0;
+float endHeading   = 0.0;
+
+//Command Parser.
+char  cmd0[16]; //{"move", "turn"}
+char  cmd1[16]; //{{"forward", "backward"}, {"left", "right"}}
+float cmd2 = 0; //{distance in METRES, angle in DEGREES}
+
+void initializeMotors(void) {
+
+    //Set motor PWM periods to 20KHz.
+    leftMotor.period(0.00005);
+    leftMotor.speed(0.0);
+
+    rightMotor.period(0.00005);
+    rightMotor.speed(0.0);
+
+}
+
+void initializePid(void) {
+
+    //Input and output limits have been determined
+    //empirically with the specific motors being used.
+    //Change appropriately for different motors.
+    //Input  units: counts per second.
+    //Output units: PwmOut duty cycle as %.
+    //Default limits are for moving forward.
+    leftPid.setInputLimits(L_INPUT_MIN, L_INPUT_MAX);
+    leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX);
+    leftPid.setMode(AUTO_MODE);
+    rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX);
+    rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX);
+    rightPid.setMode(AUTO_MODE);
+
+}
+
+void processCommand(char* cmd, char* direction, float length) {
+
+    //move command.
+    if (strcmp(cmd, "move") == 0) {
+
+        int i = 0; //Data log array index.
+
+        //The PID controller works in the % (unsigned) domain, so we'll
+        //need to multiply the output by -1 if our motors need
+        //to go "backwards".
+        int leftDirection  = 1;
+        int rightDirection = 1;
+
+        //Convert from metres into millimetres.
+        length *= 1000;
+        //Work out how many pulses are required to go that many millimetres.
+        length *= PULSES_PER_MM;
+        //Make sure we scale the number of pulses according to our encoding method.
+        length /= ENCODING;
+
+        positionSetPoint = length;
+
+        if (strcmp(direction, "forward") == 0) {
+            //Leave left and rightDirection as +ve.
+        } else if (strcmp(cmd1, "backward") == 0) {
+            //Change left and rightDirection to -ve.
+            leftDirection  = -1;
+            rightDirection = -1;
+        }
+
+        startHeading = imu.getYaw();
+
+        //With left set point == right set point, the rover veers off to the
+        //right - by slowing the right motor down slightly it goes relatively
+        //straight.
+        leftPid.setSetPoint(1000);
+        rightPid.setSetPoint(975);
+
+        //Keep going until we've moved the correct distance defined by the
+        //position set point. Take the absolute value of the pulses as we might
+        //be moving backwards.
+        while ((abs(leftPulses) < positionSetPoint) &&
+                (abs(rightPulses) < positionSetPoint)) {
+
+            //Get the current pulse count and subtract the previous one to
+            //calculate the current velocity in counts per second.
+            leftPulses = leftQei.getPulses();
+            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+            leftPrevPulses = leftPulses;
+            //Use the absolute value of velocity as the PID controller works
+            //in the % (unsigned) domain and will get confused with -ve values.
+            leftPid.setProcessValue(fabs(leftVelocity));
+            leftMotor.speed(leftPid.compute() * leftDirection);
+
+            rightPulses = rightQei.getPulses();
+            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
+            rightPrevPulses = rightPulses;
+            rightPid.setProcessValue(fabs(rightVelocity));
+            rightMotor.speed(rightPid.compute() * rightDirection);
+
+            i++;
+
+            wait(RATE);
+
+        }
+
+        leftMotor.brake();
+        rightMotor.brake();
+
+        endHeading = imu.getYaw();
+
+    }
+    //turn command.
+    else if (strcmp(cmd0, "turn") == 0) {
+
+        //The PID controller works in the % (unsigned) domain, so we'll
+        //need to multiply the output by -1 for the motor which needs
+        //to go "backwards".
+        int leftDirection  = 1;
+        int rightDirection = 1;
+        headingSetPoint = length + (endHeading - startHeading);
+
+        //In case the rover tries to [pointlessly] turn >360 degrees.
+        if (headingSetPoint > 359.8) {
+
+            headingSetPoint -= 359.8;
+
+        }
+        
+        //Set up the right previous heading for the initial degreesTurned
+        //calculation.
+        prevHeading = fabs(imu.getYaw());
+
+        if (strcmp(cmd1, "left") == 0) {
+
+            //When turning left, the left motor needs to go backwards
+            //while the right motor goes forwards.
+            leftDirection  = -1;
+
+        } else if (strcmp(cmd1, "right") == 0) {
+
+            //When turning right, the right motor needs to go backwards
+            //while the left motor goes forwards.
+            rightDirection = -1;
+
+        }
+
+        //Turn slowly to ensure we don't overshoot the angle by missing
+        //the relatively slow readings [in comparison to the PID loop speed]
+        //from the IMU.
+        leftPid.setSetPoint(500);
+        rightPid.setSetPoint(500);
+
+        //Keep turning until we've moved through the correct angle as defined
+        //by the heading set point.
+        while (degreesTurned < headingSetPoint) {
+
+            //Get the new heading and subtract the previous heading to
+            //determine how many more degrees we've moved through.
+            //As we're only interested in the relative angle (as opposed to
+            //absolute) we'll take the absolute value of the heading to avoid
+            //any nastiness when trying to calculate angles as they jump from
+            //179.9 to -179.9 degrees.
+            heading = fabs(imu.getYaw());
+            degreesTurned += fabs(heading - prevHeading);
+            prevHeading = heading;
+
+            //Get the current pulse count and subtract the previous one to
+            //calculate the current velocity in counts per second.
+            leftPulses = leftQei.getPulses();
+            leftVelocity = (leftPulses - leftPrevPulses) / RATE;
+            leftPrevPulses = leftPulses;
+            //Use the absolute value of velocity as the PID controller works
+            //in the % (unsigned) domain and will get confused with -ve values.
+            leftPid.setProcessValue(fabs(leftVelocity));
+            leftMotor.speed(leftPid.compute() * leftDirection);
+
+            rightPulses = rightQei.getPulses();
+            rightVelocity = (rightPulses - rightPrevPulses) / RATE;
+            rightPrevPulses = rightPulses;
+            rightPid.setProcessValue(fabs(rightVelocity));
+            rightMotor.speed(rightPid.compute() * rightDirection);
+
+            wait(RATE);
+
+        }
+
+        leftMotor.brake();
+        rightMotor.brake();
+
+    }
+
+    //Reset working variables.
+    leftQei.reset();
+    rightQei.reset();
+
+    leftPulses     = 0;
+    leftPrevPulses = 0;
+    leftVelocity   = 0.0;
+    leftMotor.speed(0.0);
+
+    rightPulses     = 0;
+    rightPrevPulses = 0;
+    rightVelocity   = 0.0;
+    rightMotor.speed(0.0);
+
+    leftPid.setSetPoint(0.0);
+    leftPid.setProcessValue(0.0);
+    rightPid.setSetPoint(0.0);
+    rightPid.setProcessValue(0.0);
+
+    positionSetPoint = 0.0;
+    headingSetPoint  = 0.0;
+    heading          = 0.0;
+    prevHeading      = 0.0;
+    degreesTurned    = 0.0;
+
+}
+
+int main() {
+
+    pc.printf("Starting mbed rover test...\n");
+
+    initializeMotors();
+    initializePid();
+
+    //Some delay before we start moving.
+    wait(5);
+
+    //Open the command script.
+    FILE* commands = fopen("/local/commands.txt", "r");
+
+    //Check if we were successful in opening the command script.
+    if (commands == NULL) {
+        pc.printf("Could not open command file...\n");
+        exit(EXIT_FAILURE);
+    } else {
+        pc.printf("Succesfully opened command file!\n");
+    }
+
+    //While there's another line to read from the command script.
+    while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) {
+
+        pc.printf("%s %s %f\n", cmd0, cmd1, cmd2);
+
+        processCommand(cmd0, cmd1, cmd2);
+
+        wait(1);
+
+    }
+
+    fclose(commands);
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 10 14:03:00 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/74b8d43b5817