Revision 5:210cd333f770, committed 2014-04-30

Comitter:

Strikewolf

Date:

Wed Apr 30 05:53:51 2014 +0000

Parent:

4:b2a30c313297

Child:

6:3fb9f96765f6

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:53:51 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/peterswanson87/code/ADXL345_I2C/#7e41b9136e7a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AI.cpp	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,53 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "IMU_RPY.h"
+#include "GameCode.h"
+#include "AIPosition.h"
+#include "AIControl.h"
+
+InterruptIn sw(p30);
+
+void StopISR()
+{
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    exit(1);
+}
+
+int main()
+{
+    pc.printf("\n\rStartup...\r\n");
+
+    //Emergency stop mechanism
+    sw.rise(&StopISR);
+
+    //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();
+
+    // 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
+
+    // get away from user before starting game
+    pc.printf("Getting head start (straight)...\n\r");
+    wait(5);
+    // gyroDriveStraight(0.7, false, 10000);
+    // pc.printf("Getting head start (turn right)...\n\r");
+    // centerTurnRight(90);
+    
+    // bursts of 10 seconds for driving straight
+    pc.printf("Running!\r\n");
+    while(!gameOver) {
+       gyroDriveStraight(0.7, true, 10000);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AIControl.h	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,232 @@
+//*******************************************
+//* Robot motor control and drive functions *
+//*******************************************
+
+#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
+
+//Radio Calibration Signals - Need to implement calibration routine...
+#define CHAN3_MAX 2060  //Throttle Hi
+#define CHAN3_MIN 1150 ` //Throttle Lo
+#define CHAN2_MAX 1260  //Elevator Hi
+#define CHAN2_MIN 2290  //Elevator Lo
+#define CHAN1_MAX 2160  //Rudder Hi
+#define CHAN1_MIN 1210  //Rudder Lo
+
+//H-bridge
+PwmOut rightMotorPWM(p22);  //Channel A
+PwmOut leftMotorPWM(p21);   //Channel B
+DigitalOut leftMotor1(p23);
+DigitalOut leftMotor2(p24);
+DigitalOut rightMotor1(p25);
+DigitalOut rightMotor2(p26);
+
+//Angle state variable
+int rtheta;
+
+
+//Non-feedback corrected 'dumb' driving
+void setMove(float speed, bool reverse)
+{
+    //Set speed
+    rightMotorPWM = speed;
+    leftMotorPWM = speed;
+
+    //Set motor function
+    leftMotor1 = (!reverse) ? 0 : 1;
+    leftMotor2 = (!reverse) ? 1 : 0;
+    rightMotor1 = (!reverse) ? 0 : 1;
+    rightMotor2 = (!reverse) ? 1 : 0;
+}
+
+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;
+}
+
+//Turn left about the center
+void centerTurnLeft(int delta_degrees)
+{
+    //Reset the filter and re-init the IMU
+    imuFilter.reset();
+    rpy_init();
+
+    //So there's this weird thing where the gyro treats left turns as POSITIVE degrees...
+    float initial = toDegrees(imuFilter.getYaw());
+    float target = initial + delta_degrees;
+    double sample;
+    // pc.printf("Init: %f Target: %f\r\n", initial, target);
+
+    //Continue to turn to target
+    while (!gameOver) {
+        setTurnLeft(0.6);
+        sample = toDegrees(imuFilter.getYaw());
+        wait(0.02);
+        if(sample > target - 5)
+            break;
+    }
+    stop();
+}
+
+//Turn right about the center
+void centerTurnRight(int delta_degrees)
+{
+    //Reset the filter and re-init the IMU
+    imuFilter.reset();
+    rpy_init();
+
+    //So there's this weird thing where the gyro treats right turns as NEGATIVE degrees...
+    float initial = toDegrees(imuFilter.getYaw());
+    float target = initial - delta_degrees;
+    double sample;
+
+    // pc.printf("Init: %f Target: %f\r\n", initial, target);
+
+    //Continue to turn to target
+    while (!gameOver) {
+        setTurnRight(0.6);
+        sample = toDegrees(imuFilter.getYaw());
+        wait(0.02);
+        if(sample < target)
+            break;
+    }
+    stop();
+}
+
+void checkSonars()
+{
+    //Collision Detection
+    // read all sonar sensors
+    float s1 = sonar1.read() * 100;
+    float s2 = sonar2.read() * 100;
+    float s3 = sonar3.read() * 100;
+
+    // check if obstacle is near and adjust
+    pc.printf("%.2f, %.2f, %.2f, %f\n\r", s1, s2, s3, SONAR_STOP);
+    if(s2 < SONAR_STOP) {
+
+        if(s1 <= s3) {
+            centerTurnRight(90);
+        } else {
+            centerTurnLeft(90);
+        }
+        imuFilter.reset();
+        rpy_init();
+    }
+}
+
+//Drive straight with compass correction
+void gyroDriveStraight(float speed, bool gameRun, int ms)
+{
+    Timer timer;
+    double sample;
+
+    //Reset IMU
+    imuFilter.reset();
+    rpy_init();
+
+    wait(0.2);
+
+    timer.start();
+    while (timer.read_ms() < ms) {
+
+        timer.stop();
+        //checkSonars();
+        if(gameRun) {
+            receivePosition(NULL);
+        }
+        timer.start();
+
+        imuFilter.computeEuler();
+        sample = toDegrees(imuFilter.getYaw());
+/*
+        //Drift Correction
+        sample = sample + float(timer.read_ms() / 800);
+        // pc.printf("%f :::", sample);
+
+        if (sample < 3) {
+            // pc.printf("Steer Left\r\n");
+            leftMotorPWM = speed - 0.2;
+            rightMotorPWM = speed;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+        } else if (sample > -3) {
+            // pc.printf("Steer Right \r\n");
+            leftMotorPWM = speed;
+            rightMotorPWM = speed - 0.18;
+            leftMotor1 = 0;
+            leftMotor2 = 1;
+            rightMotor1 = 0;
+            rightMotor2 = 1;
+        } else {
+            // pc.printf("Straight\r\n");
+            setMove(speed, false);
+        }*/
+    }
+    stop();
+}
+
+//Check where the evil honeywell-wielding human scum is and perform avoidance
+void avoidHoneywell()
+{
+    //Grab telemetry from human car
+    int human_x = x_hum;
+    int human_y = y_hum;
+
+    //Calculate avoidance angle
+    int dx = x_position - human_x;
+    int dy = y_position - human_y;
+
+    //Set to current angle just in case everything screws up
+    int desiredAngle = 0;// t_position;
+
+    if (dx >= 0 && dy >= 0)
+        desiredAngle = (dx > dy) ? 180 : 270;
+    if (dx >= 0 && dy < 0)
+        desiredAngle = (dx > dy) ? 180 : 90;
+    if (dx < 0 && dy >= 0 )
+        desiredAngle = (dx > dy) ? 0 : 270;
+    if (dx < 0 && dy < 0)
+        desiredAngle = (dx > dy) ? 0 : 90;
+
+    int delta_angle = 0;// t_position - desiredAngle;
+    if (delta_angle >= 0) {
+        //Right turn to some degree
+        centerTurnRight(delta_angle);
+    } else {
+        centerTurnLeft(delta_angle);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AIPosition.h	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,168 @@
+#include <math.h>
+#include "USBHostMouse.h"
+
+#define SONAR_STOP (2.0)
+
+#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;
+
+// sonar sensors
+AnalogIn sonar1(p16); // FL
+AnalogIn sonar2(p17); // FC
+AnalogIn sonar3(p18); // FR
+
+// 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;
+
+double x_hum = 123456;
+double y_hum = 123456;
+
+// 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);
+
+    // check if human car is close enough to end game
+    gameOver = isGameOver(x_hum, y_hum, x_position, y_position);
+    if(gameOver) {
+        // game is over at this point
+        xbee.putc('d');
+        pc.printf("Game over sent!\n\r");
+
+        // go to end game routine
+        endGame();
+    }
+}
+
+/* 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 receivePosition(void const *)
+{
+    char buffer[SERIAL_BUFFER_SIZE];
+    int index = 0;
+    int xt = 0;
+    int yt = 0;
+
+    // clear any garbage
+    // xbee.getc();
+    //xbee.getc();
+
+    // while(!gameOver) {
+
+    // wait for start character
+    while(xbee.readable() && xbee.getc() != 'x' && !gameOver);
+
+    // receive data packet of size PACKET_SIZE bytes
+    pc.printf("Receiving...\n\r");
+    index = 0;
+    while(index < PACKET_SIZE && !gameOver) {
+        if(xbee.readable())
+            buffer[index++] = xbee.getc();
+    }
+    buffer[index] = NULL;
+
+    // reassemble data
+    xt = buffer[1];
+    xt = xt << 8;
+    xt = xt | buffer[2];
+    xt = xt << 8;
+    xt = xt | buffer[3];
+    xt = xt << 8;
+    xt = xt | buffer[4];
+    x_hum = (double) xt;
+
+    yt = buffer[6];
+    yt = yt << 8;
+    yt = yt | buffer[7];
+    yt = yt << 8;
+    yt = yt | buffer[8];
+    yt = yt << 8;
+    yt = yt | buffer[9];
+    y_hum = (double) yt;
+
+    pc.printf("Recieve complete: %d, %d\n\r", (int) x_hum, (int) y_hum);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GameCode.h	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,21 @@
+#define THRESHOLD 500
+
+Serial pc(USBTX, USBRX);
+
+bool gameOver = false;
+
+bool isGameOver(double x_hum, double y_hum, double x_cpu, double y_cpu)
+{
+    pc.printf("AI:(%f, %f), HUM: (%f, %f)\n\r", x_cpu, y_cpu, x_hum, y_hum);
+    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/IMU_RPY.h	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +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;
+
+//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)
+{
+
+    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;
+
+}
+
+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);
+
+    }
+
+    //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;
+        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);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Wed Apr 30 05:53:51 2014 +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	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ITG3200/#9e6042257318

--- a/PositionSystem.h	Fri Apr 25 23:08:01 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,77 +0,0 @@
-#include "USBHostMouse.h"
-#include "HMC6352.h"
-
-#define PFLAG_ON 0
-#define PFLAG_OFF 1
-#define PFLAG_CALIB 2
-#define PTURN_SPEED (0.5)
-
-float PTURN_RIGHT = 0;
-
-extern Serial pc(USBTX, USBRX);
-
-// updates xy position if on, does nothing if off
-extern char PFlag = PFLAG_ON;
-
-// whenever a turn is complete, this should store 
-// the degrees facing (0, 90, 180, 270) in system
-extern int t_position = 0;
-
-// variables to keep track of coordinate position
-float x_position = 0;
-float y_position = 0;
-
-// variables to keep track of movement during turning
-float pturn_x = 0;
-float pturn_y = 0;
-
-/* mouse event handler */
-void onMouseEvent(uint8_t buttons, int8_t x_mickey, int8_t y_mickey, int8_t z) {
-    
-    // calculate new position 
-    if(PFlag == PFLAG_ON) {
-        
-        // mouse movements are in mickeys. 1 mickey = ~230 DPI = ~1/230th of an inch
-        float temp = y_mickey / 232.6;
-        
-        // determine direction we are facing and add to that direction
-        if(t_position == 0)
-            y_position += temp;
-        else if(t_position == 90)
-            x_position += temp;
-        else if(t_position == 180)
-            y_position -= temp;
-        else
-            x_position -= temp;
-    } else if(PFlag == PFLAG_CALIB) {    
-        PTURN_RIGHT += x_mickey / 232.6;
-    } else {
-        pturn_x += x_mickey / 232.6;
-        pturn_y += y_mickey / 232.6;
-    }
-}
-
-// intialize x and y variables for turning
-void turnInit() {
-    pturn_x = 0;
-    pturn_y = 0;
-}
-    
-/* positioning system thread function */
-void PositionSystemMain(void const *) {
-    
-    USBHostMouse mouse;
-    
-    while(1) {
-        // try to connect a USB mouse
-        while(!mouse.connect())
-            Thread::wait(500);
-    
-        // when connected, attach handler called on mouse event
-        mouse.attachEvent(onMouseEvent);
-        
-        // wait until the mouse is disconnected
-        while(mouse.connected())
-            Thread::wait(500);
-    }
-}
\ No newline at end of file

--- a/RobotControl.h	Fri Apr 25 23:08:01 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,167 +0,0 @@
-//*******************************************
-//* Robot motor control and drive functions *
-//*******************************************
- 
-#include "HMC6352.h"
-#include "PositionSystem.h"
-#include "SonarSystem.h"
-#include <math.h>
-
-#define ROTATE_90_TIME 1330 // Time for bot to rotate 90 degrees at PTURN_SPEED in ms
- 
-//Radio Calibration Signals - Need to implement calibration routine...
-#define CHAN3_MAX 2060  //Throttle Hi
-#define CHAN3_MIN 1150  //Throttle Lo
-#define CHAN2_MAX 1260  //Elevator Hi
-#define CHAN2_MIN 2290  //Elevator Lo
-#define CHAN1_MAX 2160  //Rudder Hi
-#define CHAN1_MIN 1210  //Rudder Lo
- 
-//Gyro
-HMC6352 compass(p28, p27);
- 
-//H-bridge
-PwmOut rightMotorPWM(p22);  //Channel A
-PwmOut leftMotorPWM(p21);   //Channel B
-DigitalOut leftMotor1(p23);
-DigitalOut leftMotor2(p24);
-DigitalOut rightMotor1(p25);
-DigitalOut rightMotor2(p26);
-
-float sampleCompass(int sample_size) {
-    float c = 0;
-    for(int i = 0; i < sample_size; i++)
-        c += compass.sample() / 10.0;
-    c = c / (double) sample_size;
-    return c;
-}
- 
-//Non-feedback corrected 'dumb' driving
-void setMove(float speed, bool reverse) {
-    //Set speed
-    rightMotorPWM = speed;
-    leftMotorPWM = speed;
-    
-    //Set motor function
-    leftMotor1 = (!reverse) ? 0 : 1;
-    leftMotor2 = (!reverse) ? 1 : 0;
-    rightMotor1 = (!reverse) ? 0 : 1;
-    rightMotor2 = (!reverse) ? 1 : 0;
-}
- 
-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;
-}
-
-// calibrate 90 degree turns
-void turnCalibrate() {
-    
-    // tell positioning system to set the turn
-    // calibration variable, PTURN_RIGHT
-    PFlag = PFLAG_CALIB;
-        
-    // start turning hardcoded speed PTURN_SPEED
-    setTurnRight(PTURN_SPEED);
-    
-    // wait until ROTATE_90_TIME has passed. This should
-    // be the time to complete a 90 degree turn
-    wait_ms(ROTATE_90_TIME);
-    stop();
-    
-    // done calibrating, turn positioning system back on
-    PFlag = PFLAG_ON;
-}
-
-// -------------------------------------------------------------------
- 
-//Turn right about the center
-void turnRight(int delta_degrees)
-{
-    pc.printf("Turnleft: \n\r");
-    
-    // turn positioning system off during turns since
-    // no x and y movements
-    PFlag = PFLAG_OFF;
-    
-    // store new orientation after this turn executes
-    t_position += 90;
-    t_position %= 360;   
-    
-    // initialize turn variables
-    turnInit();
-    
-    // start turning right
-    setTurnRight(PTURN_SPEED);
-    
-    // read mouse sensor until 90 degress has completed
-    while(pturn_x > PTURN_RIGHT) {
-        pc.printf("%f, %f\n\r", pturn_x, pturn_y);
-    }
-    
-    // stop turning
-    stop();
-    
-    // turn positioning system back on
-    PFlag = PFLAG_ON;
-}
-
-//Drive straight with compass correction
-void move(float speed, bool reverse) {
-
-    // begin moving
-    setMove(speed, reverse);
-    
-    // blocking call
-    while(1) {
-  
-        // read all sonar sensors
-        float s1 = sonar1.read() * 100;
-        float s2 = sonar2.read() * 100;
-        float s3 = sonar3.read() * 100;
-            
-        // check if obstacle is near and adjust 
-        if(s2 < SONAR_STOP) {
-            pc.printf("%.2f, %.2f, %.2f\n\r", s1, s2, s3);
-            if(s1 >= SONAR_STOP) {
-               //turnLeft(90);
-            } else if(s3 >= SONAR_STOP) {
-               turnRight(90);
-            } else {
-               pc.printf("IM STUCK HALP\n\t");
-            }
-        }  
-          
-    }
- }
-
- 
\ No newline at end of file

--- a/SonarSystem.h	Fri Apr 25 23:08:01 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#include "mbed.h"
-
-#define SONAR_STOP (1.8)
-
-AnalogIn sonar1(p16); // FL
-AnalogIn sonar2(p17); // FC
-AnalogIn sonar3(p15); // FR
-

--- a/main.cpp	Fri Apr 25 23:08:01 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#include "mbed.h"
-#include "RobotControl.h"
-#include "rtos.h"
- 
-#define ENTERCALIB 0x43
-#define EXITCALIB 0x45
-
-DigitalOut led3(LED3);
-DigitalOut led4(LED4);
-DigitalOut rst1(p12);
-
-Serial xbee1(p13, p14);
- 
-InterruptIn sw(p30);
- 
-void StopISR()
-{
-    rightMotorPWM = 0;
-    leftMotorPWM = 0;
-    exit(1);
-}
- 
-int main() {
-    
-    //Emergency stop mechanism
-    sw.rise(&StopISR);
-    
-    // start positioning system
-    Thread position(PositionSystemMain, NULL, osPriorityNormal, 256 * 4);
-    wait(3); // wait for the mouse sensor to boot up
-    
-    // calibrate 90 degree turns
-    turnCalibrate();
-    
-    // PTURN_RIGHT is the measured x value for a 90 degree turn
-    pc.printf("PTURN_RIGHT: %f\n\r", PTURN_RIGHT); 
-    wait(1);
-    
-    // try a 90 degree turn with the measured PTURN_RIGHT value
-    turnRight(90);
-}
\ No newline at end of file