Example code for the UKESF Headstart robotics lab; code to make the PsiSwarm robot move forward and turn.

Dependencies:   PsiSwarm-Headstart mbed

Fork of PsiSwarm_V41 by James Hilder

Files at this revision

API Documentation at this revision

Comitter:
jah128
Date:
Mon Jun 20 13:36:30 2016 +0000
Parent:
30:513457c1ad12
Commit message:
PsiSwarm Headstart Lab

Changed in this revision

BeautifulMeme/beacon.cpp Show diff for this revision Revisions of this file
BeautifulMeme/beacon.h Show diff for this revision Revisions of this file
BeautifulMeme/bmeme.cpp Show diff for this revision Revisions of this file
BeautifulMeme/bmeme.h Show diff for this revision Revisions of this file
BeautifulMeme/programs.cpp Show diff for this revision Revisions of this file
BeautifulMeme/programs.h Show diff for this revision Revisions of this file
BeautifulMeme/vector.cpp Show diff for this revision Revisions of this file
BeautifulMeme/vector.h Show diff for this revision Revisions of this file
PsiSwarm.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.h Show annotated file Show diff for this revision Revisions of this file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/beacon.cpp
--- a/BeautifulMeme/beacon.cpp	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,376 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots
-
-#include "bmeme.h"
-
-int pulse_step = 1;                     //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8
-int low_threshold;                      //Set to be 2x mean background IR
-int beacon_threshold;                   //Set to be 4x mean background IR
-unsigned short ir_sensor_data[9][8];    // The raw sensor data from all 9x 50ms sample windows
-Ticker ir_sample_ticker;                // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot
-Ticker ir_emitter_ticker;               // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot
-Timeout ir_emitter_timeout;             // Timeout for turning off the IR emitters after 40ms
-Timer beacon_debug_timer;               // Timer for debug information only [remove later?]
-
-char show_ir_debug_info = 0;            // Set to 1 to display (via PC) the list of IR readings & visible robots every timestep
-
-/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in [BEACON_PERIOD / 2.5] (20ms) blocks.
-/// The infrared beacon is set to give a 50ms burst of IR every 500ms.  We should thus see in the sampled radiation 2 blocks
-/// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart.
-void locate_beacon()
-{
-    int sample_period = (BEACON_PERIOD * 2) / 5;
-    out("1) Searching for IR beacon...");
-    unsigned short samples[50][9];
-    Timer beacon_timer;
-    beacon_timer.start();
-    int offset = 0;
-    //This loop samples the background IR values at 50Hz for 1 second and stores in an array
-    for(int i=0; i<50; i++) {
-        store_background_raw_ir_values ();
-        if(i%2 == 0){
-            set_center_led(1, 0.5);
-            set_leds(0xAA,0x55);
-        }else{
-            set_center_led(2, 0.5);
-            set_leds(0x55,0xAA);
-        }
-        samples[i][8]=0;
-        for(int j=0; j<8; j++) {
-            samples[i][j] = get_background_raw_ir_value(j);
-            samples[i][8] += get_background_raw_ir_value(j);
-        }
-        offset+=sample_period;
-        while(beacon_timer.read_us() < offset) {}
-    }
-
-    //Print values: for testing [comment out]
-    /*
-    for(int i=0; i<50; i++) {
-        out("IR %d:",i);
-        for(int j=0; j<8; j++) {
-            out("[%d:%d]",j,samples[i][j]);
-        }
-        out("  [SUM:%d]\n",samples[i][8]);
-    }
-    */
-
-    //Bubble sort sums to find (6) highest values
-    unsigned short sorted_array[50];
-    for(int i=0; i<50; i++) {
-        sorted_array[i]=samples[i][8];
-    }
-    for (int c = 0 ; c < 49; c++) {
-        for (int d = 0 ; d < (50-c-1); d++) {
-            if (sorted_array[d] > sorted_array[d+1]) {
-                unsigned short swap = sorted_array[d];
-                sorted_array[d] = sorted_array[d+1];
-                sorted_array[d+1] = swap;
-            }
-        }
-    }
-
-    //Print sorted values: for testing [comment out]
-    /*
-    out("Sorted values:");
-    for (int c = 0 ; c < 50 ; c++ ) {
-        out("%d", sorted_array[c]);
-        if(c<49)out(",");
-    }
-    out("\n");
-    */
-
-    // Calculate mean background sum value by looking at 44 lowest sum values
-    int background_mean = 0;
-    for(int i=0;i<44;i++)background_mean += sorted_array[i];
-    background_mean /= 44;
-    
-    //out("Background mean value: %d\n",background_mean);
-    
-    //Our beacon threshold will be 4x the background mean value; find all instances where this occurs
-    low_threshold = background_mean * 2;
-    beacon_threshold = background_mean * 4;
-    char beacon_detected_indices[50];
-    for(int i=0;i<50;i++){
-        if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1;
-        else beacon_detected_indices[i]=0;
-    }
-    //Count and display matches
-    int beacon_detected_count = 0;
-    //char output_string[251] = "";
-    for(int i=0;i<50;i++){
-        if(beacon_detected_indices[i] == 1){
-            beacon_detected_count++;
-           // char index_string[6];
-           // sprintf(index_string,"[%d],",i);   
-           // strcat(output_string,index_string);
-        }
-    }
-    //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string);
-    
-    //We will use this array to store average values for each sensor when the beacon is detected
-    unsigned short beacon_averages[8];
-    char beacon_averages_count = 0;
-    for(int i=0;i<8;i++)beacon_averages[i]=0;
-    
-    //Now determine if the beacon is correctly found: must adhere to a set of rules
-    //Firstly, we should have not less than 4 and not more than 6 positive matches 
-    if(beacon_detected_count>3 && beacon_detected_count<7){
-        // Now verify that the positive samples are in valid places...
-        // Find first positive sample
-        int first_index = 0;
-        //out("Here\n",first_index);
-        
-        while(beacon_detected_indices[first_index]==0)first_index ++;
-        
-        //out("First index:%d\n",first_index);
-        
-        
-        // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high
-        if(first_index == 0){
-            if(beacon_detected_indices[49]>0)first_index = 49;
-            if(beacon_detected_indices[48]>0)first_index = 48;   
-        }
-        
-        beacon_averages_count++;
-        for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];}
-        
-        // Now count the length of the 'block' of positive hits: must be equal to 2 or 3
-        char block_length = 1;
-        int end_index = first_index + 1;
-        if(end_index == 50) end_index = 0;
-        while(beacon_detected_indices[end_index]>0){
-            beacon_averages_count++;
-            for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];}
-            block_length ++;
-            end_index ++;
-            if(end_index == 50) end_index = 0;   
-        }
-        if(block_length==2 || block_length == 3){
-            //We have found the first correct block and it is valid; now calculate its mid-point and check that the second block is also present 500ms later
-            float mid_point;
-            char second_block_okay = 0;
-            if(block_length == 2){
-                mid_point = first_index + 0.5;
-                char second_block_low = first_index + 25;
-                char second_block_high = first_index + 26;
-                if(second_block_low > 49) second_block_low -= 50;
-                if(second_block_high > 49) second_block_high -= 50;
-                beacon_averages_count+=2;
-                for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];}
-                if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1;
-            }
-            if(block_length == 3){
-                mid_point = first_index + 1;
-                if(mid_point == 50) mid_point = 0;
-                char second_block_single = first_index + 26;
-                if(second_block_single > 49) second_block_single -= 50;
-                beacon_averages_count++;
-                for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];}
-                if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1;
-            }   
-            if(second_block_okay >0){
-                beacon_found = 1;
-                beacon_heading = get_bearing_from_ir_array(beacon_averages);
-                out("Found at %d degrees\n",beacon_heading);
-                //for(int i=0;i<8;i++){
-                //    beacon_averages[i] /= beacon_averages_count;
-                //    out("[%d]",beacon_averages[i]);   
-                //}
-                out("2) Synchronising...\n");
-                // Calculate the offset to the expected start of the next beacon pulse
-                int microseconds_offset = (sample_period * mid_point) - sample_period - (sample_period / 4);
-                //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us());
-                int cycle_period = (BEACON_PERIOD * 10);
-                if(microseconds_offset < 0) microseconds_offset += cycle_period;
-                //If we have missed the start of the beacon this cycle, wait until the next cycle
-                while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){};
-                //Now wait until the start of the beacon pulse
-                while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){};
-                /*
-                out("Now:%d",beacon_timer.read_us());
-                Timer test_timer;
-                test_timer.start();
-                for(int i=0;i<50;i++){
-                    store_background_raw_ir_values ();
-                    out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2));
-                    while(test_timer.read_ms() % 10 < 9){};
-                }
-                */
-            }else{
-                beacon_found = 0;
-                out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100));   
-            }
-        }else{
-            beacon_found = 0;
-            if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index);
-            if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length);   
-        }
-    } else {
-        beacon_found = 0;
-        if(beacon_detected_count > 6) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count);
-        else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count);
-    }
-    if(beacon_found == 0){
-        set_leds(0x00,0x00);
-        set_center_led(1, 1);
-        display.clear_display();
-        display.set_position(0,0);
-        display.write_string("BEACON NOT FOUND");   
-    }
-}
-
-// The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to
-// It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot
-// The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot  
-void start_infrared_timers()
-{
-    // At this point we should be exactly at the start of a beacon cycle.
-    // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse)
-    // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step
-    out("3) Starting TDMA infrared timers\n");
-    beacon_debug_timer.start();
-    wait_us(BEACON_PERIOD / 10);  
-    ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD);
-    wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse
-    ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD);
-}
-
-
-//Return the max value in IR array
-unsigned short get_highest_sample(unsigned short * ir_array){
-    unsigned short highest = 0;
-    for(int i=0;i<8;i++){
-        if(ir_array[i]>highest) highest=ir_array[i];
-    }   
-    return highest;
-}
-
-//Return the sum total of IR array
-unsigned short get_sum_sample(unsigned short * ir_array){
-    unsigned short sum = 0;
-    for(int i=0;i<8;i++){
-        sum+=ir_array[i];
-    }   
-    return sum;  
-}
-
-//The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID
-//It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters
-void emitter_ticker_block(){
-    //If the time-step (-1) equals my ID, turn on my emitters for 40ms
-    if(pulse_step-1 == robot_id && disable_ir_emitters == 0){
-        IF_set_IR_emitter_output(0, 1);
-        IF_set_IR_emitter_output(1, 1);
-        ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10);
-    }  
-}
-
-//Turn off the emitters
-void emitter_timeout_block(){
-     //Turn off IR emitters
-     IF_set_IR_emitter_output(0, 0);
-     IF_set_IR_emitter_output(1, 0);
-}
-
-//The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot
-//There are 10 time slots in each 500ms period
-//Slot 0 is when the beacon is flashing
-//Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[]
-//Slot 2-8 are for the 7 robots; slot-1 = robot_id
-//In slot 9, the robot processes the data [and doesn't store and new readings]
-//It checks if the beacon is visible, if any robots are, calculates their bearings if they are, and transfers the background and active IR data for the robot
-void sample_ticker_block(){
-    //If we are in time-step 0 to 8, store the background data in an array
-    if(pulse_step < 9){
-    store_background_raw_ir_values ();
-    for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i);
-    }else{
-      //If not, process the data   
-      for(int i=0;i<9;i++){
-         unsigned short sum = get_sum_sample(ir_sensor_data[i]);
-         unsigned short highest = get_highest_sample(ir_sensor_data[i]);
-         //Check if beacon is visible
-         if(i==0){
-             if(sum > beacon_threshold){
-                 beacon_found = 1;
-                 beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]);
-             }else beacon_found = 0;
-             //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]);
-         }
-         if(i==1){
-            for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j];   
-         }
-         if(i>1){
-            char test_robot = i-1;
-            if(test_robot == robot_id){
-                for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j];   
-            }else{
-                if(sum > low_threshold){
-                    robots_found[test_robot] = 1;
-                    //Debug--
-                    //out("Robot %d: [%d][%d][%d][%d][%d][%d][%d][%d]\n",test_robot,ir_sensor_data[i][0],ir_sensor_data[i][1],ir_sensor_data[i][2],ir_sensor_data[i][3],ir_sensor_data[i][4],ir_sensor_data[i][5],ir_sensor_data[i][6],ir_sensor_data[i][7]);
-                    robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]);
-                    robots_distance[test_robot] = highest;
-                }else robots_found[test_robot] = 0;   
-            }  
-         }
-      }
-      if(show_ir_debug_info == 1)display_ir_readings();
-    }
-    //Increment pulse step
-    pulse_step++;
-    if(pulse_step == 10) pulse_step = 0;
-}
-
-
-//Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles
-void display_ir_readings()
-{
-    out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms());
-    if(beacon_found==1){
-        out("Beacon detected at %d degrees\n",beacon_heading);
-    }  
-    for(int j=1;j<8;j++){
-        if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]);       
-    }   
-    out("Reflected values:");
-    for(int i=0;i<8;i++){
-        out("[%d,%d]",i,reflected_sensor_data[i]);
-    }
-    out("\nBackground values:");
-    for(int i=0;i<8;i++){
-        out("[%d,%d]",i,background_sensor_data[i]);
-    }
-    out("\n\n");
-}
-
-//Returns a 0 if turn is likely to complete in a single timestep, 1 if it is beyond range for single timestep and 2 if the beacon is not found so bearing unknown
-char turn_to_bearing(int bearing)
-{
-    if(beacon_found == 0){
-        out("Beacon not found: cannot turn to specific bearing\n");   
-        return 2;
-    }else{
-        //First calculate the bearing using the angle of beacon relative to robot
-        int current_bearing = 360 - beacon_heading;
-        //Now work out turn needed to face intended heading
-        int target_turn = (bearing - current_bearing) % 360;
-        //Adjust to take 10% off turn, stops overshoot
-        target_turn = (target_turn * 9) / 10;
-        if(target_turn > 180) target_turn -= 360;
-        if(target_turn < -180) target_turn += 360;
-        //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that
-        char beyond_limit = 0;
-        int turn_limit = BEACON_PERIOD / 358;
-        if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;};
-        if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;};
-        out("Turning %d degrees\n",target_turn);
-        time_based_turn_degrees(1, target_turn,1);
-        return beyond_limit;
-    }
-}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/beacon.h
--- a/BeautifulMeme/beacon.h	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// beacon.h - Functions for detecting the beacon and taking IR readings of the robots
-
-#ifndef BEACON_H
-#define BEACON_H
-
-void emitter_ticker_block(void);
-void sample_ticker_block(void);
-void emitter_timeout_block(void);
-void locate_beacon(void);
-void start_infrared_timers(void);
-unsigned short get_highest_sample(unsigned short * ir_array);
-unsigned short get_sum_sample(unsigned short * ir_array);
-char turn_to_bearing(int bearing);
-
-#endif
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/bmeme.cpp
--- a/BeautifulMeme/bmeme.cpp	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,306 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-/// Include bmeme.h - this includes main.h, psiswarm.h all the other necessary core files
-
-#include "bmeme.h"
-
-//  IMPORTANT!!!
-//  Do not call the IR functions at all as they will interfere with the correct operation of this program
-//  Instead, use the values held in the variables below; they are updated every 500ms
-
-char beacon_found = 0;                   // This will be a 1 when a beacon was detected during the previous 500ms window
-int beacon_heading = 0;                  // This is the heading from the last time a beacon was detected
-char robots_found[8];                    // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
-int robots_heading[8];                   // These are the headings from the last time the respective robots were detected
-unsigned short robots_distance[8];       // This is the maximum sensor value from the last time the respective robot was detected
-unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
-unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
-
-char default_normal_program = 8;         // The program to run on turn on (after 'face beacon' program)
-char use_recharging_program = 0;         // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
-char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
-char display_debug_inf = 0;              // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
-char main_program_state;                 // Index of the currently running program
-char previous_program;                   // Used to hold previous running program when it is paused for switch press etc
-char program_changed = 0;                // Flag to update display when program is changed
-char program_run_init = 0;               // Flag to tell program to run its initialisation on first loop, if neccessary
-char success_count = 0;                  // Flag to indicate the success of a program
-char step_cycle = 0;                     // Alternates between 0 and 1 in successive time-steps
-char target_reached = 0;                 // Flag to indicate if a program target has been reached
-char prog_name [17];                     // Stores the name of the running program [line 0 on the display]
-char prog_info [17];                     // Stores information about the current state of the program [line 1 on the display]
-char disable_ir_emitters = 0;            // Used to disable IR emission during charging etc [use with caution!]
-char recharging_state = 0;               // Stores the state of the recharging program (0 is not currently running)
-char switch_held = 0;                    // Used for detected when the cursor switch is held to override program choice
-char choose_program_mode = 0;
-char program_count = 8;
-char program_selection;
-int flocking_headings[8];                // Beacon heading of each robot
-
-float battery_low_threshold = 3.60;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
-float battery_high_threshold = 3.95;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
-
-Ticker main_loop_ticker;
-
-///This is the main loop for the Beautiful Meme code.  The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected.
-void bmeme_main_loop()
-{
-    if(switch_held == 1)switch_held=2;
-    if(switch_held == 3 && choose_program_mode == 0) {
-        //The switch has been held right and then released:  stop the current program
-        previous_program = main_program_state;
-        program_selection = previous_program;
-        choose_program_mode = 1;
-        set_program(255);
-        set_program_info(get_program_name(program_selection));
-    }
-    if(use_recharging_program == 1)recharging_program();
-    update_display();
-    if(recharging_state == 0) {
-        switch(main_program_state) {
-            case 0: //Case 0 is the initial program: turn to face beacon
-                if(step_cycle == 0) {
-                    char turn_status = turn_to_bearing(0);
-                    if(turn_status == 0) {
-                        success_count ++;
-                        if(success_count > 1) set_program(default_normal_program);
-                    } else success_count = 0;
-                }
-                break;
-            case 1:
-                target_reached = 0;
-                head_to_bearing_program(0);
-                if(target_reached == 1) set_program(2);
-                break;
-            case 2:
-                target_reached = 0;
-                head_to_bearing_program(180);
-                if(target_reached == 1) set_program(1);
-                break;
-            case 3:
-                curved_random_walk_with_interaction_program();
-                break;
-            case 4:
-                straight_random_walk_with_interaction_program();
-                break;
-            case 5:
-                find_space_program(1);
-                break;
-            case 6:
-                clustering_program(0,1);
-                break;
-            case 7:
-                tag_game_program();
-                break;
-            case 8:
-                flocking_program();
-                break;
-            case 255:
-                stop_program();
-                break;
-        }
-    }
-    step_cycle=1-step_cycle;
-}
-
-///Place user code here that should be run after initialisation but before the main loop
-void bmeme_user_code_setup()
-{
-    program_name = "B-Meme";
-    author_name  = "YRL";
-    version_name = "0.4";
-    
-    display.clear_display();
-    display.set_position(0,0);
-    display.write_string("BEAUTIFUL MEME");
-    display.set_position(1,0);
-    display.write_string("   PROJECT");
-    wait(0.2);
-    out("------------------------------------------------------\n");
-    out("Beautiful Meme Project Demo Code                      \n");
-    out("------------------------------------------------------\n");
-    locate_beacon();
-    while(beacon_found == 0) {
-        wait(0.5);
-        locate_beacon();
-    }
-    start_infrared_timers();
-    main_loop_ticker.attach_us(&bmeme_main_loop,BEACON_PERIOD * 10);
-    set_program(0);
-    set_leds(0x00,0x00);
-    set_center_led(3,0.5);
-    display.clear_display();
-    display.set_position(0,0);
-    display.write_string("BEACON FOUND AT");
-    display.set_position(1,0);
-    char degrees_string[16];
-    sprintf(degrees_string,"%d DEGREES",beacon_heading);
-    display.write_string(degrees_string);
-}
-
-/// Code goes here to handle what should happen when the user switch is pressed
-void bmeme_handle_switch_event(char switch_state)
-{
-    /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed
-    /// NB For maximum compatability it is recommended to minimise reliance on center button press
-    if(choose_program_mode == 0) {
-        if(switch_state == 8) switch_held = 1;
-        else if(switch_state == 0 && switch_held == 2) switch_held = 3;
-        else switch_held = 0;
-    } else {
-        // We are in choose program mode
-        if(switch_state == 8) {
-            program_selection ++;
-            if(program_selection > program_count) program_selection = 0;
-            if(program_selection == program_count) set_program_info("RECHARGE");
-            else set_program_info(get_program_name(program_selection));
-        }
-        if(switch_state == 4) {
-            if(program_selection == 0) program_selection = program_count; 
-            else program_selection --;  
-            if(program_selection == program_count) set_program_info("RECHARGE");
-            else set_program_info(get_program_name(program_selection));
-        }
-        if(switch_state == 1 || switch_state == 2){
-            if(program_selection == program_count){
-                  recharging_state = 1;
-                  set_program(previous_program);
-                  strcpy(prog_name,"CHARGING PROGRAM");
-                  set_program_info("HEAD TO BEACON");  
-                  
-            }   
-            else set_program(program_selection);
-            choose_program_mode = 0; 
-            switch_held = 0;
-        }
-    }
-    //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info);
-}
-
-char * get_program_name(int index)
-{
-    char * ret_name = new char[17];
-    switch(index) {
-        case 0:
-            strcpy(ret_name,"FACE BEACON");
-            break;
-        case 1:
-            strcpy(ret_name,"HEAD TO BEACON");
-            break;
-        case 2:
-            strcpy(ret_name,"HEAD TO SOUTH");
-            break;
-        case 3:
-            strcpy(ret_name,"RANDOM WALK 1");
-            break;
-        case 4:
-            strcpy(ret_name,"RANDOM WALK 2");
-            break;
-        case 5:
-            strcpy(ret_name,"FIND SPACE");
-            break;
-        case 6:
-            strcpy(ret_name,"CLUSTERING");
-            break;
-        case 7:
-            strcpy(ret_name,"TAG GAME");
-            break;
-        case 8:
-            strcpy(ret_name,"FLOCKING");
-            break;
-        case 255:
-            strcpy(ret_name,"PROGRAM:");
-            break;
-    }
-    return ret_name;
-}
-
-void set_program(int index)
-{
-    main_program_state = index;
-    program_changed = 1;
-    program_run_init = 1;
-    strcpy(prog_info,"");
-    strcpy(prog_name,get_program_name(index));
-}
-
-void set_program_info(char * info)
-{
-    strcpy(prog_info,info);
-    program_changed = 1;
-}
-
-void update_display()
-{
-    if(program_changed == 1) {
-        program_changed = 0;
-        display.clear_display();
-
-        if(display_debug_inf==1) display_debug_info();
-        else {
-            display.set_position(0,0);
-            display.write_string(prog_name);
-        }
-        display.set_position(1,0);
-        display.write_string(prog_info);
-    }
-}
-
-void display_debug_info()
-{
-    char disp_line[16] = "- - - - - - - -";
-    if(beacon_found==1)disp_line[0]='B';
-    for(int i=1; i<8; i++) {
-        if(robots_found[i])disp_line[((i)*2)]=48+i;
-    }
-    display.set_position(0,0);
-    display.write_string(disp_line);
-}
-
-/// Verbose output
-void out(const char* format, ...)
-{
-    char buffer[256];
-    if (debug_mode) {
-        va_list vl;
-        va_start(vl, format);
-        vsprintf(buffer,format,vl);
-        if(user_code_debug == 1) pc.printf("%s", buffer);
-        va_end(vl);
-    }
-}
-
-
-void bmeme_handle_user_serial_message(char * message, char length, char interface)
-{
-    // This is where user code for handling a (non-system) serial message should go
-    //
-    // message = pointer to message char array
-    // length = length of message
-    // interface = 0 for PC serial connection, 1 for Bluetooth
-
-    if(interface) {
-        if(length == 8) {
-            for(int i = 0; i < length; i++) {
-                // Convert single byte value into a beacon heading in the range +/-180 degrees
-                float beacon_heading = message[i];
-                float degrees_per_value = 256.0f / 360.0f;
-
-                if(beacon_heading != 0)
-                    beacon_heading /= degrees_per_value;
-
-                beacon_heading -= 180;
-
-                flocking_headings[i] = beacon_heading;
-
-                debug("%d, ", flocking_headings[i]);
-                //debug("%f, ", beacon_heading);
-            }
-
-            debug("\n");
-        }
-    }
-}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/bmeme.h
--- a/BeautifulMeme/bmeme.h	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,58 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-
-#ifndef BMEME_H
-#define BMEME_H
-
-#include "main.h"
-#include "beacon.h"
-#include "programs.h"
-#include "vector.h"
-#include <math.h>
-
-// Define the on-period for a IR pulse in microseconds (eg 50000 for 2Hz system, 25000 for a 4Hz system or 20000 for a 5Hz system)
-#define BEACON_PERIOD 25000 
-
-
-// Define the on-period for a IR pulse in microseconds (eg 50000 for 2Hz system, 25000 for a 4Hz system or 20000 for a 5Hz system)
-#define BEACON_PERIOD 25000        
-
-extern char beacon_found;                       // This will be a 1 if beacon is detected, 0 if it isn't 
-extern int beacon_heading;                      // The heading from the last time the beacon was detected
-extern char robots_found[8];                    // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
-extern int robots_heading[8];                   // These are the headings from the last time the respective robots were detected
-extern unsigned short robots_distance[8];       // This is the maximum sensor value from the last time the respective robot was detected
-extern unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
-extern unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
-extern char disable_ir_emitters; 
-extern char step_cycle;
-extern char target_reached;
-extern char recharging_state;
-extern char prog_name [17];
-extern char prog_info [17];
-extern char main_program_state;
-extern char program_changed;   
-extern char program_run_init;
-extern float battery_low_threshold;      
-extern float battery_high_threshold; 
-extern int flocking_headings[8];
-
-char * get_program_name(int index);
-void set_program(int index);
-void set_program_info(char * info);
-
-void update_display(void);
-void display_debug_info(void);
-
-void bmeme_user_code_setup(void);
-void bmeme_user_code_loop(void);
-void bmeme_handle_switch_event(char switch_state); 
-void bmeme_handle_user_serial_message(char * message, char length, char interface);
-
-void display_ir_readings(void);
-void out(const char* format, ...) ;   
-
-#endif
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/programs.cpp
--- a/BeautifulMeme/programs.cpp	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,869 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
-
-#include "bmeme.h"
-
-int obstacle_avoidance_threshold = 300;
-int robot_avoidance_threshold = 2000;
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// head_to_bearing_program
-char was_turning = 0;
-
-///The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
-void head_to_bearing_program(int target_bearing)
-{
-    if(step_cycle == 0 || was_turning == 0) {
-        // Check if we are heading in the right bearing (+- 25 degrees)
-        int current_bearing = (360 - beacon_heading) % 360;
-        // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
-        char bearing_ok = 0;
-        int lower_bound = target_bearing - 25;
-        int upper_bound = target_bearing + 25;
-        if(lower_bound < 0) {
-            if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
-        } else if(upper_bound > 359) {
-            if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
-        } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
-        // Check if there is an obstacle in front of us
-        if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
-        else {
-            // Now move forward if we are facing correct bearing, otherwise turn
-            if(bearing_ok == 1) {
-                //Check if anything is in front of us to determine speed - if it is, move slowly
-                int t_time = 6 * BEACON_PERIOD;
-                float t_speed = 1.0;
-                if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
-                    t_time = 4 * BEACON_PERIOD;
-                    t_speed = 0.6;
-                }
-                if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
-                    t_time = 3 * BEACON_PERIOD;
-                    t_speed = 0.4;
-                }
-                if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
-                    t_time = 2 * BEACON_PERIOD;
-                    t_speed = 0.2;
-                }
-                time_based_forward(t_speed,t_time,0);
-                was_turning = 0;
-            } else {
-                turn_to_bearing(target_bearing);
-                was_turning = 1;
-            }
-        }
-    }
-}
-
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// recharging_program
-
-char recharge_power_check_count = 0;
-char battery_low_count = 0;
-
-void recharging_program()
-{
-    switch(recharging_state) {
-        case 0:
-            // We are not currently recharging, check the battery state
-            if(get_battery_voltage() < battery_low_threshold) {
-                // Battery is below low threshold
-                battery_low_count ++;
-                // We don't immediately start recharging routine in case as battery level can fluctuate a little due to load; if we get a low value for 4 continuous timesteps we start recharging routine
-                if(battery_low_count > 3) {
-                    // Set recharging state to 'looking for charger'
-                    recharging_state = 1;
-                    strcpy(prog_name,"CHARGING PROGRAM");
-                    set_program_info("HEAD TO BEACON");
-                }
-            } else battery_low_count = 0;
-            break;
-            // State 1:  Head to beacon [as this is where battery charger is]
-        case 1:
-            target_reached = 0;
-            head_to_bearing_program(0);
-            if(target_reached == 1) {
-                recharging_state = 2;
-                set_program_info("TURN 90 DEGREES");
-            }
-            break;
-            // Stage 2:  Turn 90 degrees to align with charging pads
-        case 2:
-            disable_ir_emitters = 1;
-            time_based_turn_degrees(0.8, 70.0, 1);
-            recharge_power_check_count = 0;
-            recharging_state = 3;
-            break;
-            // Stage 3:  Wait for turn to complete
-        case 3:
-            if (time_based_motor_action != 1) {
-                recharging_state = 4;
-                set_program_info("CHECK FOR POWER");
-            }
-            break;
-            // Stage 4:  Check if charging
-        case 4:
-            recharge_power_check_count++;
-            if(get_dc_status() == 1) {
-                recharging_state = 5;
-            } else {
-                if(recharge_power_check_count < 10)recharging_state = 6;
-                else {
-                    recharging_state = 7;
-                    set_program_info("NO POWER - RETRY");
-                }
-            }
-            break;
-            // Stage 5:  Charging.  Wait until threshold voltage exceeded
-        case 5:
-            if(get_battery_voltage() > battery_high_threshold) {
-                set_program_info("LEAVE CHARGER");
-                recharging_state = 7;
-            } else {
-                char b_voltage[16];
-                sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
-                set_program_info(b_voltage);
-            }
-            break;
-            // Stage 6:  We didn't find power, keep turning a couple of degrees at a time a recheck
-        case 6:
-            time_based_turn_degrees(0.5,4,1);
-            recharging_state = 4;
-            break;
-            // Stage 7:  Charge may be finished.  Turn 90 degrees then move away and resume previous program
-        case 7:
-            time_based_turn_degrees(0.8, 90.0, 1);
-            recharging_state = 8;
-            break;
-
-            // Stage 8:  Wait for turn to complete
-        case 8:
-            if (time_based_motor_action != 1) recharging_state = 9;
-            break;
-            // Stage 9:  Move away
-        case 9:
-            time_based_forward(0.5, 1000000, 1);
-            recharging_state = 10;
-            break;
-            // Stage 10:  Wait for move to complete
-        case 10:
-            if (time_based_motor_action != 1) recharging_state = 11;
-            break;
-            // Stage 11:  Check if battery is below low threshold; if it is, start over, else end charge cycle
-        case 11:
-            disable_ir_emitters = 0;
-            if (get_battery_voltage() < battery_low_threshold) {
-                recharging_state = 1;
-            } else {
-                recharging_state = 0;
-                //Restore name of old program on display
-                set_program(main_program_state);
-            }
-            break;
-    }
-}
-
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program)
-
-enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
-enum random_walk_state internal_state = random_walk;
-char action_timeout = 0;
-char interaction_timeout = 0;
-char random_walk_timeout = 0;
-float previous_left_motor_speed = 0.5;
-float previous_right_motor_speed = 0.5;
-
-void curved_random_walk_with_interaction_program()
-{
-    if(internal_state == random_walk) {
-        if(interaction_timeout < 4)
-            interaction_timeout++;
-
-        int closest_robot = -1;
-        unsigned short shortest_distance = 0;
-
-        // Check whether there are any other robots within range
-        for(int i = 0; i < 8; i++) {
-            if(robots_found[i]) {
-                if(robots_distance[i] > shortest_distance) {
-                    shortest_distance = robots_distance[i];
-                    closest_robot = i;
-                }
-            }
-        }
-
-        // Turn towards the closest robot
-        if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
-            time_based_turn_degrees(1, robots_heading[closest_robot], 1);
-
-            action_timeout = 0;
-            internal_state = turn_towards;
-            char temp_message[17];
-            sprintf(temp_message,"FACE ROBOT %d",closest_robot);
-            set_program_info(temp_message);
-        } else { // Otherwise, do a random walk
-            // Check the front sensors for obstacles
-            if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
-                    reflected_sensor_data[1] > obstacle_avoidance_threshold ||
-                    reflected_sensor_data[6] > obstacle_avoidance_threshold ||
-                    reflected_sensor_data[7] > obstacle_avoidance_threshold) {
-                // Ignore the rear sensors when calculating the heading
-                reflected_sensor_data[2] = 0;
-                reflected_sensor_data[3] = 0;
-                reflected_sensor_data[4] = 0;
-                reflected_sensor_data[5] = 0;
-
-                // Turn 180 degrees away from the sensed obstacle
-                int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
-
-                // Normalise
-                heading %= 360;
-
-                if(heading < -180)
-                    heading += 360;
-
-                if(heading > 180)
-                    heading -= 360;
-                set_program_info("AVOID OBSTACLE");
-                time_based_turn_degrees(1, heading, 1);
-
-                action_timeout = 0;
-                internal_state = turn_away;
-            } else {
-                // Change motor speeds every 1s
-                if(random_walk_timeout >= 2) {
-                    float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
-
-                    float left_motor_speed = previous_left_motor_speed - random_offset;
-                    float right_motor_speed = previous_right_motor_speed + random_offset;
-
-                    float threshold = 0.25;
-
-                    if(left_motor_speed < threshold)
-                        left_motor_speed = threshold;
-
-                    if(right_motor_speed < threshold)
-                        right_motor_speed = threshold;
-
-                    set_left_motor_speed(left_motor_speed);
-                    set_right_motor_speed(right_motor_speed);
-
-                    random_walk_timeout = 0;
-                }
-
-                random_walk_timeout++;
-            }
-        }
-    } else if(internal_state == turn_towards) {
-        if(action_timeout < 4)
-            action_timeout++;
-        else {
-            set_program_info("SAY HELLO");
-            vibrate();
-
-            action_timeout = 0;
-            internal_state = interact;
-        }
-    } else if(internal_state == interact) {
-        if(action_timeout < 4)
-            action_timeout++;
-        else {
-            set_program_info("TURN AROUND");
-            time_based_turn_degrees(1, 180, 1);
-
-            action_timeout = 0;
-            internal_state = turn_away;
-        }
-
-    } else if(internal_state == turn_away) {
-        if(action_timeout < 4)
-            action_timeout++;
-        else {
-            set_program_info("RANDOM WALK");
-            interaction_timeout = 0;
-            internal_state = random_walk;
-        }
-    } else if(internal_state == avoid_obstacle) {
-        if(action_timeout < 4)
-            action_timeout++;
-        else
-            set_program_info("RANDOM WALK");
-        internal_state = random_walk;
-    }
-}
-
-
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// straight_random_walk_with_interaction_program
-
-void straight_random_walk_with_interaction_program()
-{
-
-}
-
-
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// find_space_program
-
-char prog_debug = 1    ;
-float target_wheel_speed;
-int random_walk_bearing;
-
-void find_space_program(char bidirectional)
-{
-    // The find_space_program is a continuous turn-move vector program
-
-    if(program_run_init == 1) {
-        // Setup the LEDs to red
-        set_leds(0x00,0xFF);
-        set_center_led(1,1);
-        program_run_init = 0;
-        random_walk_bearing = rand() % 360;
-    }
-
-    // When step_cycle = 0 we calculate a vector to move to and a target distance
-    if(step_cycle == 0) {
-        struct FloatVector target_vector;
-        target_vector.angle = 0;
-        target_vector.distance = 0;
-        // Check for near robots within range
-        for(int i = 1; i < 8; i++) {
-            if(robots_found[i]) {
-                int res_bearing = robots_heading[i];
-                res_bearing += 180;
-                if(res_bearing > 180) res_bearing -= 360;   
-                target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
-                if(prog_debug) out("Repelled from robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
-            }
-        }
-        if(target_vector.angle!=0){
-           set_leds(0xFF,0xFF);
-           set_center_led(3,1);  
-        }
-        // Check for obstacles
-        char obstacle = 0;
-        int peak_strength = 0;
-        for(int i=0; i<8; i++){
-            if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
-            if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
-        }
-        if(obstacle){  
-           //Choose new random walk bearing
-           set_leds(0x00,0xFF);
-           set_center_led(1,1);
-           random_walk_bearing = rand() % 360;
-           int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
-           int obs_bearing = obstacle_bearing + 180;
-           if(obs_bearing > 180) obs_bearing -= 360;
-           target_vector = addVector(target_vector,obs_bearing,peak_strength);
-           if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d,  resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
-        }   
-        
-        if(target_vector.angle == 0 && target_vector.distance == 0){
-            set_leds(0xFF,0x00);
-            set_center_led(2,1);
-            random_walk_bearing += 180;
-            if(random_walk_bearing > 360) random_walk_bearing -= 360;
-            target_vector.distance = 100;   
-            int current_bearing = 360 - beacon_heading;
-            //Now work out turn needed to face intended heading
-            int target_turn = (random_walk_bearing - current_bearing) % 360;
-            if(target_turn > 180) target_turn -= 360;
-            if(target_turn < -180) target_turn += 360;
-            target_vector.angle = target_turn;
-            if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
-        }
-        char wheel_direction = 1;
-        if(bidirectional){
-            // Allow reverse wheel direction
-            if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
-            else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
-        }
-        //Now turn to angle
-        float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
-        if(target_vector.angle < 0){
-            if(target_vector.angle < -maximum_turn_angle){
-                   target_vector.angle = -maximum_turn_angle;
-                   target_vector.distance = 100;
-            }    
-        }else{
-            if(target_vector.angle > maximum_turn_angle){
-                   target_vector.angle = maximum_turn_angle;
-                   target_vector.distance = 100;
-            } 
-        }
-        //Set the wheel speed for next action
-        if(target_vector.distance < 120) target_wheel_speed = 0.25;
-        else if(target_vector.distance < 240) target_wheel_speed = 0.35;
-        else if(target_vector.distance < 480) target_wheel_speed = 0.45;
-        else if(target_vector.distance < 960) target_wheel_speed = 0.55;
-        else target_wheel_speed = 0.65;
-        if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
-        
-        //Now turn...
-        time_based_turn_degrees(1, (int) target_vector.angle, 1);
-    } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0);
-}
-
-
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// clustering_program
-
-
-void clustering_program(char invert, char bidirectional)
-{
-    // The clustering program is a continuous turn-move vector program
-    // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots
-    // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group
-
-    // Store the robot group: even robots (0) are green, odd robots (1) are red
-    char group = robot_id % 2;
-
-    if(program_run_init == 1) {
-        // Setup the LEDs based on robot_id
-        if(group == 0) {
-            set_leds(0xFF,0x00);
-            set_center_led(2,1);
-        } else {
-            set_leds(0x00,0xFF);
-            set_center_led(1,1);
-        }
-        program_run_init = 0;
-        random_walk_bearing = rand() % 360;
-    }
-
-    // When step_cycle = 0 we calculate a vector to move to and a target distance
-    if(step_cycle == 0) {
-        char avoiding_friend = 0;
-        struct FloatVector target_vector;
-        target_vector.angle = 0;
-        target_vector.distance = 0;
-        // Check for near robots within range
-        for(int i = 1; i < 8; i++) {
-            if(robots_found[i]) {
-                // Determine if the robot is an attractor or a repellor
-                char attract = 0;
-                if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1;
-                // Avoid very close attractors to stop collisions
-                if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;}
-                int res_bearing = robots_heading[i];
-                if(attract==0){
-                    res_bearing += 180;
-                    if(res_bearing > 180) res_bearing -= 360;   
-                }
-                target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
-                if(prog_debug) {
-                    if(attract) {
-                        out("Attracted to robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
-                    } else {
-                        out("Repelled from robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
-                    }
-                }
-            }
-        }
-        
-        // Check for obstacles
-        char obstacle = 0;
-        int peak_strength = 0;
-        for(int i=0; i<8; i++){
-            if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
-            if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
-        }
-        if(obstacle){  
-           //Choose new random walk bearing
-           random_walk_bearing = rand() % 360;
-           int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
-           int obs_bearing = obstacle_bearing + 180;
-           if(obs_bearing > 180) obs_bearing -= 360;
-           target_vector = addVector(target_vector,obs_bearing,peak_strength);
-           if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d,  resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
-        }   
-        if(target_vector.angle == 0 && target_vector.distance == 0){
-            //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing
-            if(rand() % 100 > 97) random_walk_bearing = rand() % 360;
-            target_vector.distance = 100;   
-            int current_bearing = 360 - beacon_heading;
-            //Now work out turn needed to face intended heading
-            int target_turn = (random_walk_bearing - current_bearing) % 360;
-            if(target_turn > 180) target_turn -= 360;
-            if(target_turn < -180) target_turn += 360;
-            target_vector.angle = target_turn;
-            if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
-        }
-        char wheel_direction = 1;
-        if(bidirectional){
-            // Allow reverse wheel direction
-            if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
-            else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
-        }
-        //Now turn to angle
-        float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
-        if(target_vector.angle < 0){
-            if(target_vector.angle < -maximum_turn_angle){
-                   target_vector.angle = -maximum_turn_angle;
-                   target_vector.distance = 100;
-            }    
-        }else{
-            if(target_vector.angle > maximum_turn_angle){
-                   target_vector.angle = maximum_turn_angle;
-                   target_vector.distance = 100;
-            } 
-        }
-        if(avoiding_friend) target_vector.distance = 100;
-        //Set the wheel speed for next action
-        if(target_vector.distance < 120) target_wheel_speed = 0.25;
-        else if(target_vector.distance < 240) target_wheel_speed = 0.35;
-        else if(target_vector.distance < 480) target_wheel_speed = 0.5;
-        else if(target_vector.distance < 960) target_wheel_speed = 0.65;
-        else target_wheel_speed = 0.85;
-        if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
-        
-        //Now turn...
-        time_based_turn_degrees(1, (int) target_vector.angle, 1);
-    } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0);
-}
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// stop_program - Pauses robot
-
-void stop_program()
-{
-    if(program_run_init == 1) {
-        save_led_states();
-        set_leds(0,0);
-        set_center_led(0,0);
-        stop();
-        program_run_init = 0;
-    }
-}
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// tag_game_program
-
-enum tag_game_role {hunter, hunted};
-enum tag_game_role role;
-int tag_distance = 500;
-char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters
-Timeout hunter_timeout; // Timeout before robots switch back to being hunted
-char initial_hunter = 2; // Robot with this ID is permanently a hunter
-
-// Resets hunter robots to hunted after timeout
-void role_reset()
-{
-    role = hunted;
-    set_program_info("HUNTED");
-    // Clear memory of hunters
-    for(int i = 0; i < 8; i++)
-        hunters[i] = 0;
-}
-
-void tag_game_program()
-{
-    // Initialisation
-    if(program_run_init == 1) {
-        
-        // Set robot roles
-        if(robot_id == initial_hunter){
-            role = hunter;
-            set_program_info("FIRST HUNTER");
-        }
-        else {
-            role = hunted;
-            set_program_info("HUNTED");
-        }
-        // Clear memory of hunters    
-        for(int i = 0; i < 8; i++)
-            hunters[i] = 0;
-
-        program_run_init = 0;
-    }
-    
-    // Bias forward movement
-    float left_motor_speed = 0.5;
-    float right_motor_speed = 0.5;
-    
-    // Check the front sensors for obstacles
-    if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
-       reflected_sensor_data[1] > obstacle_avoidance_threshold ||
-       reflected_sensor_data[6] > obstacle_avoidance_threshold ||
-       reflected_sensor_data[7] > obstacle_avoidance_threshold)
-    {                
-        // Ignore the rear sensors when calculating the heading
-        reflected_sensor_data[2] = 0;
-        reflected_sensor_data[3] = 0;
-        reflected_sensor_data[4] = 0;
-        reflected_sensor_data[5] = 0;
-
-        // Get heading of sensed obstacle
-        int heading = get_bearing_from_ir_array(reflected_sensor_data);
-        
-        // Turn in opposite direction
-        if(heading > 0 && heading < 90)
-        {
-            left_motor_speed -= 0.75;
-            right_motor_speed += 0.5;
-        }
-        else if(heading < 0 && heading > -90)
-        {
-            left_motor_speed += 0.5;
-            right_motor_speed -= 0.75;
-        }
-        
-        set_left_motor_speed(left_motor_speed);
-        set_right_motor_speed(right_motor_speed);
-        
-        // Return early - obstacle avoidance is the top priority
-        return;
-    }
-        
-    int closest_robot = -1;
-    unsigned short shortest_distance = 0;
-
-    // Check whether there are any other robots within range
-    for(int i = 0; i < 8; i++)
-    {
-        if(robots_found[i])
-        {
-            if(robots_distance[i] > shortest_distance)
-            {
-                shortest_distance = robots_distance[i];
-                closest_robot = i;
-            }
-        }
-    }
-    
-    // If the closest robot is within tag distance, this robot has been tagged
-    if(shortest_distance > tag_distance)
-    {
-        // Switch role to hunter
-        if(role == hunted)
-        {
-            role = hunter;
-            set_program_info("NEW HUNTER");
-            // Reset to hunted after 10 seconds
-            hunter_timeout.attach(&role_reset, 10);
-        }
-        
-        // Keep a record of which robot tagged them, so hunters do not chase hunters
-        // Unless they are the initial hunter, who is aggresive towards everyone
-        if(robot_id != initial_hunter)
-            hunters[closest_robot] = 1;
-    }
-        
-    if(role == hunter)
-    {
-        // Set LEDS to red
-        set_leds(0x00, 0xFF);
-        set_center_led(1, 1);
-        
-        if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters
-        {
-            // Turn towards closest hunted robot (unless it is straight ahead, or behind)            
-            if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90)
-            {
-                left_motor_speed += 0.5;
-                right_motor_speed -= 0.5;
-            }
-            else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90)
-            {
-                left_motor_speed -= 0.5;
-                right_motor_speed += 0.5;
-            }
-        }
-        
-        set_left_motor_speed(left_motor_speed);
-        set_right_motor_speed(right_motor_speed);
-    }
-    else // role == hunted
-    {
-        // Set LEDs to green        
-        set_leds(0xFF, 0x00);
-        set_center_led(2, 1);
-        
-        // Avoid everyone
-        if(closest_robot >= 0)
-        {
-            // Turn away from closest robot
-            if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90)
-            {
-                left_motor_speed -= 0.5;
-                right_motor_speed += 0.5;
-            }
-            else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90)
-            {
-                left_motor_speed += 0.5;
-                right_motor_speed -= 0.5;
-            }
-        }
-        
-        set_left_motor_speed(left_motor_speed);
-        set_right_motor_speed(right_motor_speed);
-    }
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// flocking_program
-
-void flocking_program()
-{
-    char display_line[16] = "               ";      
-    int average_heading = 0;
-    int number_of_neighbours = 0;
-    int number_of_flocking_headings = 0;
-    int target_heading = 0;
-    int angles = 0;    
-    int ir_sensor_threshold = 600;
-    int sensors_activated = 0;
-    
-    struct FloatVector obstacle_vector;
-    obstacle_vector.angle = 0;
-    obstacle_vector.distance = 0;
-    
-    struct FloatVector cohesion_vector;
-    cohesion_vector.angle = 0;
-    cohesion_vector.distance = 0;
-    
-    for(int i = 0; i < 8; i++)
-    {
-        if(reflected_sensor_data[i] > ir_sensor_threshold)
-        {            
-            obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
-            sensors_activated++;
-        }
-                
-        if(robots_found[i])
-        {
-            // -180 degrees is reserved for "no byte received"
-            if(flocking_headings[i] != -180)
-            {
-                average_heading += flocking_headings[i];
-                number_of_flocking_headings++;
-            }
-            
-            cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
-            number_of_neighbours++;
-        }
-    }
-    
-    cohesion_vector.distance /= number_of_neighbours; // Normalise        
-    obstacle_vector.distance /= sensors_activated; // Normalise
-    
-    int obstacle_avoidance_angle;
-    
-    if(sensors_activated > 0)
-    {
-        obstacle_avoidance_angle = obstacle_vector.angle + 180;
-                
-        if(obstacle_avoidance_angle > 180)
-            obstacle_avoidance_angle -= 360;
-        if(obstacle_avoidance_angle < -180)
-            obstacle_avoidance_angle += 360;
-        
-        target_heading += obstacle_avoidance_angle;
-        angles++;
-    }
-
-    int cohesion_angle;
-    
-    // Don't bother performing cohesion if robots are already close enough
-    if(number_of_neighbours > 0 && cohesion_vector.distance < 200)
-    {            
-        cohesion_angle = cohesion_vector.angle;
-        
-        if(cohesion_angle > 180)
-            cohesion_angle -= 360;
-        if(cohesion_angle < -180)
-            cohesion_angle += 360;
-        
-        target_heading += cohesion_angle;
-        angles++;
-    }
-    
-    int relative_flocking_heading;
-    
-    if(number_of_flocking_headings > 0)
-    {        
-        average_heading /= number_of_flocking_headings;            
-
-        relative_flocking_heading = beacon_heading - average_heading;
-        
-        if(relative_flocking_heading > 180)
-            relative_flocking_heading -= 360;
-        if(relative_flocking_heading < -180)
-            relative_flocking_heading += 360;
-        
-        target_heading += relative_flocking_heading;
-        angles++;
-    }
-        
-    if(angles > 0)
-        target_heading /= angles; // Calculate average
-
-    float left_motor_speed = 0.25;
-    float right_motor_speed = 0.25;
-
-    if(target_heading > 22.5)
-        right_motor_speed *= -1;
-    else if(target_heading < -22.5)
-        left_motor_speed *= -1;
-        
-    set_left_motor_speed(left_motor_speed);
-    set_right_motor_speed(right_motor_speed);
-
-    // Only transmit beacon heading if the beacon is visible
-    if(beacon_found)
-    {        
-        float degrees_per_value = 256.0f / 360.0f;
-        char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value
-        
-        bt.putc(beacon_heading_byte);
-    }
-    
-    sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading);
- 
-    set_program_info(display_line);
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// generic_program - Framework for building typical programs
-
-void generic_program()
-{
-    // Do something on the first run of a program
-    if(program_run_init == 1) {
-        // Initialisation code goes here...
-
-        program_run_init = 0;
-    }
-
-    // step_cycle is either zero or one; use this avoid estimating bearings on the cycle after a turn (as the results will be skewed by the turn)
-    if(step_cycle == 0) {
-        // Do something based on sensor data (eg turn)
-    } else {
-        // Do something ignoring sensor data (eg move, or nothing!)
-    }
-
-}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/programs.h
--- a/BeautifulMeme/programs.h	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,22 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// programs.h - Various PsiSwarm Programs for Beautiful Meme Project
-
-#ifndef PROGRAMS_H
-#define PROGRAMS_H
-
-void recharging_program(void);
-void head_to_bearing_program(int target_bearing);
-void curved_random_walk_with_interaction_program(void);
-void straight_random_walk_with_interaction_program(void);
-void find_space_program(char bidirectional);
-void clustering_program(char invert, char bidirectional);
-void stop_program(void);
-void role_reset(void);
-void tag_game_program(void);
-void flocking_program(void);
-
-#endif
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/vector.cpp
--- a/BeautifulMeme/vector.cpp	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// vector.cpp - Functions for calculating floating point bearing vectors
-
-#include "bmeme.h"
-
-
-struct FloatVector addVector(struct FloatVector in_vector, int angle, int distance){
-    struct FloatVector out_vector;
-    float sin_component_old = sin(in_vector.angle * TO_RAD) * in_vector.distance;
-    float cos_component_old = cos(in_vector.angle * TO_RAD) * in_vector.distance;
-    float sin_component_new = sin(angle * TO_RAD) * distance;
-    float cos_component_new = cos(angle * TO_RAD) * distance;
-    float sin_component_sum = sin_component_old + sin_component_new;
-    float cos_component_sum = cos_component_old + cos_component_new;
-    out_vector.distance = sqrt((sin_component_sum * sin_component_sum) + (cos_component_sum * cos_component_sum));
-    out_vector.angle = atan2(sin_component_sum ,cos_component_sum) * TO_DEG;
-    //out("Angle:%f  Distance:%f\n",out_vector.angle,out_vector.distance);
-    return out_vector;
-}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/vector.h
--- a/BeautifulMeme/vector.h	Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,21 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
-/// University of York
-
-// vector.h - Functions for calculating floating point bearing vectors
-
-#ifndef VECTOR_H
-#define VECTOR_H
-
-#define TO_DEG 57.2958
-#define TO_RAD 0.017453
-
-struct FloatVector{
-    float angle;
-    float distance;   
-};
-
-struct FloatVector addVector(struct FloatVector in_Vector, int angle, int distance);
-
-#endif
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 PsiSwarm.lib
--- a/PsiSwarm.lib	Tue Mar 15 00:58:43 2016 +0000
+++ b/PsiSwarm.lib	Mon Jun 20 13:36:30 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/jah128/code/PsiSwarmLibrary/#7c0d1f581757
+https://developer.mbed.org/teams/UKESF-Headstart-Summer-School/code/PsiSwarm-Headstart/#dc77a25f29de
diff -r 513457c1ad12 -r 7fa2c47d73a2 main.cpp
--- a/main.cpp	Tue Mar 15 00:58:43 2016 +0000
+++ b/main.cpp	Mon Jun 20 13:36:30 2016 +0000
@@ -1,70 +1,87 @@
-/***********************************************************************
-**  ██████╗ ███████╗██╗███████╗██╗    ██╗ █████╗ ██████╗ ███╗   ███╗  **
-**  ██╔══██╗██╔════╝██║██╔════╝██║    ██║██╔══██╗██╔══██╗████╗ ████║  **
-**  ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║  **
-**  ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║  **
-**  ██║     ███████║██║███████║╚███╔███╔╝██║  ██║██║  ██║██║ ╚═╝ ██║  **
-**  ╚═╝     ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝  ╚═╝╚═╝  ╚═╝╚═╝     ╚═╝  **
-************************************************************************
-**(C) Dr James Hilder - York Robotics Laboratory - University of York **
-***********************************************************************/
+/**********************************************************
+* UKESF Headstart Summer School - Robot Programming Lab   *
+**********************************************************/
 
