This is a ball on the Mbed.

Dependencies:   4DGL-uLCD-SE IMUfilter ITG3200 Music mbed-rtos mbed

Files at this revision

API Documentation at this revision

Comitter:
Strikewolf
Date:
Fri Mar 14 17:03:16 2014 +0000
Commit message:
Initial commit;

Changed in this revision

4DGL-uLCD-SE.lib Show annotated file Show diff for this revision Revisions of this file
IMU_RPY.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
Music.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
main_legacy.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib 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 680348a938f8 4DGL-uLCD-SE.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/4DGL-uLCD-SE.lib	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/4180_1/code/4DGL-uLCD-SE/#e39a44de229a
diff -r 000000000000 -r 680348a938f8 IMU_RPY.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_RPY.h	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,284 @@
+    
+
+    /**
+     * 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
+     
+    Serial pc(USBTX, USBRX);
+     
+    //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)
+    {
+     
+        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);
+    }
+
diff -r 000000000000 -r 680348a938f8 IMUfilter.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/IMUfilter/#8a920397b510
diff -r 000000000000 -r 680348a938f8 ITG3200.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.lib	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/ITG3200/#b098d99dd81e
diff -r 000000000000 -r 680348a938f8 Music.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Music.lib	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mdu7078/code/Music/#c33ed3d85f97
diff -r 000000000000 -r 680348a938f8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,384 @@
+        #include <math.h>
+    #include <time.h>
+     
+    #include "mbed.h"
+    #include "uLCD_4DGL.h"
+    #include "rtos.h"
+    #include "IMU_RPY.h"
+     
+    // game configuration macros
+    #define NUMBER_OF_ROUNDS 3
+    #define CURSOR_SPEED 0.5
+    #define CURSOR_COLOR 0xFFFF00
+    #define SPACE_COLOR BLACK
+    #define WALL_COLOR RED
+    #define START_COLOR GREEN
+    #define END_COLOR BLUE
+     
+    // game mechanism macros
+    #define MAZE_DIMENSION 25
+    #define SCALAR (125 / MAZE_DIMENSION)
+    #define START_BLOCK 3
+    #define END_BLOCK 2
+    #define WALL_BLOCK 1
+    #define SPACE_BLOCK 0
+     
+    // prototypes in alphabetical order
+    bool checkVictory();
+    void displaySplashScreen();
+    void displayMaze();
+    void displayVictory();
+    void displayEndGame();
+    void generateMaze();
+    void depthFirstSearch(int r, int c);
+    void updateVelocity();
+    void updateBall();
+    void xthread(void const *args);
+    void ythread(void const *args);
+     
+    // display variables
+    uLCD_4DGL uLCD(p9, p10, p11);
+     
+    // cursor variables
+    Mutex x_mutex, y_mutex;
+    char cursor_x_pos, cursor_y_pos;
+    char old_cursor_x_pos, old_cursor_y_pos;
+    float ballxvel, ballyvel;
+     
+    // start and end variables
+    char start_x, start_y;
+    char end_x, end_y;
+     
+    // maze data structure
+    char maze[MAZE_DIMENSION][MAZE_DIMENSION];
+     
+    // level counter
+    char level = 0;
+     
+    int main()
+    {
+        pc.printf("Super Mbed Ball!!!!!\n\r");
+     
+        // set up gyroscope/accelerometer (rpy = roll, pitch, yaw)
+        rpy_init();
+     
+        //Overclock uLCD
+        uLCD.baudrate(3000000);
+     
+        //Title Screen
+        displaySplashScreen();
+     
+        //Start physics engine threads
+        Thread t1(xthread);
+        Thread t2(ythread);
+     
+        // each iteration runs a complete game until user wins.
+        // wait for user to complete max allowed levels before
+        // terminating
+        while(level < NUMBER_OF_ROUNDS) {
+     
+            //Initial velocity
+            ballxvel = 0;
+            ballyvel = 0;
+     
+            //create Maze
+            generateMaze();
+            displayMaze();
+     
+            //Game Execution Loop
+            while (checkVictory() == false) {
+                updateVelocity();
+                updateBall();
+            }
+     
+            // victory splash screen
+            displayVictory();
+           
+            // increment level counter
+            level++;
+        }
+       
+        // display last end game screen before exiting program
+        displayEndGame();
+    }
+     
+    // checks if cursor has  moved to the end block
+    bool checkVictory()
+    {
+        // check if cursor made it to end row and column
+        return(maze[cursor_x_pos][cursor_y_pos] == END_BLOCK);
+    }
+     
+    // depth first search, called by generateMaze()
+    void depthFirstSearch(int r, int c)
+    {
+        // 4 random direction
+        int randDirs1[] = {1, 2, 3, 4};
+        int randDirs2[] = {4, 2, 3, 1};
+        int *randDirs = NULL;
+     
+        if (rand() % 2)
+            randDirs = randDirs1;
+        else
+            randDirs = randDirs2;
+     
+        // Examine each direction
+        for (int i = 0; i < 4; i++) {
+            switch (randDirs[i]) {
+                case 1: // Up
+                    // Whether 2 cells up is out or not
+                    if (r - 2 <= 0)
+                        continue;
+                    if (maze[r - 2][c] != 0) {
+                        maze[r - 2][c] = SPACE_BLOCK;
+                        maze[r - 1][c] = SPACE_BLOCK;
+                        depthFirstSearch(r - 2, c);
+                    }
+                    break;
+                case 2: // Right
+                    // Whether 2 cells to the right is out or not
+                    if (c + 2 >= MAZE_DIMENSION - 1)
+                        continue;
+                    if (maze[r][c + 2] != 0) {
+                        maze[r][c + 2] = SPACE_BLOCK;
+                        maze[r][c + 1] = SPACE_BLOCK;
+                        depthFirstSearch(r, c + 2);
+                    }
+                    break;
+                case 3: // Down
+                    // Whether 2 cells down is out or not
+                    if (r + 2 >= MAZE_DIMENSION - 1)
+                        continue;
+                    if (maze[r + 2][c] != 0) {
+                        maze[r + 2][c] = SPACE_BLOCK;
+                        maze[r + 1][c] = SPACE_BLOCK;
+                        depthFirstSearch(r + 2, c);
+                    }
+                    break;
+                case 4: // Left
+                    // Whether 2 cells to the left is out or not
+                    if (c - 2 <= 0)
+                        continue;
+                    if (maze[r][c - 2] != 0) {
+                        maze[r][c - 2] = SPACE_BLOCK;
+                        maze[r][c - 1] = SPACE_BLOCK;
+                        depthFirstSearch(r, c - 2);
+                    }
+                    break;
+            }
+        }
+    }
+     
+    //Take whats in the mazearr and write it
+    void displayMaze()
+    {
+     
+        // Clear previous maze
+        uLCD.filled_rectangle(0, 0, 127, 127, SPACE_COLOR);
+     
+        // display start location
+        uLCD.filled_rectangle(start_x * SCALAR, start_y * SCALAR,
+                              (start_x + 1) * SCALAR - 1, (start_y + 1) * SCALAR - 1, START_COLOR);
+     
+        // display end location
+        uLCD.filled_rectangle(end_x * SCALAR, end_y * SCALAR,
+                              (end_x + 1) * SCALAR - 1, (end_y + 1) * SCALAR - 1, END_COLOR);
+     
+        // display walls of maze
+        for (int i = 0; i < MAZE_DIMENSION; i++) {
+            for (int j = 0; j < MAZE_DIMENSION; j++) {
+                if (maze[i][j] == WALL_BLOCK)
+                    uLCD.filled_rectangle(i * SCALAR, j * SCALAR, (i + 1) * SCALAR - 1, (j + 1) * SCALAR - 1, WALL_COLOR);
+            }
+        }
+    }
+     
+    //Game title screen
+    void displaySplashScreen()
+    {
+        uLCD.text_width(2);
+        uLCD.text_height(2);
+        uLCD.locate(0, 0);
+        uLCD.printf("SuperMbed");
+        uLCD.text_width(2);
+        uLCD.text_height(3);
+        uLCD.locate(2, 1);
+        uLCD.printf("Ball!");
+        wait(3);
+    }
+     
+    //Victory screen - 5 sec delay and then next level
+    void displayVictory()
+    {
+        uLCD.text_width(2);
+        uLCD.text_height(2);
+        uLCD.locate(1, 3);
+        uLCD.printf("VICTORY!");
+        wait(5);
+    }
+     
+    // End game screen
+    void displayEndGame() {
+        // wipe screen
+        uLCD.filled_rectangle(0, 0, 127, 127, BLACK);
+       
+        // write game over
+        uLCD.text_width(2);
+        uLCD.text_height(2);
+        uLCD.locate(1, 3);
+        uLCD.printf("GAME OVER");
+    }
+     
+    // randomely generates a maze using depth first search
+    void generateMaze()
+    {
+        // Initialize all spaces to walls
+        for (int i = 0; i < MAZE_DIMENSION; i++)
+            for (int j = 0; j < MAZE_DIMENSION; j++)
+                maze[i][j] = WALL_BLOCK;
+     
+        // unused z-value of gyroscope will be random seed
+        srand(imuFilter.getYaw());
+     
+        // calculate starting row and column of maze DFS
+        // starting row and column must be an odd number
+        int seed = 0;
+        while(seed % 2 == 0)
+            seed = rand() % (MAZE_DIMENSION -1) + 1;
+     
+        pc.printf("seed: %d\r\n", seed);
+     
+        // Starting cell
+        maze[seed][seed] = SPACE_BLOCK;
+     
+        // Allocate the maze with recursive method
+        depthFirstSearch(seed, seed);
+     
+        // find start and end positions
+        start_x = start_y = 0xF;
+        end_x = end_y = 0;
+        for (int i = 0; i < MAZE_DIMENSION; i++) {
+            for (int j = 0; j < MAZE_DIMENSION; j++) {
+     
+                if (maze[i][j] == SPACE_BLOCK) {
+                    // start space
+                    if((i*MAZE_DIMENSION + j) < (start_x*MAZE_DIMENSION + start_y)) {
+                        start_x = i;
+                        start_y = j;
+                    }
+     
+                    // end space
+                    if((i*MAZE_DIMENSION + j) > (end_x*MAZE_DIMENSION + end_y)) {
+                        end_x = i;
+                        end_y = j;
+                    }
+                }
+            }
+        }
+     
+        // reset cursor starting position
+        cursor_x_pos = old_cursor_x_pos = start_x;
+        cursor_y_pos = old_cursor_y_pos = start_y;
+     
+        // mark spots in maze data structure
+        maze[start_x][start_y] = START_BLOCK;
+        maze[end_x][end_y] = END_BLOCK;
+     
+    }
+     
+    //Move the ball around and draw to the screen
+    void updateBall()
+    {
+        x_mutex.lock();
+        y_mutex.lock();
+     
+        // redraw ball only if the position has changed
+        if (cursor_x_pos != old_cursor_x_pos || cursor_y_pos != old_cursor_y_pos) {
+     
+            //Wipe the old ball
+     
+            uLCD.filled_rectangle(old_cursor_x_pos * SCALAR, old_cursor_y_pos * SCALAR,
+                                  (old_cursor_x_pos + 1) * SCALAR - 1, (old_cursor_y_pos + 1) * SCALAR - 1, SPACE_COLOR);
+     
+            //Out with the old in with the new!
+            uLCD.filled_rectangle(cursor_x_pos * SCALAR, cursor_y_pos * SCALAR,
+                                  (cursor_x_pos + 1) * SCALAR - 1, (cursor_y_pos + 1) * SCALAR - 1, CURSOR_COLOR);
+     
+            // store new position
+            old_cursor_x_pos = cursor_x_pos;
+            old_cursor_y_pos = cursor_y_pos;
+        }
+     
+        x_mutex.unlock();
+        y_mutex.unlock();
+    }
+     
+     
+    //This will be where the gyro values are used to accelerate/decelerate the ball
+    void updateVelocity()
+    {
+        x_mutex.lock();
+        y_mutex.lock();
+     
+        // sample gyroscope/accelerometer through filter
+        ballxvel = toDegrees(imuFilter.getPitch()) / -10;
+        ballyvel = toDegrees(imuFilter.getRoll()) / 10;
+     
+        // bound velocities to max speed for x
+        if (ballxvel > 1)
+            ballxvel = CURSOR_SPEED;
+        else if (ballxvel < -1)
+            ballxvel = -CURSOR_SPEED;
+     
+        // bound velocities to max speed for y
+        if (ballyvel > 1)
+            ballyvel = CURSOR_SPEED;
+        else if (ballyvel < -1)
+            ballyvel = -CURSOR_SPEED;
+     
+        // round to 2 decimal places
+        ballxvel = floorf(ballxvel * 100.0) / 100.0;
+        ballyvel = floorf(ballyvel * 100.0) / 100.0;
+     
+        x_mutex.unlock();
+        y_mutex.unlock();
+    }
+     
+     
+    //xthread and ythread act as the physics engine, simulating velocity and wall detection
+    void xthread(const void* args)
+    {
+        while (1) {
+            x_mutex.lock();
+            y_mutex.lock();
+            if (ballxvel > 0) {
+                if (maze[cursor_x_pos + 1][cursor_y_pos] != WALL_BLOCK)
+                    cursor_x_pos++;
+            } else if (ballxvel < 0) {
+                if (maze[cursor_x_pos - 1][cursor_y_pos] != WALL_BLOCK)
+                    cursor_x_pos--;
+            }
+            x_mutex.unlock();
+            y_mutex.unlock();
+            Thread::wait(100 - 98 * abs(ballxvel));
+        }
+    }
+     
+    void ythread(const void* args)
+    {
+        while (1) {
+            x_mutex.lock();
+            y_mutex.lock();
+            if (ballyvel > 0) {
+                if (maze[cursor_x_pos][cursor_y_pos + 1] != WALL_BLOCK)
+                    cursor_y_pos++;
+            } else if (ballyvel < 0) {
+                if (maze[cursor_x_pos][cursor_y_pos - 1] != WALL_BLOCK)
+                    cursor_y_pos--;
+            }
+            x_mutex.unlock();
+            y_mutex.unlock();
+            Thread::wait(100 - 98 * abs(ballyvel));
+        }
+    }
+
diff -r 000000000000 -r 680348a938f8 main_legacy.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_legacy.cpp	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,316 @@
+#include "mbed.h"
+#include "uLCD_4DGL.h"
+#include "rtos.h"
+#include "Music.h"
+#include "IMU_RPY.h"
+
+DigitalOut myled(LED1);
+AnalogIn xpot(p19);
+AnalogIn ypot(p20);
+uLCD_4DGL uLCD(p9,p10,p11);
+music ms(p21);
+
+Mutex xmutex,ymutex;
+char mazearr[128][128];
+char ballx, bally;
+char ballx_old, bally_old;
+float ballxvel, ballyvel, ballvel;
+enum LEVELSELECT {LEVEL1, LEVEL2, LEVEL3};
+LEVELSELECT gamestate = LEVEL1;
+
+//Sound bytes
+char s1[] = "E4:8; E4:8; R:8; E4:8; R:8; C4:8; E4:4; G4:4; R:4; G3:4; R:4;";
+int s1_len = 61;
+char wallbeep[] = "C4:48;";
+int wallbeep_len = 6;
+char victory[] = "A4:12; R:32; A4:12; R:32; A4:12; R:32; A4:4; R:8; F4:4; R:8; G4:4; R:8; A4:4; R:16; G4:20; R:32; A4:4;";
+int victory_len = 102;
+double tempo = 180;
+
+void displayTitle();
+void displayMaze();
+void updateVelocity();
+void updateBall();
+void loadLevel();
+void displayVictory();
+
+//xthread and ythread act as the physics engine, simulating velocity and wall detection
+void xthread(void const *args)
+{
+    while(1)
+    {
+        xmutex.lock();
+        ymutex.lock();
+        if(ballxvel > 0)
+        {
+            if(mazearr[ballx+1][bally] != 1)
+            {
+                ballx++;
+            }
+            else
+            {
+                ballxvel = -ballxvel;
+                
+                ms.play(wallbeep,tempo,wallbeep_len);
+            }
+        }
+        else if (ballxvel < 0)
+        {
+            if(mazearr[ballx-1][bally] != 1)
+            {
+                ballx--;
+            }
+            else
+            {
+                ms.play(wallbeep,tempo,wallbeep_len);
+                ballxvel = -ballxvel;
+            }
+        }
+        xmutex.unlock();
+        ymutex.unlock();
+        Thread::wait(100-98*abs(ballxvel));
+    }
+} 
+void ythread(void const *args)
+{
+    while(1)
+    {
+        xmutex.lock();
+        ymutex.lock();
+        if(ballyvel > 0)
+        {
+            if(mazearr[ballx][bally+1] != 1)
+            {
+                bally++;
+            }
+            else
+            {
+                ballyvel = -ballyvel;
+                ms.play(wallbeep,tempo,wallbeep_len);
+            }
+        }
+        else if (ballyvel < 0)
+        {
+            if(mazearr[ballx][bally-1] != 1)
+            {
+                bally--;
+            }
+            else
+            {
+                ballyvel = -ballyvel;
+                ms.play(wallbeep,tempo,wallbeep_len);
+            }
+        }
+        xmutex.unlock();
+        ymutex.unlock();
+        Thread::wait(100-98*abs(ballyvel));
+    }
+}
+
+int main() {
+    //Overclock uLCD
+    uLCD.baudrate(3000000);
+    
+    //Title Screen
+    //displayTitle();
+    
+    //Display Maze
+    loadLevel();
+    displayMaze();
+    
+    //Music init
+    ms.freq(240);
+    ms.play(s1,tempo,s1_len);
+    //ms.play(victory, tempo, victory_len);
+    
+    //Start physics engine threads
+    Thread t1(xthread);
+    Thread t2(ythread);
+    
+    //Initial velocity
+    ballxvel = 0.5;
+    ballyvel = -1;
+        
+    //Execution Loop
+    while(1) {
+        
+        updateVelocity();
+        updateBall();
+        
+    }
+}
+
+//This will be where the gyro values are used to accelerate/decelerate the ball
+void updateVelocity()
+{
+    xmutex.lock();
+    ymutex.lock();
+    //These should be 0.0 - 1.0 values
+    //ballxvel = 0.25;
+    //ballyvel = -0.25;    //Note: positive yvel will send the ball DOWN due to the uLCD coordinate system
+    xmutex.unlock();
+    ymutex.unlock();
+}
+
+//Move the ball around and draw to the screen
+void updateBall()
+{
+    xmutex.lock();
+    ymutex.lock();
+    
+    uLCD.pixel(ballx_old,bally_old,BLACK);  //Wipe the old ball
+    uLCD.pixel(ballx,bally,0xFFFF00);   //Out with the old in with the new!
+    ballx_old = ballx;
+    bally_old = bally;
+    
+    xmutex.unlock();
+    ymutex.unlock();
+    
+    //Draw start/end zones
+    uLCD.filled_rectangle(2,2,22,22,GREEN);
+    uLCD.filled_rectangle(106,106,126,126,0xFFFF00);
+    
+    //Check victory condition
+    if(ballx > 106 && bally > 106)
+    {
+        displayVictory();   
+    }
+}
+
+//Take whats in the mazearr and write it
+void displayMaze()
+{
+    //Clear previous maze
+    uLCD.filled_rectangle(0, 0, 128, 128, BLACK);
+    
+    //Write in new maze
+    for(int i = 0; i < 128; i++)
+    {
+        for(int j = 0; j < 128; j++)
+        {
+            if(mazearr[i][j] == 1)
+            {
+                uLCD.pixel(i,j,RED);
+            }
+        }
+    }
+}
+
+//This function will load a different map into the mazearray depending on the level selected
+void loadLevel()
+{
+    //Load ball into starting zone
+    ballx = 12;
+    bally = 12;
+    
+    //Zero out the previous maze
+    for(int i = 0; i < 128; i++)
+    {
+        for(int j = 0; j < 128; j++)
+        {
+            mazearr[i][j] = 0;
+        }
+    }
+    
+    //Load in test maze
+    switch(gamestate)
+    {
+        case LEVEL1:
+            //Load a test maze
+            for(int i = 0; i <= 127; i++)
+            {
+                mazearr[i][0] = 1;
+               mazearr[i][127] = 1;
+               mazearr[0][i] = 1;
+                mazearr[127][i] = 1;
+            }
+            for(int i = 0; i <= 100; i++)
+            {
+               mazearr[i][50] = 1;
+            }
+            for(int i = 0; i <= 75; i++) {
+               mazearr[i][95] = 1;
+            }
+            for(int i = 50; i <= 75; i++) {
+                mazearr[50][i] = 1;
+            }    
+            break;
+        
+         case LEVEL2:
+            //Load a test maze
+            for(int i = 0; i <= 127; i++)
+            {
+                mazearr[i][0] = 1;
+               mazearr[i][127] = 1;
+               mazearr[0][i] = 1;
+                mazearr[127][i] = 1;
+            }
+            for(int i = 0; i <= 100; i++)
+            {
+               mazearr[i][24] = 1;
+            }
+            for(int i = 100; i <= 125; i++) {
+               mazearr[i][95] = 1;
+            }
+            for(int i = 100; i <= 125; i++) {
+                mazearr[95][i] = 1;
+            }      
+            for(int i = 40; i <= 65; i++) {
+                mazearr[i][i] = 1;
+            }      
+             for(int i = 80; i <= 95; i++) {
+                mazearr[i][i] = 1;
+            } 
+            break;
+        
+    }    
+}
+
+//Victory screen - 5 sec delay and then next level
+void displayVictory()
+{
+    //Lock out physics engine (hacky way to stop ball movement)
+    xmutex.lock();
+    ymutex.lock();
+    
+    ms.play(victory, tempo, victory_len);
+            
+    //Move ball to start zone
+    ballx = 12;
+    bally = 12;
+    
+    while(1)
+    {
+        uLCD.text_width(2);
+        uLCD.text_height(2);
+        uLCD.locate(1,3);
+        uLCD.printf("VICTORY!");
+        wait(5);
+        if (gamestate == LEVEL1)
+        {
+            gamestate = LEVEL2;
+            loadLevel();
+            displayMaze();
+            wait(1);
+            break;
+        }
+    } 
+    xmutex.unlock();
+    ymutex.unlock();
+}
+
+//Game title screen
+void displayTitle()
+{
+    while(1)
+    {
+        uLCD.text_width(2);
+        uLCD.text_height(2);
+        uLCD.locate(0,0);
+        uLCD.printf("SuperMbed");        
+        uLCD.text_width(2);
+        uLCD.text_height(3);
+        uLCD.locate(2,1);
+        uLCD.printf("Ball!");
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r 680348a938f8 mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#f88660a9bed1
diff -r 000000000000 -r 680348a938f8 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Mar 14 17:03:16 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8e73be2a2ac1
\ No newline at end of file