File content as of revision 0:9ffe8ebd1c40:

/* University of York Robot Lab Pi Swarm Robot Library
 *
 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
 * 
 * Version 0.4  January 2014
 *
 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
 *
 * Based on the original m3pi library, Copyright (c) 2007-2010 cstyles (see copyright notice at end of file) * 
 */

#include "piswarm.h" 

// Variables
DigitalOut  gpio_led (LED1);
DigitalOut  calibrate_led (LED2);
DigitalOut  adc_led (LED3);
DigitalOut  interrupt_led (LED4);
InterruptIn expansion_interrupt (p29); 
Timeout debounce_timeout;
Timeout led_timeout;
Ticker calibrate_ticker;
char id;

char oled_array [10];
char enable_ir_ldo = 0;
char enable_led_ldo = 0;
char calibrate_led_state = 1;
char switches = 0;
char cled_red = 0;
char cled_green = 0;
char cled_blue = 0;
char oled_red = 0;
char oled_green = 0;
char oled_blue = 0;
char cled_enable = 0;
char cled_brightness = CENTER_LED_BRIGHTNESS;  // The default brightness value:  must be between 0 (dimmest) and 100 (full brightness)
char oled_brightness = OUTER_LED_BRIGHTNESS;  // The default brightness value:  must be between 0 (dimmest) and 100 (full brightness)
float gyro_cal = 3.3;
float gyro_error = 0;
float acc_x_cal = 3350;
float acc_y_cal = 3350;
float acc_z_cal = 3350;
float left_speed = 0;
float right_speed = 0;
signed short mag_values [3];


PiSwarm::PiSwarm() :  Stream("PiSwarm"), _ser(p9, p10), _oled_r(p26), _oled_g(p25), _oled_b(p24), _cled_r(p23), _cled_g(p22), _cled_b(p21), _gyro(p16), _accel_x(p19), _accel_y(p18), _accel_z(p17), _temperature(p20), _light(p15), _i2c(p28,p27), _irpulse_1(p12), _irpulse_2(p13), _rf(p5,p6,p7,p8,p11)  {
    setup();
    reset();
}

int PiSwarm::get_oled_colour(){
    return (oled_red << 16) + (oled_green << 8) + oled_blue;
}

int PiSwarm::get_cled_colour(){
    return (cled_red << 16) + (cled_green << 8) + cled_blue;
}

void PiSwarm::set_oled(char oled, char value){
    oled_array[oled]=value;
}

void PiSwarm::set_oleds(char oled_0, char oled_1, char oled_2, char oled_3, char oled_4, char oled_5, char oled_6, char oled_7, char oled_8, char oled_9){
    oled_array[0]=oled_0;
    oled_array[1]=oled_1;
    oled_array[2]=oled_2;
    oled_array[3]=oled_3;
    oled_array[4]=oled_4;
    oled_array[5]=oled_5;
    oled_array[6]=oled_6;
    oled_array[7]=oled_7;
    oled_array[8]=oled_8;
    oled_array[9]=oled_9;
    activate_oleds();
}

// Switches off all outer LEDs
void PiSwarm::turn_off_all_oleds(){
    for(int i=0;i<10;i++){
        oled_array[i]=0;
    }
    activate_oleds();
    enable_led_ldo = 0;
    enable_ldo_outputs();
    char data[3];
    data[0]=0x88;  //Write to bank 1 then bank 2
    data[1]=0x00;  //Reset everything in bank 1
    data[2]=0x00;  //Reset everything in bank 2
    _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);    
}


// Sends the messages to enable/disable outer LEDs
void PiSwarm::activate_oleds(){
    if(enable_led_ldo == 0){
        enable_led_ldo = 1;
        enable_ldo_outputs();
    }
    char data[3];
    
    data[0]=0x88;  //Write to bank 1 then bank 2
    data[1]=0x00;  //Reset everything in bank 1
    data[2]=0x00;  //Reset everything in bank 2
    if(oled_array[0]>0) data[1]+=1;
    if(oled_array[1]>0) data[1]+=2;
    if(oled_array[2]>0) data[1]+=4;
    if(oled_array[3]>0) data[1]+=8;
    if(oled_array[4]>0) data[1]+=16;
    if(oled_array[5]>0) data[1]+=32;
    if(oled_array[6]>0) data[1]+=64;
    if(oled_array[7]>0) data[1]+=128;
    if(oled_array[8]>0) data[2]+=1;
    if(oled_array[9]>0) data[2]+=2;
    _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);    
}