-/// PsiSwarm Blank Example Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
-/// University of York
+// Note: A line beginning with // is a 'comment' and is
+// ignored by the compiler [it is shown in green text in the IDE]
 
-/// Include main.h - this includes psiswarm.h all the other necessary core files
+// Include main.h - this contains the code needed to setup
+// the Psi-Swarm robot and its hardware
 #include "main.h"
 
-char * program_name = "Blank";
+// You can change the author name to saomething else if you want - just make sure it is no more than 16 characters long
 char * author_name  = "YRL";
-char * version_name = "0.41";
-
-void user_code_loop()
-{
-    wait(1);   
-}
-
-///Place user code here that should be run after initialisation but before the main loop
-void user_code_setup()
-{
-    wait(1);
-    display.clear_display();
-    display.set_position(0,0);
-    display.write_string("No Code");  
-    
-    //bmeme_user_code_setup(); 
-}
-
-/// Code goes here to handle what should happen when the user switch is pressed
-void handle_switch_event(char switch_state)
-{
-    /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed
-    /// NB For maximum compatability it is recommended to minimise reliance on center button press
-
-    //bmeme_handle_switch_event(switch_state);
-}
-
-void handle_user_serial_message(char * message, char length, char interface)
-{
-    // This is where user code for handling a (non-system) serial message should go
-    //
-    // message = pointer to message char array
-    // length = length of message
-    // interface = 0 for PC serial connection, 1 for Bluetooth
-    
-    //bmeme_handle_user_serial_message(message, length, interface);
-}
 
 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
 int main()
 {
-    ///init() in psiswarm.cpp sets up the robot
+    // init() in psiswarm.cpp sets up the robot - we need to run this line of code before anything else or the robot won't work.
     init();
-    user_code_setup();
-    user_code_running = 1;
-    while(1) {
-        user_code_loop();
+
+    // We shall create a for-loop to try to move the robot in a 50cm square.
+    for(int i=0; i<4; i++){
+      
+      // Move the robot forward 50cm
+      move_forward(50);
+      
+      // Wait one second
+      wait(1.0);
+      
+      // Turn the robot right 90 degrees
+      turn_right();
+      
+      // Wait for one second
+      wait(1.0);
     }
-}
\ No newline at end of file
+    
+    // When we have finished our code, we want to leave the MBED in an endless loop.
+    while(1){}
+}
+
+/**
+* void move_forward(int distance)
+* This function should move the robot forward for the distance
+* (in centimeter) specified by distance
+*/
+void move_forward(int distance)
+{
+    float left_motor_speed = 0.4;
+    float right_motor_speed = 0.4;
+    float cm_per_second = 25.0;
+    float delay = distance / cm_per_second;
+    set_motor_speed(left_motor_speed, right_motor_speed);
+    wait(delay);
+    brake();
+}
+
+/**
+* void turn_right()
+* This function should turn the robot right by 90 degrees.
+*/
+void turn_right()
+{
+    float speed = 0.2;
+    float degrees_per_second = 150.0;
+    float delay = 90 / degrees_per_second;
+    turn(speed);
+    wait(delay);
+    brake();
+}
+
+/**
+* void turn_left()
+* This function should turn the robot left by 90 degrees.  You will need to fill in the code here 
+* HINT: You can use Ctrl-C and Ctrl-V to copy\paste blocks of code!
+*/
+void turn_left()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// You can ignore the code beyond this point: we need these variables and functions to make the code
+// compile without errors, but we aren't using the functions for this lab session.
+char * program_name = "Headstart";
+char * version_name = "0.5";
+void handle_switch_event(char switch_state){}
+void handle_user_serial_message(char * message, char length, char interface){}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 main.h
--- a/main.h	Tue Mar 15 00:58:43 2016 +0000
+++ b/main.h	Mon Jun 20 13:36:30 2016 +0000
@@ -1,30 +1,17 @@
-/*********************************************************************** 
-**  ██████╗ ███████╗██╗███████╗██╗    ██╗ █████╗ ██████╗ ███╗   ███╗  **
-**  ██╔══██╗██╔════╝██║██╔════╝██║    ██║██╔══██╗██╔══██╗████╗ ████║  **
-**  ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║  **
-**  ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║  **
-**  ██║     ███████║██║███████║╚███╔███╔╝██║  ██║██║  ██║██║ ╚═╝ ██║  **
-**  ╚═╝     ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝  ╚═╝╚═╝  ╚═╝╚═╝     ╚═╝  **
-************************************************************************
-**(C) Dr James Hilder - York Robotics Laboratory - University of York **
-***********************************************************************/
-
-/// PsiSwarm Blank Example Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
-/// University of York
+/**********************************************************
+* UKESF Headstart Summer School - Robot Programming Lab   *
+**********************************************************/
 
 #ifndef MAIN_H
 #define MAIN_H
 
 #include "psiswarm.h"
-//#include "bmeme.h"
 
 int main(void);
-void user_code_setup(void);
-void user_code_loop(void);
 void handle_switch_event(char switch_state); 
 void handle_user_serial_message(char * message, char length, char interface);
-
+void move_forward(int distance);
+void turn_right();
+void turn_left();
 
 #endif
\ No newline at end of file