Revision 1:dacf7db790f6, committed 2014-04-30

Comitter:

Strikewolf

Date:

Wed Apr 30 05:54:17 2014 +0000

Parent:

0:84d5aa80fd77

Child:

2:188b0fbf3b43

Commit message:

works without any motors

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.lib	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/peterswanson87/code/ADXL345_I2C/#a7184ee0a913

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GameCode.h	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,20 @@
+#define THRESHOLD 100
+
+Serial pc(USBTX, USBRX);
+
+bool gameOver = false;
+
+bool isGameOver(short x_hum, short y_hum, short x_cpu, short y_cpu)
+{
+    if(abs(x_hum - x_cpu) < THRESHOLD && abs(y_hum - y_cpu) < THRESHOLD) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void endGame()
+{
+    pc.printf("GAME OVER\n\r");
+    exit(1);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Humain.cpp	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,42 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "IMU_RPY.h"
+#include "GameCode.h"
+#include "HumanPosition.h"
+#include "HumanControl.h"
+
+int main()
+{
+    pc.printf("\n\rStartup...\r\n");
+
+    //Reset xbee
+    pc.printf("Resetting XBee...\r\n");
+    rst1 = 0;
+    wait_us(1);
+    rst1 = 1;
+    wait_us(1);
+
+    //Initialize IMU and filter
+    pc.printf("Initializing IMU...\r\n");
+    imuFilter.reset();
+    rpy_init();
+
+    // wait for AI car to get away from user
+    pc.printf("Waiting for AI car...\n\r");
+    wait(10);
+
+    // start positioning system
+    pc.printf("Starting Positioning System...\n\r");
+    Thread position(PositionSystemMain, NULL, osPriorityRealtime, 256 * 4);
+    wait(3); // wait for the mouse sensor to boot up
+
+    // give control to radio driver while game is running
+    pc.printf("Running!\n");
+    Timer sendTimer;
+    sendTimer.start();
+    while(!gameOver) {
+        radioDrive();
+        if(sendTimer.read_ms() % 250)
+            sendPosition(NULL);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HumanControl.h	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,178 @@
+//Radio Calibration Signals - Need to implement calibration routine...
+#define CHAN2_MID 1470  //Elevator Midpoint
+#define CHAN1_MID 1470  //Rudder Midpoint
+#define DEADZONE 105
+#define MAXMIN_OFFSET 400
+
+//Drive Modes
+typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
+DRIVEMODE mode;
+float xpwm, ypwm;
+
+//H-bridge
+PwmOut rightMotorPWM(p22);  //Channel A
+PwmOut leftMotorPWM(p21);   //Channel B
+DigitalOut leftMotor1(p23);
+DigitalOut leftMotor2(p24);
+DigitalOut rightMotor1(p25);
+DigitalOut rightMotor2(p26);
+
+//Radio
+DigitalIn rudder(p11);
+DigitalIn elevator(p12);
+
+void setTurnLeft(float speed)
+{
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+
+    //Set motor function
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+void setTurnRight(float speed)
+{
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+
+    //Set motor function
+    leftMotor1 = 1;
+    leftMotor2 = 0;
+    rightMotor1 = 0;
+    rightMotor2 = 1;
+}
+
+//Stop with braking
+void stop()
+{
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    leftMotor1 = 0;
+    leftMotor2 = 1;
+    rightMotor1 = 1;
+    rightMotor2 = 0;
+}
+
+
+int getRadioX()
+{
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!rudder && !gameOver);
+    timer.start();
+    while(rudder && !gameOver);
+    dur = timer.read_us();
+    return dur;
+}
+
+int getRadioY()
+{
+    Timer timer;
+    timer.reset();
+    int dur;
+    while(!elevator && !gameOver);
+    timer.start();
+    while(elevator && !gameOver);
+    dur = timer.read_us();
+    return dur;
+}
+
+//Primary function for setting motors for radio driving
+void radioDrive()
+{   
+    __disable_irq(); // disable interrupts
+    int radioY = getRadioY();
+    int radioX = getRadioX();
+    __enable_irq(); // enable interrupts
+
+    //Identify drive mode from radio values
+    if (radioY < CHAN2_MID - DEADZONE)
+        mode = FORWARD;
+    if (radioY > CHAN2_MID + DEADZONE)
+        mode = REVERSE;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+            radioX < CHAN1_MID - DEADZONE)
+        mode = CENTERLEFT;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+            radioX > CHAN1_MID + DEADZONE)
+        mode = CENTERRIGHT;
+    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
+            radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
+        mode = STOP;
+
+    //Normalize values for PWM magnitude
+    xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
+    ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
+
+    //Set Motors Accordingly
+    switch(mode) {
+        case FORWARD:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = 0.2;
+                leftMotorPWM = 0.85;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = 0.85;
+                leftMotorPWM = 0.2;
+            } else {
+                rightMotorPWM = .85;
+                leftMotorPWM = .85;
+            }
+            leftMotor1 =  0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+
+        case REVERSE:
+            //Handle variable turns
+            if (radioX > CHAN1_MID + DEADZONE) {
+                rightMotorPWM = 0.2;
+                leftMotorPWM = 0.85;
+            } else if (radioX < CHAN1_MID - DEADZONE) {
+                rightMotorPWM = 0.85;
+                leftMotorPWM = 0.2;
+            } else {
+                rightMotorPWM = .85;
+                leftMotorPWM = .85;
+            }
+            leftMotor1 =  1;
+            leftMotor2 = 0;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+
+        case CENTERRIGHT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 1;
+            leftMotor2 = 0;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+            break;
+
+        case CENTERLEFT:
+            rightMotorPWM = xpwm;
+            leftMotorPWM = xpwm;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+
+        case STOP:
+            rightMotorPWM = 0;
+            leftMotorPWM = 0;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 1;
+            rightMotor2 = 0;
+            break;
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HumanPosition.h	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,146 @@
+#include <math.h>
+#include "USBHostMouse.h"
+
+#define PFLAG_ON 0
+#define PFLAG_OFF 1
+#define PFLAG_CALIB 2
+
+#define SERIAL_BUFFER_SIZE 20
+#define PACKET_SIZE 10
+
+#define X_HUMAN_UPPER 1
+#define X_HUMAN_LOWER 2
+#define Y_HUMAN_UPPER 4
+#define Y_HUMAN_LOWER 5
+
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+
+//Quadrant enum
+typedef enum {FORWARD_LEFT, FORWARD_RIGHT, BACKWARD_LEFT, BACKWARD_RIGHT} QUADRANT;
+
+// updates xy position if on, does nothing if off
+extern char PFlag = PFLAG_ON;
+
+// variables to keep track of coordinate position
+double x_position = 0;
+double y_position = 0;
+
+// human position
+Serial xbee(p13, p14);
+DigitalOut rst1(p15);
+
+/* mouse event handler */
+void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z)
+{
+    // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
+    double y_temp = y_mickey / 232.6 * 100;
+    double g_temp = imuFilter.getYaw();
+
+    // determine direction we are facing and add to that direction
+    //y_position += y_temp;
+    //x_position += y_temp * tan(g_temp);
+    double dx = 0;
+    double dy = 0;
+    QUADRANT quadrant;
+    if (toDegrees(g_temp) > 0 && toDegrees(g_temp) <= 90)
+        quadrant = FORWARD_LEFT;
+    if (toDegrees(g_temp) < 0 && toDegrees(g_temp) >= -90)
+        quadrant = FORWARD_RIGHT;
+    if (toDegrees(g_temp) > 90 && toDegrees(g_temp) <= 180)
+        quadrant = BACKWARD_LEFT;
+    if (toDegrees(g_temp) < -90 && toDegrees(g_temp) >= -180)
+        quadrant = BACKWARD_RIGHT;
+
+    switch (quadrant) {
+        case FORWARD_LEFT:
+            dy = y_temp * cos(g_temp);
+            dx = -y_temp * sin(g_temp);
+            break;
+        case FORWARD_RIGHT:
+            dy = y_temp * cos(g_temp);
+            dx = -y_temp * sin(g_temp);
+            break;
+        case BACKWARD_LEFT:
+            dy = -y_temp * sin(g_temp);
+            dx = y_temp * cos(g_temp);
+            break;
+        case BACKWARD_RIGHT:
+            dy = y_temp * sin(g_temp);
+            dx = -y_temp * cos(g_temp);
+            break;
+    }
+
+    x_position += dx;
+    y_position += dy;
+
+    // pc.printf("sin(g): %f, cos(g): %f\n\r", sin(g_temp), cos(g_temp));
+    // pc.printf("DEBUG: dx: %f, dy: %f, gyro: %f, quadrant: %d\n\r", dx, dy, toDegrees(g_temp), quadrant);
+    // pc.printf("x: %f, y: %f, dx: %f, dy:  %f, g: %f, q: %d\n\r", x_position, y_position, dx, dy, toDegrees(g_temp), quadrant);
+}
+
+/* positioning system thread function */
+void PositionSystemMain(void const *)
+{
+
+    USBHostMouse mouse;
+
+    while(!gameOver) {
+        // try to connect a USB mouse
+        while(!mouse.connect() && !gameOver)
+            Thread::wait(500);
+
+        // when connected, attach handler called on mouse event
+        mouse.attachEvent(onMouseEvent);
+
+        // wait until the mouse is disconnected
+        while(mouse.connected() && !gameOver)
+            Thread::wait(500);
+    }
+}
+
+void sendPosition(void const *)
+{
+    int x = 0;
+    int y = 0;
+    char a = 0;
+    char b = 0;
+    char c = 0;
+    char d = 0;
+
+    // sample current xy position
+    x = (int) x_position;
+    y = (int) y_position;
+
+    // send current xy position to cpu bot
+    pc.printf("Sending: %d %d...\n\r", x, y);
+    a = x >> 24;
+    b = x >> 16;
+    c = x >> 8;
+    d = x;
+    // pc.printf("\ta: %x b: %x\n\r", a, b);
+    xbee.putc('x');
+    xbee.putc(a);   // send upper bits
+    xbee.putc(b);   // send lower bits
+    xbee.putc(c);
+    xbee.putc(d);
+
+    a = y >> 24;
+    b = y >> 16;
+    c = y >> 8;
+    d = y;
+    //pc.printf("\ta: %x b: %x\n\r", a, b);
+    xbee.putc('y');
+    xbee.putc(a);   // send upper bits
+    xbee.putc(b);   // send lower bits
+    xbee.putc(c);
+    xbee.putc(d);
+
+    pc.printf("Send complete.\n\r");
+    wait(0.25);
+    if(xbee.readable() && xbee.getc() == 'd') {
+        gameOver = true;
+        pc.printf("gameOver Received!\n\r");
+        endGame();
+    }
+}
\ No newline at end of file

--- a/IMU_RPY.h	Sun Apr 27 04:31:07 2014 +0000
+++ b/IMU_RPY.h	Wed Apr 30 05:54:17 2014 +0000
@@ -1,281 +1,279 @@
-    
+/**
+ * IMU filter example.
+ *
+ * Calculate the roll, pitch and yaw angles.
+ */
+#include "IMUfilter.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+
+//Gravity at Earth's surface in m/s/s
+#define g0 9.812865328
+//Number of samples to average.
+#define SAMPLES 4
+//Number of samples to be averaged for a null bias calculation
+//during calibration.
+#define CALIBRATION_SAMPLES 32
+//Convert from radians to degrees.
+#define toDegrees(x) (x * 57.2957795)
+//Convert from degrees to radians.
+#define toRadians(x) (x * 0.01745329252)
+//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
+#define GYROSCOPE_GAIN (1 / 14.375)
+//Full scale resolution on the ADXL345 is 4mg/LSB.
+#define ACCELEROMETER_GAIN (0.004 * g0)
+//Sampling gyroscope at 200Hz.
+#define GYRO_RATE   0.005
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE    0.005
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.1
+
+//At rest the gyroscope is centred around 0 and goes between about
+//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+//5/15 = 0.3 degrees/sec.
+IMUfilter imuFilter(FILTER_RATE, 0.3);
+ADXL345_I2C accelerometer(p9, p10);
+ITG3200 gyroscope(p9, p10);
+Ticker accelerometerTicker;
+Ticker gyroscopeTicker;
+Ticker filterTicker;
+
+//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;
+
+//Offsets for the accelerometer.
+//Same as with the gyroscope.
+double a_xBias;
+double a_yBias;
+double a_zBias;
 
-    /**
-     * IMU filter example.
-     *
-     * Calculate the roll, pitch and yaw angles.
-     */
-    #include "IMUfilter.h"
-    #include "ADXL345_I2C.h"
-    #include "ITG3200.h"
-     
-    //Gravity at Earth's surface in m/s/s
-    #define g0 9.812865328
-    //Number of samples to average.
-    #define SAMPLES 4
-    //Number of samples to be averaged for a null bias calculation
-    //during calibration.
-    #define CALIBRATION_SAMPLES 128
-    //Convert from radians to degrees.
-    #define toDegrees(x) (x * 57.2957795)
-    //Convert from degrees to radians.
-    #define toRadians(x) (x * 0.01745329252)
-    //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
-    #define GYROSCOPE_GAIN (1 / 14.375)
-    //Full scale resolution on the ADXL345 is 4mg/LSB.
-    #define ACCELEROMETER_GAIN (0.004 * g0)
-    //Sampling gyroscope at 200Hz.
-    #define GYRO_RATE   0.005
-    //Sampling accelerometer at 200Hz.
-    #define ACC_RATE    0.005
-    //Updating filter at 40Hz.
-    #define FILTER_RATE 0.1
-     
-    //At rest the gyroscope is centred around 0 and goes between about
-    //-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
-    //5/15 = 0.3 degrees/sec.
-    IMUfilter imuFilter(FILTER_RATE, 0.3);
-    ADXL345_I2C accelerometer(p28, p27);
-    ITG3200 gyroscope(p28, p27);
-    Ticker accelerometerTicker;
-    Ticker gyroscopeTicker;
-    Ticker filterTicker;
-     
-    //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;
-     
-    //Offsets for the accelerometer.
-    //Same as with the gyroscope.
-    double a_xBias;
-    double a_yBias;
-    double a_zBias;
-     
-    //Accumulators used for oversampling and then averaging.
-    volatile double a_xAccumulator = 0;
-    volatile double a_yAccumulator = 0;
-    volatile double a_zAccumulator = 0;
-    volatile double w_xAccumulator = 0;
-    volatile double w_yAccumulator = 0;
-    volatile double w_zAccumulator = 0;
-     
-    //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];
-    //Number of accelerometer samples we're on.
-    int accelerometerSamples = 0;
-    //Number of gyroscope samples we're on.
-    int gyroscopeSamples = 0;
-     
-    /**
-     * Prototypes
-     */
-    //Set up the ADXL345 appropriately.
-    void initializeAcceleromter(void);
-    //Calculate the null bias.
-    void calibrateAccelerometer(void);
-    //Take a set of samples and average them.
-    void sampleAccelerometer(void);
-    //Set up the ITG3200 appropriately.
-    void initializeGyroscope(void);
-    //Calculate the null bias.
-    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);
-     
-    void 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 sampleAccelerometer(void)
-    {
-     
-        //Have we taken enough samples?
-        if (accelerometerSamples == SAMPLES) {
-     
-            //Average the samples, remove the bias, and calculate the acceleration
-            //in m/s/s.
-            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;
-     
-        } else {
-            //Take another sample.
-            accelerometer.getOutput(readings);
-     
-            a_xAccumulator += (int16_t) readings[0];
-            a_yAccumulator += (int16_t) readings[1];
-            a_zAccumulator += (int16_t) readings[2];
-     
-            accelerometerSamples++;
-     
-        }
-     
-    }
-     
-    void calibrateAccelerometer(void)
-    {
-     
+//Accumulators used for oversampling and then averaging.
+volatile double a_xAccumulator = 0;
+volatile double a_yAccumulator = 0;
+volatile double a_zAccumulator = 0;
+volatile double w_xAccumulator = 0;
+volatile double w_yAccumulator = 0;
+volatile double w_zAccumulator = 0;
+
+//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];
+//Number of accelerometer samples we're on.
+int accelerometerSamples = 0;
+//Number of gyroscope samples we're on.
+int gyroscopeSamples = 0;
+
+/**
+ * Prototypes
+ */
+//Set up the ADXL345 appropriately.
+void initializeAcceleromter(void);
+//Calculate the null bias.
+void calibrateAccelerometer(void);
+//Take a set of samples and average them.
+void sampleAccelerometer(void);
+//Set up the ITG3200 appropriately.
+void initializeGyroscope(void);
+//Calculate the null bias.
+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);
+
+void 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 sampleAccelerometer(void)
+{
+
+    //Have we taken enough samples?
+    if (accelerometerSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the acceleration
+        //in m/s/s.
+        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;
-     
-        //Take a number of readings and average them
-        //to calculate the zero g offset.
-        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(ACC_RATE);
-     
-        }
-     
-        a_xAccumulator /= CALIBRATION_SAMPLES;
-        a_yAccumulator /= CALIBRATION_SAMPLES;
-        a_zAccumulator /= CALIBRATION_SAMPLES;
-     
-        //At 4mg/LSB, 250 LSBs is 1g.
-        a_xBias = a_xAccumulator;
-        a_yBias = a_yAccumulator;
-        a_zBias = (a_zAccumulator - 250);
-     
-        a_xAccumulator = 0;
-        a_yAccumulator = 0;
-        a_zAccumulator = 0;
-     
+        accelerometerSamples = 0;
+
+    } else {
+        //Take another sample.
+        accelerometer.getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+
+    }
+
+}
+
+void calibrateAccelerometer(void)
+{
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    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(ACC_RATE);
+
     }