void PiSwarm::test_oled(){
    enable_led_ldo = 1;
    enable_ldo_outputs();
    set_oled_colour(100,100,100);
    char data[2];
    data[0] = 0x09;     //Address for PCA9505 Output Port 1
    data[1] = 3;
    _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
}

void PiSwarm::send_rf_message(char* message, char length){
    _rf.sendString(length, message);
}

void PiSwarm::set_oled_colour( char red, char green, char blue ){
    oled_red = red;
    oled_green = green;
    oled_blue = blue;
    _oled_r.pulsewidth_us(red);
    _oled_g.pulsewidth_us(green);
    _oled_b.pulsewidth_us(blue);
}

void PiSwarm::set_cled_colour( char red, char green, char blue ){
    cled_red = red;
    cled_green = green;
    cled_blue = blue;
    if(cled_enable != 0) enable_cled(1);
}

void PiSwarm::enable_cled( char enable ){
    cled_enable = enable;
    if(enable != 0) {
        _cled_r.pulsewidth_us(cled_red);
        _cled_g.pulsewidth_us(cled_green);
        _cled_b.pulsewidth_us(cled_blue);
    }else{
        _cled_r.pulsewidth_us(0);
        _cled_g.pulsewidth_us(0);
        _cled_b.pulsewidth_us(0);
    }
}

void PiSwarm::setup () {
    _ser.baud(115200);
    set_oled_brightness (oled_brightness);
    set_cled_brightness (cled_brightness);
    gpio_led = setup_expansion_ic();
    adc_led = setup_adc();
}

void PiSwarm::setup_radio() {
    //Setup RF transceiver
    _rf.rf_init();
    _rf.setFrequency(RF_FREQUENCY);
    _rf.setDatarate(RF_DATARATE);
}

void PiSwarm::enable_ldo_outputs () {
    char data[2];
    data[0] = 0x0A;     //Address for PCA9505 Output Port 2
    data[1] = 0;
    if(enable_led_ldo) data[1] +=128;
    if(enable_ir_ldo) data[1] +=64;
    _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
}

int PiSwarm::setup_expansion_ic () {
    //Expansion IC is NXP PCA9505
    //Address is 0x40 (defined by EXPANSION_IC_ADDRESS)
    //Block IO 0 : 7-0 Are OLED Outputs
    //Block IO 1 : 7 is NC.  6-2 are Cursor switches (interrupt inputs).  1 and 0 are OLED outputs.
    //Block IO 2 : 7 and 6 are LDO enable outputs.  5 - 1 are ID switch. 0 is NC.
    //Block IO 3 and 4 are for the expansion connector (not assigned here)
    char data [4];
    data [0] = 0x98;    //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command)
    data [1] = 0x00;    //All 8 pins in block 0 are outputs (0)
    data [2] = 0xFC;    //Pins 1 & 0 are outputs, rest are inputs (or NC)
    data [3] = 0x3F;    //Pins 7 & 6 are outputs, rest are inputs (or NC)
    //If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands
    int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
    
    // test_val should return 0 for correctly acknowledged response
   
    //Invert the polarity for switch inputs (they connect to ground when pressed\on)
    data [0] = 0x90;    //Write to polarity inversion register using auto increment
    data [1] = 0x00;
    data [2] = 0x7C;    //Invert pins 6-2 for cursor switches
    data [3] = 0x3E;    //Invert pins 5-1 for ID switch
    _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
    
    //Setup the interrupt masks.  By default, only the direction switches trigger an interrupt
    data [0] = 0xA0;
    data [1] = 0xFF;
    data [2] = 0x83;    // Disable mask on pins 6-2 for cursor switches
    data [3] = 0xFF;
    _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);

    //Read the ID switches
    data [0] = 0x80;    //Read input registers
    char read_data [5];
    _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
    _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
    id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift   
    
    //Setup interrupt handler
    expansion_interrupt.mode(PullUp);                                    // Pull Up by default (needed on v1 PCB - no pullup resistor)
    expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler);   // Add local handler on falling edge to read switch
    return test_val;
}

