File content as of revision 0:46cd1498a39a:

// York Robotics Laboratory
// 3-Pi Swarm Robot Test Code
// Version 0.1 - 11th November, 2013
// James Hilder, University of York 

#include "mbed.h"
#include "s3pi.h"

s3pi s3pi;

int line_data[5];
char shift_mode = 0;
char current_mode = 0;
char cycle_mode = 0;

void switch_pressed() {
    //Switch pressed
    //1 = Center
    //2 = Right
    //4 = Left
    //8 = Down
    //16 = Up
    char switches = s3pi.get_switches();
    //s3pi.locate(0,1);
    //s3pi.printf("%d  ", switches);
    
    if(switches == 1) {
        shift_mode = 1;
        cycle_mode = 1-cycle_mode;
    }
    
    if(switches == 4) {
        shift_mode = 1;
        if(current_mode == 0) current_mode = 17;
        else current_mode --;
    }
    if(switches == 2) {
        shift_mode = 1;
        current_mode ++;
        if (current_mode > 17) current_mode =0;
    }
}

void init() {
    //Initialisation routine for 3-Pi Robot
    //Displays a message on the screen and flashes the central LED
    //Calibrates the gyro and accelerometer
    //Make sure robot is flat and stationary when this code is run (calibration starts after about 1 second to allow robot to settle)
    s3pi.play_tune("MSCEG>C",7);
    s3pi.cls();
    s3pi.enable_cled(1);
    s3pi.set_cled_colour(200,0,0);
    s3pi.printf("  YORK  ");
    s3pi.locate(0,1);
    s3pi.printf("ROBOTLAB");
    wait(0.3);
    s3pi.set_cled_colour(0,200,0);
    wait(0.3);
    s3pi.set_cled_colour(0,0,200);
    wait(0.3);
    s3pi.cls();
    s3pi.set_cled_colour(20,20,20);
    s3pi.printf("3piSwarm");
    s3pi.locate(0,1);
    s3pi.printf("ID : %d ",s3pi.get_id());
    if(s3pi.calibrate_magnetometer() != 0){
        s3pi.cls();
        s3pi.locate(0,0);
        s3pi.printf("Mag Cal ");
        s3pi.locate(0,1);
        s3pi.printf("Failed  ");
        wait(0.5);
    }else{
        if(s3pi.calibrate_gyro() == 0){
            s3pi.cls();
            s3pi.locate(0,0);
            s3pi.printf("Gyro Cal");
            s3pi.locate(0,1);
            s3pi.printf("Failed  ");
            wait(0.5);
        }else{
            if(s3pi.calibrate_accelerometer() == 0){
                s3pi.cls();
                s3pi.locate(0,0);
                s3pi.printf("Acc. Cal");
                s3pi.locate(0,1);
                s3pi.printf("Failed  ");
                wait(0.5);
            }
        }
    }
    wait(1.5);
    s3pi.cls();
    s3pi.set_cled_colour(10,20,30);
    wait(1.5);
}