-     
-    void initializeGyroscope(void)
-    {
-     
-        //Low pass filter bandwidth of 42Hz.
-        gyroscope.setLpBandwidth(LPFBW_42HZ);
-        //Internal sample rate of 200Hz. (1kHz / 5).
-        gyroscope.setSampleRateDivider(4);
-     
+
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+}
+
+void initializeGyroscope(void)
+{
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope.setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz. (1kHz / 5).
+    gyroscope.setSampleRateDivider(4);
+
+}
+
+void calibrateGyroscope(void)
+{
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the gyroscope bias offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+        wait(GYRO_RATE);
+
     }
-     
-    void calibrateGyroscope(void)
-    {
-     
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void sampleGyroscope(void)
+{
+
+    //Have we taken enough samples?
+    if (gyroscopeSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the angular
+        //velocity in rad/s.
+        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;
-     
-        //Take a number of readings and average them
-        //to calculate the gyroscope bias offset.
-        for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
-     
-            w_xAccumulator += gyroscope.getGyroX();
-            w_yAccumulator += gyroscope.getGyroY();
-            w_zAccumulator += gyroscope.getGyroZ();
-            wait(GYRO_RATE);
-     
-        }
-     
-        //Average the samples.
-        w_xAccumulator /= CALIBRATION_SAMPLES;
-        w_yAccumulator /= CALIBRATION_SAMPLES;
-        w_zAccumulator /= CALIBRATION_SAMPLES;
-     
-        w_xBias = w_xAccumulator;
-        w_yBias = w_yAccumulator;
-        w_zBias = w_zAccumulator;
-     
-        w_xAccumulator = 0;
-        w_yAccumulator = 0;
-        w_zAccumulator = 0;
-     
+        gyroscopeSamples = 0;
+
+    } else {
+        //Take another sample.
+        w_xAccumulator += gyroscope.getGyroX();
+        w_yAccumulator += gyroscope.getGyroY();
+        w_zAccumulator += gyroscope.getGyroZ();
+
+        gyroscopeSamples++;
+
     }
-     
-    void sampleGyroscope(void)
-    {
-     
-        //Have we taken enough samples?
-        if (gyroscopeSamples == SAMPLES) {
-     
-            //Average the samples, remove the bias, and calculate the angular
-            //velocity in rad/s.
-            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;
-     
-        } else {
-            //Take another sample.
-            w_xAccumulator += gyroscope.getGyroX();
-            w_yAccumulator += gyroscope.getGyroY();
-            w_zAccumulator += gyroscope.getGyroZ();
-     
-            gyroscopeSamples++;
-     
-        }
-     
-    }
-     
-    void 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 rpy_init()
-    {
-        //Initialize inertial sensors.
-        initializeAccelerometer();
-        calibrateAccelerometer();
-        initializeGyroscope();
-        //calibrateGyroscope();
-     
-     
-       /* //Set up timers.
-        //Accelerometer data rate is 200Hz, so we'll sample at this speed.
-        accelerometerTicker.attach(&sampleAccelerometer, 0.005);
-        //Gyroscope data rate is 200Hz, so we'll sample at this speed.
-        gyroscopeTicker.attach(&sampleGyroscope, 0.005);
-        //Update the filter variables at the correct rate.
-        filterTicker.attach(&filter, FILTER_RATE);*/
-    }
+
+}
+
+void 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 rpy_init()
+{
+    //Initialize inertial sensors.
+    initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
+    calibrateGyroscope();
+
+
+    //Set up timers.
+    //Accelerometer data rate is 200Hz, so we'll sample at this speed.
+    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+    //Gyroscope data rate is 200Hz, so we'll sample at this speed.
+    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+    //Update the filter variables at the correct rate.
+    filterTicker.attach(&filter, FILTER_RATE);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ITG3200/#4803de31e650

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHost.lib	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ganeshsubram1/code/USBHost/#b010bddab593

--- a/main.cpp	Sun Apr 27 04:31:07 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,203 +0,0 @@
-#include "mbed.h"
-
-//Radio Calibration Signals - Need to implement calibration routine...
-#define CHAN2_MID 1470  //Elevator Midpoint
-#define CHAN1_MID 1470  //Rudder Midpoint
-#define DEADZONE 105
-#define MAXMIN_OFFSET 400
-
-//Drive Modes
-typedef enum {FORWARD, REVERSE, STOP, CENTERLEFT, CENTERRIGHT} DRIVEMODE;
-DRIVEMODE mode;
-float xpwm, ypwm;
-
-//H-bridge
-PwmOut rightMotorPWM(p22);  //Channel A
-PwmOut leftMotorPWM(p21);   //Channel B
-DigitalOut leftMotor1(p23);
-DigitalOut leftMotor2(p24);
-DigitalOut rightMotor1(p25);
-DigitalOut rightMotor2(p26);
-
-//Radio 
-Serial pc(USBTX, USBRX);
-DigitalIn rudder(p11);
-DigitalIn elevator(p12);
-
-//Xbee
-Serial xbee(p13, p14);
-DigitalOut xbeeReset(p15);
-
-void setTurnLeft(float speed) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = 0;
-    leftMotor2 = 1;
-    rightMotor1 = 1;
-    rightMotor2 = 0;
-}
-
-void setTurnRight(float speed) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = 1;
-    leftMotor2 = 0;
-    rightMotor1 = 0;
-    rightMotor2 = 1;
-}
-
-//Stop with braking
-void stop() {
-    rightMotorPWM = 0;
-    leftMotorPWM = 0;
-    leftMotor1 = 0;
-    leftMotor2 = 1;
-    rightMotor1 = 1;
-    rightMotor2 = 0;
-}
-
-
-int getRadioX() {
-    Timer timer;
-    timer.reset();
-    int dur;
-    while(!rudder);
-    timer.start();
-    while(rudder);
-    dur = timer.read_us();
-    return dur;
-}
-
-int getRadioY() {
-    Timer timer;
-    timer.reset();
-    int dur;
-    while(!elevator);
-    timer.start();
-    while(elevator);
-    dur = timer.read_us();
-    return dur;
-}
-
-//Primary function for setting motors for radio driving
-void radioDrive() {
-    int radioY = getRadioY();
-    int radioX = getRadioX();
-    
-    //Identify drive mode from radio values
-    if (radioY < CHAN2_MID - DEADZONE)
-        mode = FORWARD;
-    if (radioY > CHAN2_MID + DEADZONE)
-        mode = REVERSE;
-    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
-        radioX < CHAN1_MID - DEADZONE)
-        mode = CENTERLEFT;
-    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
-        radioX > CHAN1_MID + DEADZONE)
-        mode = CENTERRIGHT;        
-    if (radioY < CHAN2_MID + DEADZONE && radioY > CHAN2_MID - DEADZONE &&
-        radioX < CHAN1_MID + DEADZONE && radioX > CHAN1_MID - DEADZONE)
-        mode = STOP;
-        
-    //Normalize values for PWM magnitude
-    xpwm = abs((float)radioX - CHAN1_MID) / MAXMIN_OFFSET;
-    ypwm = abs((float)radioY - CHAN2_MID) / MAXMIN_OFFSET;
-        
-    //Set Motors Accordingly
-    switch(mode) {
-        case FORWARD:
-            //Handle variable turns
-            if (radioX > CHAN1_MID + DEADZONE) {
-                rightMotorPWM = ypwm - 0.8*xpwm;
-                leftMotorPWM = ypwm;
-            } else if (radioX < CHAN1_MID - DEADZONE) {
-                rightMotorPWM = ypwm;
-                leftMotorPWM = ypwm - 0.8*xpwm;
-            } else {
-                rightMotorPWM = ypwm;
-                leftMotorPWM = ypwm; 
-            }
-            leftMotor1 =  0;
-            leftMotor2 = 1;
-            rightMotor1 = 0;
-            rightMotor2 = 1;
-            break;
-        
-        case REVERSE:
-            //Handle variable turns
-            if (radioX > CHAN1_MID + DEADZONE) {
-                rightMotorPWM = ypwm - 0.8*xpwm;
-                leftMotorPWM = ypwm;
-            } else if (radioX < CHAN1_MID - DEADZONE) {
-                rightMotorPWM = ypwm;
-                leftMotorPWM = ypwm - 0.8*xpwm;
-            } else {
-                rightMotorPWM = ypwm;
-                leftMotorPWM = ypwm; 
-            }
-            leftMotor1 =  1;
-            leftMotor2 = 0;
-            rightMotor1 = 1;
-            rightMotor2 = 0;
-            break;
-            
-        case CENTERRIGHT:
-            rightMotorPWM = xpwm;
-            leftMotorPWM = xpwm;
-            leftMotor1 = 1;
-            leftMotor2 = 0;
-            rightMotor1 = 0;
-            rightMotor2 = 1;
-            break;
-
-        case CENTERLEFT:
-            rightMotorPWM = xpwm;
-            leftMotorPWM = xpwm;
-            leftMotor1 = 0;
-            leftMotor2 = 1;
-            rightMotor1 = 1;
-            rightMotor2 = 0;
-            break;
-
-        case STOP:
-            rightMotorPWM = 0;
-            leftMotorPWM = 0;
-            leftMotor1 = 0;
-            leftMotor2 = 1;
-            rightMotor1 = 1;
-            rightMotor2 = 0;
-            break;
-    }
-}
-
-//Transmit position and heading
-void xbeeTelemetry() {
-    xbee.printf("Human Update \r\n");   
-    wait_ms(10);
-}
-
-bool isGameOver() {
-    return xbee.readable();
-}
-        
-
-
-int main() {
-    
-    //Init Xbee
-    xbeeReset = 0;
-    wait_ms(1);
-    xbeeReset = 1;
-    wait_ms(1);
-    
-    while(1) {
-        xbee.printf("HELLO!\r\n");
-        wait(0.1);
-    }
-}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Wed Apr 30 05:54:17 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#4ef72665e2c8