char PiSwarm::setup_adc ( void ){
    //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
    return 0;
}

float PiSwarm::read_reflected_ir_distance ( char index ){
    //This function attempts to read a real-world distance approximation using the IR proximity sensors
    //1.  Check that the IR LDO regulator is on, enable if it isn't
    if(enable_ir_ldo == 0){
        enable_ir_ldo = 1;
        enable_ldo_outputs();
    }
    //2.  Read the ADC value without IR emitter on
    unsigned short background_value = read_adc_value ( index );
    //3.  Enable the relevant IR emitter by turning on its pulse output
    switch(index){
        case 0: case 1: case 6: case 7:
        _irpulse_1 = 1;
        break;
        case 2: case 3: case 4: case 5:
        _irpulse_2 = 1;
        break;
    }
    wait_us(500);               
    //4.  Read the ADC value now IR emitter is on
    unsigned short strong_value = read_adc_value ( index );
    //5.  Turn off IR emitter
       switch(index){
        case 0: case 1: case 6: case 7:
        _irpulse_1 = 0;
        break;
        case 2: case 3: case 4: case 5:
        _irpulse_2 = 0;
        break;
    }
    //6.  Estimate distance based on 2 values
    float app_distance = 4000 + background_value - strong_value;
    if(app_distance < 0) app_distance = 0;
    // Very approximate: root value, divide by 2, approx distance in mm
    app_distance = sqrt (app_distance) / 2.0;
    // Then add adjustment value if value >27
    if(app_distance > 27) {
        float shift = pow(app_distance - 27,3);
        app_distance += shift;
    }
    if(app_distance > 90) app_distance = 100.0;
    return app_distance;
}

unsigned short PiSwarm::read_adc_value ( char index ){
    short value = 0;
    // Read a single value from the ADC
    if(index<8){
        char apb[1];
        char data[2];
        switch(index){
            case 0: apb[0]=0x80; break;
            case 1: apb[0]=0x90; break;
            case 2: apb[0]=0xA0; break;
            case 3: apb[0]=0xB0; break;
            case 4: apb[0]=0xC0; break;
            case 5: apb[0]=0xD0; break;
            case 6: apb[0]=0xE0; break;
            case 7: apb[0]=0xF0; break;
        }
        _i2c.write(ADC_ADDRESS,apb,1,false);
        _i2c.read(ADC_ADDRESS,data,2,false);
        value=((data[0] % 16)<<8)+data[1];
        if(value > 4096) value=4096;
        value=4096-value;
    }
    return value;
}

void PiSwarm::interrupt_timeout_event ( void ){
    //Read switches
    char data [1];
    data [0] = 0x80;    //Read input registers
    char read_data [5];
    _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
    _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
    switches = (read_data[1] & 0x7C) >> 2;
    led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1);               // Switch off the LED after 0.1s
}

void PiSwarm::led_timeout_event ( void ){
    interrupt_led = 0;
}

void PiSwarm::expansion_interrupt_handler ( void ){
    interrupt_led = 1;
    debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce
    debounce_timeout.add_function(&switch_pressed);                       // Call the switch pressed routine (in main) when done
}

char PiSwarm::get_id () {
    return id;
}

char PiSwarm::get_switches () {
    return switches;
}

void PiSwarm::set_cled_brightness ( char brightness ) {
    if( brightness > 100 ) brightness = 100;
    // Brightness is set by adjusting period of PWM
    // Max brightness is when total period = 256 uS
    // When brightness is 90 total period =  274 uS
    // When brightness is 50 total period =  546 uS
    // When brightness is 10 total period = 1138 uS
    // When brightness is 0  total period = 1336 uS
    // When calibrate_colours = 1, red = 2x normal, green = 2x normal
    cled_brightness = brightness;
    int cled_period = (104 - brightness);
    cled_period *= cled_period;
    cled_period /= 10;
    cled_period += 255;
    if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2); 
    else _cled_r.period_us(cled_period);
    if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2); 
    else _cled_g.period_us(cled_period);
    _cled_b.period_us(cled_period);
}