int main() {
    init();
    s3pi.locate(0,0);
    s3pi.printf("  Gyro:");
    int count = 0;
    float sum = 0;
    while(1) {
        count++;
        if(shift_mode == 1) {
            shift_mode = 0;
            count = 0;
            sum = 0;
            s3pi.cls();
            s3pi.locate(0,0);
            if(cycle_mode == 1) {
                s3pi.printf("CYCLE ON");   
            } 
            else{
            switch(current_mode) {
                case 0: s3pi.printf("  Gyro: "); break;
                case 1: s3pi.printf("  Temp: "); break;
                case 2: s3pi.printf(" Light: "); break;
                case 3: s3pi.printf("  AccX: "); break;
                case 4: s3pi.printf("  AccY: "); break;
                case 5: s3pi.printf("  AccZ: "); break;
                case 6: s3pi.printf("  MagX: "); break;
                case 7: s3pi.printf("  MagY: "); break;
                case 8: s3pi.printf("  MagZ: "); break;
                case 9: s3pi.printf(" BG IR: "); break;
                case 10: s3pi.printf(" Dist1: "); break;
                case 11: s3pi.printf(" Dist2: "); break;
                case 12: s3pi.printf(" Dist3: "); break;
                case 13: s3pi.printf(" Dist4: "); break;
                case 14: s3pi.printf(" Dist5: "); break;
                case 15: s3pi.printf(" Dist6: "); break;
                case 16: s3pi.printf(" Dist7: "); break;
                case 17: s3pi.printf(" Dist8: "); break;
            }
            }
        }
        switch(current_mode) {
            case 0: sum+=s3pi.read_gyro(); break;
            case 1: sum+=s3pi.read_temperature(); break;
            case 2: sum+=s3pi.read_light_sensor(); break;
            case 3: sum+=s3pi.read_accelerometer_x(); break;
            case 4: sum+=s3pi.read_accelerometer_y(); break;
            case 5: sum+=s3pi.read_accelerometer_z(); break;
            case 6: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_x(); break;
            case 7: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_y(); break;
            case 8: s3pi.read_magnetometer(); sum+=s3pi.get_magnetometer_z(); break;
            case 9: sum+=s3pi.read_adc_value(0)+s3pi.read_adc_value(1)+s3pi.read_adc_value(2)+s3pi.read_adc_value(3)+s3pi.read_adc_value(4)+s3pi.read_adc_value(5)+s3pi.read_adc_value(6)+s3pi.read_adc_value(7); break;
            case 10: sum+=s3pi.read_reflected_ir_distance(0); break;
            case 11: sum+=s3pi.read_reflected_ir_distance(1); break;
            case 12: sum+=s3pi.read_reflected_ir_distance(2); break;
            case 13: sum+=s3pi.read_reflected_ir_distance(3); break;
            case 14: sum+=s3pi.read_reflected_ir_distance(4); break;      
            case 15: sum+=s3pi.read_reflected_ir_distance(5); break;
            case 16: sum+=s3pi.read_reflected_ir_distance(6); break;
            case 17: sum+=s3pi.read_reflected_ir_distance(7); break;      
        }
        s3pi.read_raw_sensors(line_data);
        int red = (line_data [0] - 50) / 38;
        int green = (line_data [1] - 50) / 38;
        int blue = (line_data [2] - 50) / 38;
        s3pi.set_cled_colour(red,green,blue);
        s3pi.set_oled_colour(red,green,blue);
        if (count>9){
            sum /= count;
            count = 0;
            if(cycle_mode == 1){
                s3pi.cls();
                s3pi.locate(0,0);
                switch(current_mode) {
                    case 0: s3pi.printf("  Gyro: "); break;
                    case 1: s3pi.printf("  Temp: "); break;
                    case 2: s3pi.printf(" Light: "); break;
                    case 3: s3pi.printf("  AccX: "); break;
                    case 4: s3pi.printf("  AccY: "); break;
                    case 5: s3pi.printf("  AccZ: "); break;
                    case 6: s3pi.printf("  MagX: "); break;
                    case 7: s3pi.printf("  MagY: "); break;
                    case 8: s3pi.printf("  MagZ: "); break;
                    case 9: s3pi.printf(" BG IR: "); break;
                    case 10: s3pi.printf(" Dist1: "); break;
                    case 11: s3pi.printf(" Dist2: "); break;
                    case 12: s3pi.printf(" Dist3: "); break;
                    case 13: s3pi.printf(" Dist4: "); break;
                    case 14: s3pi.printf(" Dist5: "); break;
                    case 15: s3pi.printf(" Dist6: "); break;
                    case 16: s3pi.printf(" Dist7: "); break;
                    case 17: s3pi.printf(" Dist8: "); break;
                }
                current_mode ++;
                if(current_mode == 18) current_mode = 0;
            }
            s3pi.locate(0,1);
            s3pi.printf("%.1f  ",sum);
            sum = 0;
        }
        wait(0.05);
    }
}