Diff: piswarm.cpp

Revision:

0:9ffe8ebd1c40

Child:

1:b067a08ff54e

diff -r 000000000000 -r 9ffe8ebd1c40 piswarm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/piswarm.cpp	Fri Jan 31 22:59:25 2014 +0000
@@ -0,0 +1,939 @@
+/* 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.                                                                *
+ *                                                                              *
+ ********************************************************************************/
+
+