void PiSwarm::set_oled_brightness ( char brightness ) {
    if ( brightness > 100 ) brightness = 100;
    // Brightness is set by adjusting period of PWM
    // Max brightness is when total period = 256 uS
    // When brightness is 90 total period =  274 uS
    // When brightness is 50 total period =  546 uS
    // When brightness is 10 total period = 1138 uS
    // When brightness is 0  total period = 1336 uS
    oled_brightness = brightness;
    int oled_period = (104 - brightness);
    oled_period *= oled_period;
    oled_period /= 10;
    oled_period += 255;    
    if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
    else _oled_r.period_us(oled_period);
    if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
    else _oled_g.period_us(oled_period);
    _oled_b.period_us(oled_period);
}

    
float PiSwarm::read_temperature ( void ){
    // Temperature Sensor is a Microchip MCP9701T-E/LT
    // 19.5mV/C  0C = 400mV
    float r_temp = _temperature.read();
    r_temp -= 0.1212f;      // 0.4 / 3.3
    r_temp *= 169.231f;  // 3.3 / .0195
    return r_temp;
}
    
char PiSwarm::calibrate_gyro ( void ){
    //This routine attempts to calibrate the offset of the gyro
    int samples = 8;
    float gyro_values[samples];
    calibrate_led = 1;
    calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
    for(int i=0;i<samples;i++){
        gyro_values[i] = _gyro.read();
        wait(0.12);
    }
    char pass = 1;
    float mean = 0;
    float min = 3.5;
    float max = 3.2;
    for(int i=0;i<samples;i++){
        if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed!
        float test_value = 1.50 / gyro_values[i];
        mean += test_value;
        if (test_value > max) max = test_value;
        if (test_value < min) min = test_value;
    }
    if(pass == 1){
        gyro_cal = mean / samples;
        gyro_error = max - min;
        calibrate_ticker.detach();
        calibrate_led = 0;
    }
    return pass;
}    

char PiSwarm::calibrate_accelerometer ( void ){
    //This routine attempts to calibrate the offset and multiplier of the accelerometer
    int samples = 8;
    float acc_x_values[samples];
    float acc_y_values[samples];
    float acc_z_values[samples];
    calibrate_led = 1;
    calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
    for(int i=0;i<samples;i++){
        acc_x_values[i] = _accel_x.read();
        acc_y_values[i] = _accel_y.read();
        acc_z_values[i] = _accel_z.read();
        wait(0.12);
    }
    char pass = 1;
    float x_mean = 0, y_mean=0, z_mean=0;
    float x_min = 3600, y_min=3600, z_min=3600;
    float x_max = 3100, y_max=3100, z_max=3100;
    for(int i=0;i<samples;i++){
        if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
        if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
        if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed!
        
        float x_test_value = 1250 / acc_x_values[i];
        x_mean += x_test_value;
        if (x_test_value > x_max) x_max = x_test_value;
        if (x_test_value < x_min) x_min = x_test_value;
        float y_test_value = 1250 / acc_y_values[i];
        y_mean += y_test_value;
        if (y_test_value > y_max) y_max = y_test_value;
        if (y_test_value < y_min) y_min = y_test_value; 
        float z_test_value = 887 / acc_z_values[i];
        z_mean += z_test_value;
        if (z_test_value > z_max) z_max = z_test_value;
        if (z_test_value < z_min) z_min = z_test_value; 
    }
    if(pass == 1){
        acc_x_cal = x_mean / samples;
        acc_y_cal = y_mean / samples;
        acc_z_cal = z_mean / samples;
        calibrate_ticker.detach();
        calibrate_led = 0;
    }
    return pass;
}   

void PiSwarm::calibrate_ticker_routine ( void ){
    calibrate_led_state = 1 - calibrate_led_state;
    calibrate_led = calibrate_led_state;
}

float PiSwarm::read_gyro ( void ){
    // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro
    // 0.67mV / d.p.s.
    // 1.5V V offset 
    float r_gyro = _gyro.read();
    r_gyro *= gyro_cal;     // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
    r_gyro = 1.5 - r_gyro;  // 1.5V off-set, invert sign
    r_gyro *= 1492.5;       // Convert to DPS (+ve = clockwise, -ve = c.clockwise) 
    return r_gyro;
}
    
