Revision 0:0150acbc6cf4, committed 2012-04-18

Comitter:

jakobholsen

Date:

Wed Apr 18 06:00:26 2012 +0000

Commit message:

first publish

Changed in this revision

diff -r 000000000000 -r 0150acbc6cf4 BalancingRobot.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BalancingRobot.cpp	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,455 @@
+/*
+ * The code is released under the GNU General Public License.
+ * Developed by Kristian Lauszus
+ * This is the algorithm for my balancing robot/segway.
+ * It is controlled by a PS3 Controller via bluetooth.
+ * The remote control code can be found at my other github repository: https://github.com/TKJElectronics/BalancingRobot
+ * For details, see http://blog.tkjelectronics.dk/2012/02/the-balancing-robot/
+ */
+
+#include "mbed.h"
+#include "BalancingRobot.h"
+#include "dof9RazorImuAhrs.h"
+
+
+/* Setup encoders */
+//#include "Encoder.h"
+
+//Encoder leftEncoder(p29,p30);
+//Encoder rightEncoder(p28,p27);
+
+/* Setup serial communication */
+Serial xbee(p13,p14); // For wireless debugging and setting PID constants
+Serial ps3(p9,p10); // For remote control
+Serial debug(USBTX, USBRX); // USB
+dof9RazorImuAhrs theRazor(p28, p27);
+
+
+/* Setup timer instance */
+Timer t;
+
+int main() {
+
+    /* Set baudrate */
+    xbee.baud(57600);
+    ps3.baud(115200);
+    debug.baud(115200);
+    
+    /* Set PWM frequency */
+    leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
+    rightPWM.period(0.00005);
+    
+    /* Calibrate the gyro and accelerometer relative to ground - JAOL dos that on the IMU - thank you ATmega!
+    
+    */
+   //calibrateSensors();
+    //debug.printf("Initialized\n");
+    xbee.printf("Initialized\n");
+    processing(); // Send output to processing application
+    
+    /* Setup timing */
+    t.start();
+    loopStartTime = t.read_us();
+    timer = loopStartTime;
+    
+    while (1) {
+        theRazor.update();
+        /* Calculate pitch  */
+        //accYangle = getAccY();
+        //gyroYrate = getGyroYrate();
+     
+        // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.html
+       // pitch = kalman(accYangle, gyroYrate, t.read_us() - timer); // calculate the angle using a Kalman filter
+       
+     // pitch = kalman(accYangle, theRazor.getGyroY(), t.read_us() - timer); // calculate the angle using a Kalman filter
+    
+    
+     
+     pitch = theRazor.getPitch();
+    
+    
+    // Trace Bay
+    //xbee.printf("gyroYrate: %f\n",gyroYrate);
+    //xbee.printf("accYangle: %f\n",accYangle);
+    //xbee.printf("accYangle: %f\n",gyroYrate*dt);
+    
+   // xbee.printf("IMUs Gyro Y: %f\n",theRazor.getGyroY());
+   // xbee.printf("IMUs Accel Y: %f\n",theRazor.getAccY());
+    //xbee.printf("theRazor.getPitch(): %f\n",theRazor.getPitch());
+  // xbee.printf("Pitch: %f\n",pitch);
+        
+    timer = t.read_us();
+ 
+        
+        /* Drive motors */
+        if (pitch < -75 || pitch > 75) // Stop if falling or laying down
+            stopAndReset();
+            
+        else 
+            PID(targetAngle,targetOffset);
+        
+        /* Update wheel velocity every 100ms  */
+       loopCounter++;
+       /*
+        if (loopCounter%10 == 0) { // If remainder is equal 0, it must be 10,20,30 etc.
+           xbee.printf("Wheel - timer: %i\n",t.read_ms());
+           wheelPosition = 0;
+           wheelVelocity = wheelPosition - lastWheelPosition;
+           lastWheelPosition = wheelPosition;
+           xbee.printf("WheelVelocity: %i\n",wheelVelocity);
+            
+           if (abs(wheelVelocity) <= 20 && !stopped) { // Set new targetPosition if breaking
+               targetPosition = wheelPosition;
+               stopped = true;
+               xbee.printf("Stopped\n");
+           }
+        }
+        */
+      
+       
+        /* Check for incoming serial data */
+        if (ps3.readable()) // Check if there's any incoming data
+            receivePS3();
+        if (xbee.readable()) // For setting the PID values
+            receiveXbee();
+        
+        //Read battery voltage every 1s 
+        if (loopCounter == 100) {
+           loopCounter = 0; 
+            // Reset loop counter
+            //double analogVoltage = batteryVoltage.read()/1*3.3; // Convert to voltage
+            //analogVoltage *= 6.6; // The analog pin is connected to a 56k-10k voltage divider
+            //xbee.printf("analogVoltage: %f - timer: %i\n",analogVoltage,t.read_ms());
+            //if (analogVoltage < 9 && pitch > 60 && pitch < 120) // Set buzzer on, if voltage gets critical low
+            // buzzer = 1; // The mbed resets at aproximatly 1V           
+        }
+       
+         
+         
+        /* Use a time fixed loop*/
+        lastLoopUsefulTime = t.read_us() - loopStartTime;
+        
+        if (lastLoopUsefulTime < STD_LOOP_TIME)
+            wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
+            lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
+            loopStartTime = t.read_us();
+        
+       //debug.printf("%i,%i\n",lastLoopUsefulTime,lastLoopTime);
+       
+    }
+   
+}
+void PID(double restAngle, double offset) {
+
+    /* Steer robot */
+    if (steerForward) {
+        //offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
+        restAngle -= offset;
+        //xbee.printf("Forward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
+    } else if (steerBackward) {
+        //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
+        restAngle += offset;
+        //xbee.printf("Backward offset: %f\t WheelVelocity: %i\n",offset,wheelVelocity);
+    }
+ 
+    /* Break */
+    else if (steerStop) {
+        /*
+        long positionError = wheelPosition - targetPosition;
+        if (abs(positionError) > zoneA) // Inside zone A
+            restAngle -= (double)positionError/positionScaleA;
+        else if (abs(positionError) > zoneB) // Inside zone B
+            restAngle -= (double)positionError/positionScaleB;
+        else // Inside zone C
+            restAngle -= (double)positionError/positionScaleC;
+        
+        restAngle -= (double)wheelVelocity/velocityScaleStop;
+        */
+          //xbee.printf("restAngle: %f\n", restAngle);
+       /*
+        if (restAngle < 10) // Limit rest Angle
+            restAngle = 0;
+        else if (restAngle > 85)
+            restAngle = 60;
+      */
+        
+    }
+    /* Update PID values */
+    double error = (restAngle - pitch)/100; // was 100
+    double pTerm = Kp * error;
+    iTerm += Ki * error;
+    double dTerm = Kd * (error - lastError);
+    lastError = error;
+    
+    double PIDValue = pTerm + iTerm + dTerm;
+    
+    xbee.printf("Pitch: %5.2f\tPIDValue: %5.2f\tpTerm: %5.2f\tiTerm: %5.2f\tdTerm: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,pTerm,iTerm,dTerm,Kp,Ki,Kd);
+    
+    /* Steer robot sideways */
+    double PIDLeft;
+    double PIDRight;
+    if (steerLeft) {
+        PIDLeft = PIDValue-turnSpeed;
+        PIDRight = PIDValue+turnSpeed;
+    } else if (steerRotateLeft) {
+        PIDLeft = PIDValue-rotateSpeed;
+        PIDRight = PIDValue+rotateSpeed;
+    } else if (steerRight) {
+        PIDLeft = PIDValue+turnSpeed;
+        PIDRight = PIDValue-turnSpeed;
+    } else if (steerRotateRight) {
+        PIDLeft = PIDValue+rotateSpeed;
+        PIDRight = PIDValue-rotateSpeed;
+    } else {
+        PIDLeft = PIDValue;
+        PIDRight = PIDValue;
+    }
+    //PIDLeft *= 0.95; // compensate for difference in the motors
+    
+    /* Set PWM Values */
+    if (PIDLeft >= 0)
+        move(left, forward, PIDLeft);
+    else
+        move(left, backward, PIDLeft * -1);
+    if (PIDRight >= 0)
+        move(right, forward, PIDRight);
+    else
+        move(right, backward, PIDRight * -1);
+}
+void receivePS3() {
+    char input[16]; // The serial buffer is only 16 characters
+    int i = 0;
+    while (1) {
+        input[i] = ps3.getc();
+        if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
+            break;
+        i++;
+    }
+    debug.printf("Input PS3 Remote: %s\n",input);
+    
+    // Set all false
+    steerForward = false;
+    steerBackward = false;
+    steerStop = false;
+    steerLeft = false;
+    steerRotateLeft = false;
+    steerRight = false;
+    steerRotateRight = false;
+    
+    /* For remote control */
+    if (input[0] == 'F') { // Forward
+        strtok(input, ","); // Ignore 'F'
+        targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+        debug.printf("%f\n",targetOffset); // Print targetOffset for debugging
+        steerForward = true;
+    } else if (input[0] == 'B') { // Backward
+        strtok(input, ","); // Ignore 'B'
+        targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+        //xbee.printf("%f\n",targetOffset); // Print targetOffset for debugging
+        steerBackward = true;
+    } else if (input[0] == 'L') { // Left
+        if (input[1] == 'R') // Left Rotate
+            steerRotateLeft = true;
+        else
+            steerLeft = true;
+    } else if (input[0] == 'R') { // Right
+        if (input[1] == 'R') // Right Rotate
+            steerRotateRight = true;
+        else
+            steerRight = true;
+    } else if (input[0] == 'S') { // Stop
+        steerStop = true;
+        stopped = false;
+        //targetPosition = wheelPosition;
+    }
+    
+    else if (input[0] == 'T') { // Set the target angle
+        strtok(input, ","); // Ignore 'T'
+        targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+        xbee.printf("%f\n",targetAngle); // Print targetAngle for debugging
+    } else if (input[0] == 'A') { // Abort
+        stopAndReset();
+        while (ps3.getc() != 'C'); // Wait until continue is send
+    }
+}
+void receiveXbee() {
+    char input[16]; // The serial buffer is only 16 characters
+    int i = 0;
+    while (1) {
+        input[i] = xbee.getc();
+        if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
+            break;
+        i++;
+    }
+   xbee.printf("xBee Received Input: %s\n",input);
+    
+    if (input[0] == 'T') { // Set the target angle
+        strtok(input, ","); // Ignore 'T'
+        targetAngle = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+    } else if (input[0] == 'P') {
+        strtok(input, ",");//Ignore 'P'
+        Kp = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+    } else if (input[0] == 'I') {
+        strtok(input, ",");//Ignore 'I'
+        Ki = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+    } else if (input[0] == 'D') {
+        strtok(input, ",");//Ignore 'D'
+        Kd = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
+    } else if (input[0] == 'A') { // Abort
+        stopAndReset();
+        while (xbee.getc() != 'C'); // Wait until continue is send
+    } else if (input[0] == 'G') // The processing application sends this at startup
+        processing(); // Send output to processing application
+}
+void processing() {
+    /* Send output to processing application */
+    xbee.printf("Processing,%5.2f,%5.2f,%5.2f,%5.2f\n",Kp,Ki,Kd,targetAngle);
+  
+}
+void stopAndReset() {
+    stop(both);
+    lastError = 0;
+    iTerm = 0;
+    //targetPosition = wheelPosition;
+    //buzzer= 0;
+}
+/*
+double kalman(double newAngle, double newRate, double dtime) {
+    // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418 - http://www.x-firm.com/?page_id=145
+    // with slightly modifications by Kristian Lauszus
+    // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
+    dt = dtime / 1000000; // Convert from microseconds to seconds
+    
+    // Discrete Kalman filter time update equations - Time Update ("Predict")
+    // Update xhat - Project the state ahead
+    
+    angle += dt * (newRate - bias);
+    
+    //angle += dt * newRate;
+    
+    // Update estimation error covariance - Project the error covariance ahead
+    P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
+    P_01 += -dt * P_11;
+    P_10 += -dt * P_11;
+    P_11 += +Q_gyro * dt;
+    
+    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
+    // Calculate Kalman gain - Compute the Kalman gain
+    S = P_00 + R_angle;
+    K_0 = P_00 / S;
+    K_1 = P_10 / S;
+    
+    // Calculate angle and resting rate - Update estimate with measurement zk
+    y = newAngle - angle;
+    angle += K_0 * y;
+    bias += K_1 * y;
+    
+    // Calculate estimation error covariance - Update the error covariance
+    P_00 -= K_0 * P_00;
+    P_01 -= K_0 * P_01;
+    P_10 -= K_1 * P_00;
+    P_11 -= K_1 * P_01;
+    
+    return angle;
+}
+
+double getGyroYrate() {
+    // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3=0.001009091
+    double gyroRate = -((theRazor.getGyroY() - zeroValues[0]) / 0.001009091);
+    return gyroRate;
+}
+double getAccY() {
+    // (accAdc-accZero)/Sensitivity (In quids) - Sensitivity = 0.33/3.3=0.1
+    double accXval = (theRazor.getAccX() - zeroValues[1]) / 0.1;
+    double accYval = (theRazor.getAccY() - zeroValues[2]) / 0.1;
+    //accYval--;//-1g when lying down
+   
+    accYval++;// stading up
+   
+    double accZval = (theRazor.getAccZ() - zeroValues[3]) / 0.1;
+    
+    double R = sqrt(pow(accXval, 2) + pow(accYval, 2) + pow(accZval, 2)); // Calculate the force vector
+    double angleY = acos(accYval / R) * RAD_TO_DEG;
+    return angleY;
+}
+
+void calibrateSensors() {
+    LEDs = 0xF; // Turn all onboard LEDs on
+    
+    double adc[4] = {0,0,0,0};
+    for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
+        adc[0] += theRazor.getGyroY();
+        adc[1] += theRazor.getAccX();
+        adc[2] += theRazor.getAccY();
+        adc[3] += theRazor.getAccZ();
+        
+        wait_ms(10);
+    }
+    
+    zeroValues[0] = adc[0] / 100; // Gyro X-axis
+    zeroValues[1] = adc[1] / 100; // Accelerometer X-axis
+    zeroValues[2] = adc[2] / 100; // Accelerometer Y-axis
+    zeroValues[3] = adc[3] / 100; // Accelerometer Z-axis
+    
+    LEDs = 0x0; // Turn all onboard LEDs off
+}
+*/
+void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
+
+//xbee.printf("Motor actions (Motors, Direction, Speed):%f,%f,%f\n",motor,direction,speed);
+
+    if (motor == left) {
+        leftPWM = speed;
+        if (direction == forward) {
+            leftA = 0;
+            leftB = 1;
+        } else if (direction == backward) {
+            leftA = 1;
+            leftB = 0;
+        }
+    } else if (motor == right) {
+        rightPWM = speed;
+        if (direction == forward) {
+            rightA = 0;
+            rightB = 1;
+        } else if (direction == backward) {
+            rightA = 1;
+            rightB = 0;
+        }
+    } else if (motor == both) {
+        leftPWM = speed;
+        rightPWM = speed;
+        if (direction == forward) {
+            leftA = 0;
+            leftB = 1;
+            
+            rightA = 0;
+            rightB = 1;
+        } else if (direction == backward) {
+            leftA = 1;
+            leftB = 0;
+            
+            rightA = 1;
+            rightB = 0;
+        }
+    }
+}
+void stop(Motor motor) {
+    if (motor == left) {
+        leftPWM = 1;
+        leftA = 1;
+        leftB = 1;
+    } else if (motor == right) {
+        rightPWM = 1;
+        rightA = 1;
+        rightB = 1;
+    } else if (motor == both) {
+        leftPWM = 1;
+        leftA = 1;
+        leftB = 1;
+        
+        rightPWM = 1;
+        rightA = 1;
+        rightB = 1;
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 0150acbc6cf4 BalancingRobot.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BalancingRobot.h	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,127 @@
+#ifndef _balancingrobot_h_
+#define _balancingrobot_h_
+
+#define RAD_TO_DEG 57.295779513082320876798154814105 // 180/pi
+
+BusOut LEDs(LED1, LED2, LED3, LED4);
+
+
+/* 
+Motor A(p22, p6, p5, 1); // pwm, fwd, rev, can break
+Motor B(p21, p7, p8, 1); // pwm, fwd, rev, can break
+*/ 
+
+/* Left motor */
+DigitalOut leftA(p6);
+DigitalOut leftB(p5);
+PwmOut leftPWM(p22);
+
+/* Right motor */
+DigitalOut rightA(p7);
+DigitalOut rightB(p8);
+PwmOut rightPWM(p21);
+
+/* IMU 
+AnalogIn gyroY(p17);
+AnalogIn accX(p18);
+AnalogIn accY(p19);
+AnalogIn accZ(p20);
+*/
+/* NEW IMU */
+
+
+
+/* Battery voltage
+AnalogIn batteryVoltage(p15); // The analog pin is connected to a 56k-10k voltage divider
+DigitalOut buzzer(p5); // Connected to a BC547 transistor - there is a protection diode at the buzzer 
+ */
+// Zero voltage values for the sensors - [0] = gyroY, [1] = accX, [2] = accY, [3] = accZ
+double zeroValues[4];
+
+/* Kalman filter variables and constants */
+const float Q_angle = 0.001; // Process noise covariance for the accelerometer - Sw
+const float Q_gyro = 0.001; // Process noise covariance for the gyro - Sw
+const float R_angle = 0.02; // Measurement noise covariance - Sv
+
+double angle = 0; // It starts at 180 degrees
+double bias = 0;
+double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
+double dt, y, S;
+double K_0, K_1;
+
+// Results
+double accYangle;
+double gyroYrate;
+double pitch;
+
+/* PID variables */
+double Kp = 0; //11 - 7
+double Ki = 0; //2
+double Kd = 0; //12
+double targetAngle = 0;
+
+double lastError;
+double iTerm;
+
+/* Used for timing */
+long timer;
+
+const long STD_LOOP_TIME = 10*1000; // Fixed time loop of 10 milliseconds
+long lastLoopTime = STD_LOOP_TIME;
+long lastLoopUsefulTime = STD_LOOP_TIME;
+long loopStartTime;
+
+enum Motor {
+    left,
+    right,
+    both,
+};
+enum Direction {
+    forward,
+    backward,
+};
+
+bool steerForward;
+bool steerBackward;
+bool steerStop = true; // Stop by default
+bool steerLeft;
+bool steerRotateLeft;
+bool steerRight;
+bool steerRotateRight;
+
+bool stopped;
+
+const double turnSpeed = 0.1;
+const double rotateSpeed = 0.2;
+
+double targetOffset = 0;
+
+uint8_t loopCounter = 0; // Used for wheel velocity
+
+long wheelPosition;
+long lastWheelPosition;
+long wheelVelocity;
+long targetPosition;
+int zoneA = 2000; // 2000
+int zoneB = 1000; // 1000
+/*
+double positionScaleA = 250; // one resolution is 464 pulses
+double positionScaleB = 500; 
+double positionScaleC = 1000;
+double velocityScaleMove = 40;
+double velocityScaleStop = 30;//30 - 40 - 60
+*/
+void calibrateSensors();
+void PID(double restAngle, double offset);
+double kalman(double newAngle, double newRate, double looptime);
+double getGyroYrate();
+double getAccY();
+
+void move(Motor motor, Direction direction, float speed);
+void stop(Motor motor);
+void processing();
+void receivePS3();
+void receiveXbee();
+void stopAndReset();
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 0150acbc6cf4 dof9RazorImuAhrs.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dof9RazorImuAhrs.cpp	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,119 @@
+//****************************************************************************/
+// Description:
+//
+//  Read attitude and heading reference system [AHRS] data from the SparkFun
+//  9DOF Razor Inertial Measurement Unit [IMU].
+//
+// AHRS code:
+//
+//  http://code.google.com/p/sf9domahrs/
+//
+// 9DOF Razor IMU:
+//
+//  http://www.sparkfun.com/commerce/product_info.php?products_id=9623
+//****************************************************************************/
+
+//****************************************************************************/
+// Includes
+//****************************************************************************/
+#include "dof9RazorImuAhrs.h"
+
+dof9RazorImuAhrs::dof9RazorImuAhrs(PinName tx, PinName rx) {
+
+    razor = new Serial(tx, rx);
+    razor->baud(BAUD_RATE);
+
+}
+
+void dof9RazorImuAhrs::update(void) {
+
+    //Make sure we get to the start of a line.
+    while (razor->getc() != '\n');
+
+#if PRINT_EULER == 1
+    // OLD
+    //razor->scanf("!ANG:%f,%f,%f", &roll, &pitch, &yaw);
+    razor->scanf("#YPR=%f,%f,%f", &roll, &pitch, &yaw);
+    // JAOL NEW
+    //razor->scanf("#AG=%f,%f,%f,%f",  &acc_x, &acc_y, &acc_z, &gyro_y);
+#endif
+
+#if PRINT_ANALOGS == 1
+    razor->scanf(",AN:%f,%f,%f", &gyro_x, &gyro_y, &gyro_z);
+    razor->scanf(",%f,%f,%f", &acc_x, &acc_y, &acc_z);
+    razor->scanf(",%f,%f,%f", &mag_x, &mag_y, &mag_z);
+#endif
+
+}
+
+float dof9RazorImuAhrs::getRoll(void){
+
+    return roll;
+
+}
+
+float dof9RazorImuAhrs::getPitch(void){
+
+    return pitch;
+
+}
+
+float dof9RazorImuAhrs::getYaw(void){
+
+    return yaw;
+
+}
+
+float dof9RazorImuAhrs::getGyroX(void){
+
+    return gyro_x;
+
+}
+
+float dof9RazorImuAhrs::getGyroY(void){
+
+    return gyro_y;
+
+}
+
+float dof9RazorImuAhrs::getGyroZ(void){
+
+    return gyro_z;
+
+}
+
+float dof9RazorImuAhrs::getAccX(void){
+
+    return acc_x;
+
+}
+
+float dof9RazorImuAhrs::getAccY(void){
+
+    return acc_x;
+
+}
+
+float dof9RazorImuAhrs::getAccZ(void){
+
+    return acc_z;
+
+}
+
+float dof9RazorImuAhrs::getMagX(void){
+
+    return mag_x;
+
+}
+
+float dof9RazorImuAhrs::getMagY(void){
+
+    return mag_y;
+
+}
+
+float dof9RazorImuAhrs::getMagZ(void){
+
+    return mag_z;
+
+}

diff -r 000000000000 -r 0150acbc6cf4 dof9RazorImuAhrs.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dof9RazorImuAhrs.h	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,90 @@
+//****************************************************************************/
+// Description:
+//
+//  Read attitude and heading reference system [AHRS] data from the SparkFun
+//  9DOF Razor Inertial Measurement Unit [IMU].
+//
+// AHRS code:
+//
+//  http://code.google.com/p/sf9domahrs/
+//
+// 9DOF Razor IMU:
+//
+//  http://www.sparkfun.com/commerce/product_info.php?products_id=9623
+//****************************************************************************/
+
+#ifndef DOF9_RAZOR_IMU_H
+#define DOF9_RAZOR_IMU_H
+
+//****************************************************************************/
+// Includes
+//****************************************************************************/
+#include "mbed.h"
+
+//****************************************************************************/
+// Defines
+//****************************************************************************/
+#define PRINT_EULER   1     //Corrected heading data.
+#define PRINT_ANALOGS 0     //Razor spits out raw gyro/accelerometer/magneto
+                            //data.
+                            //Set as a define when compiling AHRS code.
+#define BAUD_RATE     57600 //Default in AHRS code.
+
+class dof9RazorImuAhrs {
+
+public:
+
+    /**
+     * Constructor.
+     *
+     * Parameters:
+     *
+     *  tx - Pin to use for Serial transmission.
+     *  rx - Pin to use for Serial receive.
+     */
+    dof9RazorImuAhrs(PinName tx, PinName rx);
+    
+    /**
+     * Update all of the heading data.
+     */
+    void update(void);
+    
+    float getRoll(void);
+    float getPitch(void);
+    float getYaw(void);
+    
+    float getGyroX(void);
+    float getGyroY(void);
+    float getGyroZ(void);
+    
+    float getAccX(void);
+    float getAccY(void);
+    float getAccZ(void);
+    
+    float getMagX(void);
+    float getMagY(void);
+    float getMagZ(void);
+
+private:
+
+    Serial* razor;
+
+    float roll;
+    float pitch;
+    float yaw;
+    
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    
+    float acc_x;
+    float acc_y;
+    float acc_z;
+    
+    float mag_x;
+    float mag_y;
+    float mag_z;
+
+};
+
+#endif /* DOF9_RAZOR_IMU_H */

diff -r 000000000000 -r 0150acbc6cf4 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 18 06:00:26 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479