An advanced robot that uses PID control on the motor speed, and an IMU for making accurate turns.

Dependencies:   mbed Motor ITG3200 QEI ADXL345 IMUfilter PID

Files at this revision

API Documentation at this revision

Comitter:
aberk
Date:
Fri Sep 10 14:03:00 2010 +0000
Commit message:
Version 1.0

Changed in this revision

ADXL345.lib Show annotated file Show diff for this revision Revisions of this file
IMU.cpp Show annotated file Show diff for this revision Revisions of this file
IMU.h Show annotated file Show diff for this revision Revisions of this file
IMUfilter.lib Show annotated file Show diff for this revision Revisions of this file
ITG3200.lib Show annotated file Show diff for this revision Revisions of this file
Motor.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 7440a03255a7 ADXL345.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 IMU.cpp
--- /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_);
+
+}
diff -r 000000000000 -r 7440a03255a7 IMU.h
--- /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 */
diff -r 000000000000 -r 7440a03255a7 IMUfilter.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 ITG3200.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 Motor.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 PID.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 QEI.lib
--- /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
diff -r 000000000000 -r 7440a03255a7 main.cpp
--- /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);
+
+}
diff -r 000000000000 -r 7440a03255a7 mbed.bld
--- /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