float PiSwarm::read_accelerometer_x ( void ){
    // LIS 332 AX Accelerometer - 0.363 V/g - 1.25V 
    
    float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754;    // Return value in mG
    return r_acc_x;
}
    
float PiSwarm::read_accelerometer_y ( void ){
    float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754;    // Return value in mG 
    return r_acc_y;
}
    
float PiSwarm::read_accelerometer_z ( void ){
    float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754;    // Return value in mG
    return r_acc_z;
}


char PiSwarm::calibrate_magnetometer ( void ){
    char command_data[2];
    command_data[0] = 0x11; //Write to CTRL_REG2
    command_data[1] = 0x80; // Enable auto resets
    _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
    command_data[0] = 0x10; //Write to CTRL_REG1
    command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS  NOT-FAST  NORMAL  ACTIVE MODE] 
    return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
}

char PiSwarm::read_magnetometer ( void ){
    char read_data[7];
    char success;
    char command_data[1];
    command_data[0] = 0x00; //Auto-read from register 0x00 (status)
    success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false);
    _i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false);
    mag_values[0] = (read_data[1] << 8) + read_data[2];
    mag_values[1] = (read_data[3] << 8) + read_data[4];
    mag_values[2] = (read_data[5] << 8) + read_data[6];
    return success; 
}
    
signed short PiSwarm::get_magnetometer_x ( void ){
    return mag_values[0];
}
    
signed short PiSwarm::get_magnetometer_y ( void ){
    return mag_values[1];
}
    
signed short PiSwarm::get_magnetometer_z ( void ){
    return mag_values[2];
}

void PiSwarm::write_eeprom_byte ( int address, char data ){
    char write_array[3];
    write_array[0] = address / 256;
    write_array[1] = address % 256;
    write_array[2] = data;
    _i2c.write(EEPROM_ADDRESS, write_array, 3, false);
    //Takes 5ms to write a page: ideally this could be done with a timer or RTOS
    wait(0.005);
}
  
void PiSwarm::write_eeprom_block ( int address, char length, char * data){
    /** Note from datasheet:
    Page write operations are limited to writing bytes within a single physical page,
    regardless of the number of bytes actually being written. Physical page
    boundaries start at addresses that are integer multiples of the page buffer size (or
    ‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a
    Page Write command attempts to write across a physical page boundary, the
    result is that the data wraps around to the beginning of the current page (overwriting
    data previously stored there), instead of being written to the next page, as might be
    expected. It is therefore necessary for the application software to prevent page write
    operations that would attempt to cross a page boundary.   */
    
    //First check to see if first block starts at the start of a page
    int subpage = address % 32;
    
    //If subpage > 0 first page does not start at a page boundary
    int write_length = 32 - subpage;
    if( write_length > length ) write_length = length;
    char firstpage_array [write_length+2];
    firstpage_array[0] = address / 256;
    firstpage_array[1] = address % 256;
    for(int i=0;i<write_length;i++){
       firstpage_array[i+2] = data [i];
    } 
    _i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false);
    wait(0.005);
    
    if(length > write_length){
        int page_count = (length + subpage) / 32;
        int written = write_length;
        for(int p=0;p<page_count;p++){
            int to_write = length - written;
            if (to_write > 32) {
                to_write = 32;
            } 
            char page_array [to_write + 2];
            page_array[0] = (address + written) / 256;
            page_array[1] = (address + written) % 256;
            for(int i=0;i<to_write;i++){
                page_array[i+2] = data[written + i];
            }
            _i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false);
            wait(0.005);
            written += 32;
        }
    } 
}
    
char PiSwarm::read_eeprom_byte ( int address ){
    char address_array [2];
    address_array[0] = address / 256;
    address_array[1] = address % 256;
    char data [1];
    _i2c.write(EEPROM_ADDRESS, address_array, 2, false);
    _i2c.read(EEPROM_ADDRESS, data, 1, false);
    return data [0];
}

char PiSwarm::read_next_eeprom_byte (){
    char data [1];
    _i2c.read(EEPROM_ADDRESS, data, 1, false);
    return data [0];
}
    
char PiSwarm::read_eeprom_block ( int address, char length ){
    char ret = 0;
    return ret;
}
    
float PiSwarm::read_light_sensor ( void ){
    // Light sensor is a Avago APDS-9005 Ambient Light Sensor
    //float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light
    float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures
    if(r_light > 100) r_light = 100;
    return r_light;
}


void PiSwarm::reset () {
  //  _nrst = 0;
    wait (0.01);
   // _nrst = 1;
    wait (0.1);
}

void PiSwarm::play_tune (char * tune, int length) {
    _ser.putc(DO_PLAY);
    _ser.putc(length);       
    for (int i = 0 ; i < length ; i++) {
        _ser.putc(tune[i]); 
    }
}

void PiSwarm::read_raw_sensors ( int * raw_ls_array ) {
    _ser.putc(SEND_RAW_SENSOR_VALUES);
    for (int i = 0; i < 5 ; i ++) {
        int val = _ser.getc();
        val += _ser.getc() << 8;
        raw_ls_array [i] = val;
    }
}

float PiSwarm::get_left_motor (){
    return left_speed;
}

float PiSwarm::get_right_motor (){
    return right_speed;
}

void PiSwarm::left_motor (float speed) {
    motor(0,speed);
}

void PiSwarm::right_motor (float speed) {
    motor(1,speed);
}

void PiSwarm::forward (float speed) {
    motor(0,speed);
    motor(1,speed);
}

void PiSwarm::backward (float speed) {
    motor(0,-1.0*speed);
    motor(1,-1.0*speed);
}

void PiSwarm::left (float speed) {
    motor(0,speed);
    motor(1,-1.0*speed);
}

void PiSwarm::right (float speed) {
    motor(0,-1.0*speed);
    motor(1,speed);
}

void PiSwarm::stop (void) {
    motor(0,0.0);
    motor(1,0.0);
}

void PiSwarm::motor (int motor, float speed) {
    char opcode = 0x0;
    if (speed > 0.0) {
        if (motor==1)
            opcode = M1_FORWARD;
        else
            opcode = M2_FORWARD;
    } else {
        if (motor==1)
            opcode = M1_BACKWARD;
        else
            opcode = M2_BACKWARD;
    }
    if(motor==1)right_speed = speed;
    else left_speed = speed;
    unsigned char arg = 0x7f * abs(speed);

    _ser.putc(opcode);
    _ser.putc(arg);
}

float PiSwarm::battery() {
    _ser.putc(SEND_BATTERY_MILLIVOLTS);
    char lowbyte = _ser.getc();
    char hibyte  = _ser.getc();
    float v = ((lowbyte + (hibyte << 8))/1000.0);
    return(v);
}

float PiSwarm::line_position() {
    int pos = 0;
    _ser.putc(SEND_LINE_POSITION);
    pos = _ser.getc();
    pos += _ser.getc() << 8;
    
    float fpos = ((float)pos - 2048.0)/2048.0;
    return(fpos);
}

char PiSwarm::sensor_auto_calibrate() {
    _ser.putc(AUTO_CALIBRATE);
    return(_ser.getc());
}


void PiSwarm::calibrate(void) {
    _ser.putc(PI_CALIBRATE);
}

void PiSwarm::reset_calibration() {
    _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
}

void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d) {
    _ser.putc(max_speed);
    _ser.putc(a);
    _ser.putc(b);
    _ser.putc(c);
    _ser.putc(d);
}

void PiSwarm::PID_stop() {
    _ser.putc(STOP_PID);
}

float PiSwarm::pot_voltage(void) {
    int volt = 0;
    _ser.putc(SEND_TRIMPOT);
    volt = _ser.getc();
    volt += _ser.getc() << 8;
    return(volt);
}

void PiSwarm::locate(int x, int y) {
    _ser.putc(DO_LCD_GOTO_XY);
    _ser.putc(x);
    _ser.putc(y);
}

void PiSwarm::cls(void) {
    _ser.putc(DO_CLEAR);
}

int PiSwarm::print (char* text, int length) {
    _ser.putc(DO_PRINT);  
    _ser.putc(length);       
    for (int i = 0 ; i < length ; i++) {
        _ser.putc(text[i]); 
    }
    return(0);
}

int PiSwarm::_putc (int c) {
    _ser.putc(DO_PRINT);  
    _ser.putc(0x1);       
    _ser.putc(c);         
    wait (0.001);
    return(c);
}

int PiSwarm::_getc (void) {
    char r = 0;
    return(r);
}

int PiSwarm::putc (int c) {
    return(_ser.putc(c));
}

int PiSwarm::getc (void) {
    return(_ser.getc());
}

void PiSwarm::start_system_timer(void) {
    _system_timer.reset();
    _system_timer.start();
}

float PiSwarm::get_uptime (void) {
    return _system_timer.read();
}

#ifdef MBED_RPC
const rpc_method *PiSwarm::get_rpc_methods() {
    static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> },
        { "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> },
        { "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> },
        { "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> },
        { "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> },
        { "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> },
        { "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> },
        { "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> },
        { "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> },
        { "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> },
        RPC_METHOD_SUPER(Base)
    };
    return rpc_methods;
}
#endif

void display_system_time(){
    if(PISWARM_DEBUG == 1){
        time_t system_time = time(NULL);
        printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());   
    }
}

void init() {
    //Standard initialisation routine for Pi Swarm 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 .5 second to allow robot to settle)
    piswarm.start_system_timer();
    pc.baud(PC_BAUD);
    if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.4 Initialisation...\n");
    display_system_time();
    piswarm.play_tune("T298MSCG>C",10);
    piswarm.cls();
    piswarm.enable_cled(1);
    piswarm.set_cled_colour(200,0,0);
    piswarm.set_oled_colour(200,0,0);
    piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
    piswarm.printf("  YORK  ");
    piswarm.locate(0,1);
    piswarm.printf("ROBOTLAB");
    wait(0.07);
    piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
    wait(0.07);
    piswarm.set_cled_colour(0,200,0);
    piswarm.set_oled_colour(0,200,0);
    piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
    wait(0.07);
    piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
    wait(0.07);
    piswarm.set_cled_colour(0,0,200);
    piswarm.set_oled_colour(0,0,200);
    piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
    wait(0.07);
    piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
    wait(0.07);
    piswarm.set_oled_colour(20,150,200);
    piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
    
    piswarm.cls();
    piswarm.set_cled_colour(20,20,20);
    piswarm.printf("Pi Swarm");
    piswarm.locate(0,1);
    piswarm.printf("ID : %d ",piswarm.get_id());
    if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: ");
    if(piswarm.calibrate_magnetometer() != 0){
        if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
        wait(0.1);
        piswarm.cls();
        piswarm.locate(0,0);
        piswarm.printf("Mag Cal ");
        piswarm.locate(0,1);
        piswarm.printf("Failed  ");
        wait(0.1);
    }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
    if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: ");
    if(piswarm.calibrate_gyro() == 0){
        if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
        wait(0.1);
        piswarm.cls();
        piswarm.locate(0,0);
        piswarm.printf("Gyro Cal");
        piswarm.locate(0,1);
        piswarm.printf("Failed  ");
        wait(0.1);
    }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
    if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: ");
    if(piswarm.calibrate_accelerometer() == 0){
        if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
        wait(0.1);     
        piswarm.cls();
        piswarm.locate(0,0);
        piswarm.printf("Acc. Cal");
        piswarm.locate(0,1);
        piswarm.printf("Failed  ");
        wait(0.1);
    }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
    piswarm.turn_off_all_oleds();
    wait(0.1);
    piswarm.cls();
    piswarm.set_cled_colour(0,0,0);
    if(START_RADIO_ON_BOOT == 1){
        if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
        piswarm.setup_radio();
    }
}



/********************************************************************************
 * COPYRIGHT NOTICE                                                             *
 *                                                                              *
 * Permission is hereby granted, free of charge, to any person obtaining a copy *
 * of this software and associated documentation files (the "Software"), to deal*
 * in the Software without restriction, including without limitation the rights *
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell    *
 * copies of the Software, and to permit persons to whom the Software is        * 
 * furnished to do so, subject to the following conditions:                     *
 *                                                                              *
 * The above copyright notice and this permission notice shall be included in   *
 * all copies or substantial portions of the Software.                          *
 *                                                                              *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE  *
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN    *
 * THE SOFTWARE.                                                                *
 *                                                                              *
 ********************************************************************************/