C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

serial.cpp

Committer:
jah128
Date:
2016-10-15
Revision:
5:3cdd1a37cdd7
Parent:
3:7c0d1f581757
Child:
6:b340a527add9

File content as of revision 5:3cdd1a37cdd7:

/* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
 *
 * File: serial.cpp
 *
 * (C) Dept. Electronics & Computer Science, University of York
 * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
 *
 * PsiSwarm Library Version: 0.7
 *
 * October 2016
 *
 *
 */

#include "psiswarm.h"

static float command_timeout_period = 0.1f;     //If a complete command message is not received in 0.1s then consider it a user message
char pc_command_message_started = 0;
char pc_command_message_byte = 0;
char pc_command_message[3];
char bt_command_message_started = 0;
char bt_command_message_byte = 0;
char bt_command_message[3];

char allow_commands = 1;
char allow_requests = 1;
char file_transfer_state = 0;

int block_size = 88;   // The data block size for file transfer
char data_block[89];   // Stores the data block to write for Bluetooth file transfer
int data_written;       // Stores if partial data has been written to a file
int file_length;        // Stores the file length for a Bluetooth file transfer
int final_block;        // Stores the index of the final data block for a Bluetooth file transfer
int block_index;        // Stores the current block index for a Bluetooth file transfer
char filename [21];     // Stores the filename for a Bluetooth file transfer

Timeout ft_timeout;
Timeout pc_command_timeout;
Timeout bt_command_timeout;

// A predefined message structure for command messages is as follows:
// [Byte 0][Byte 1][Byte 2][Byte 3][Byte 4]
// Byte 0 and Byte 4 must be equal to COMMAND_MESSAGE_BYTE [in psiswarm.h] or message is treated as a user message




void IF_start_file_transfer_mode()
{
    display.clear_display();
    display.set_position(0,0);
    display.write_string("FILE TRANSFER");
    display.set_position(1,0);
    display.write_string("MODE...");
    data_written = 0;
    file_transfer_mode = 1;
    file_transfer_state = 0;
    file_length = 0;
    user_code_restore_mode = user_code_running;
    user_code_running = 0;
    ft_timeout.attach(IF_file_transfer_timeout,2.0);
}


void IF_invalid_transfer(void)
{
    debug("File transfer failed\n");
    if(data_written == 1){
        debug("Deleting corrupted file\n");
        remove(filename);
    }
    display.clear_display();
    display.set_position(0,0);
    display.write_string("TRANSFER FAILED");
    wait(0.5);
    IF_end_file_transfer_mode();
}

void IF_file_transfer_timeout(void)
{
    debug("File transfer failed: timeout\n");
    display.clear_display();
    display.set_position(0,0);
    display.write_string("TRANSFER TIMEOUT");
    wait(0.5);
    IF_end_file_transfer_mode();
}

void IF_end_file_transfer_mode(void)
{
    display.clear_display();
    file_transfer_mode = 0;
    user_code_running = user_code_restore_mode;
}


void IF_handle_file_transfer_serial_message(char * message, char length, char interface)
{
    // Code for handling a serial (Bluetooth) message when in file-transfer mode
    //
    // message = pointer to message char array
    // length = length of message
    // interface = 0 for PC serial connection, 1 for Bluetooth [NB only Bluetooth used for file transfer in this version]

    if(file_transfer_state < 2)debug("FTM Message:%.*s [%d]\n",length,message,length);
    else debug("FTM data block received (%d bytes)\n",length);
    int expected_size;
    // The first byte in EVERY message received should be 33; if it isn't, abort the transfer
    if(message[0] != 33) {
        IF_invalid_transfer();
    } else {
        switch(file_transfer_state) {
            case 0: //First message received is the target filename
                //The filenames cannot be more that 8.3 characters long (FAT12 format)
                if(length == 1 || length > 13) IF_invalid_transfer();
                else {
                    strcpy(filename, "/local/");
                    strncat(filename, message + 1, length - 1);
                    debug("Target filename:%s\n",filename);
                    //Send acknowledge ("FN")
                    ft_timeout.detach();
                    ft_timeout.attach(IF_file_transfer_timeout,2.0);
                    bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,2,"FN");
                    file_transfer_state = 1;
                }
                break;
            case 1: //Second message is the length of the file in bytes
                //Length is encoded as a 3-byte value
                if(length != 4) IF_invalid_transfer();
                else {
                    file_length = (message[1]) * 256;
                    file_length += (message[2]);
                    file_length *= 256;
                    file_length += message[3];
                    file_transfer_state = 2;
                    display.clear_display();
                    char display_message[17];
                    sprintf(display_message,"F:%s",filename);
                    display.set_position(0,0);
                    display.write_string(display_message);
                    display.set_position(1,0);
                    sprintf(display_message,"S:%d b",file_length);
                    display.write_string(display_message);
                    block_index = 0;
                    //Work out how many blocks the file will be sent in (size = block_size, tested at 100 bytes)
                    //Allocate memory for the file up to a limit of 16 blocks; larger files will be split across
                    //multiple blocks....
                    final_block = file_length / block_size;
                    if(file_length % block_size != 0) final_block ++;
                    //int target_size = file_length;
                    //if(file_length > (block_size * 16)) target_size = block_size * 16;
                    //file_data = (char *) malloc(target_size);
                    debug("File size %d bytes (%d blocks of %d bytes)\n",file_length,final_block,block_size);
                    ft_timeout.detach();
                    ft_timeout.attach(IF_file_transfer_timeout,1.0);
                    //Send acknowledge (size of file)
                    bt.printf("%c%c%c%c%c",RESPONSE_MESSAGE_BYTE,3,message[1],message[2],message[3]);
                }
                break;
            case 2:
                block_index ++;
                display.clear_display();
                display.set_position(0,0);
                display.write_string("FILE TRANSFER");
                display.set_position(1,0);
                char details_string[17];
                sprintf(details_string,"BLOCK %d OF %d",block_index,final_block);
                display.write_string(details_string);
                expected_size = block_size;
                if(block_index == final_block) expected_size = file_length % block_size;
                if(expected_size == 0) expected_size = block_size;
                if(length!=expected_size + 1){
                    // Unexpected length   
                    debug("File data unexpected length in packet %d (%d bytes received, %d bytes expected)\n",block_index,length-1,expected_size);
                }else{
                    char transfer_mode[2]={'a'};
                    if(block_index == 1){
                        transfer_mode[0]='w';
                    }
                    FILE *fp = fopen(filename,transfer_mode);
                    //strncpy(data_block,message+1,length);
                    //data_block[length]=0;
                    //fprintf(fp,data_block);
                    int bytes_written;
                    bytes_written = fwrite(message+1,expected_size,1,fp);
                    fclose(fp);
                    if(data_written == false && bytes_written > 0) data_written = true;
                    debug("Bytes written: %d\n",expected_size * bytes_written);
                    if(block_index < final_block){
                    debug("Message packet %d received and written\n",block_index);
                    //Send acknowledge ("D")
                    ft_timeout.detach();
                    ft_timeout.attach(IF_file_transfer_timeout,1.0);   
                    bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
                    }
                    else{
                        //Last data block written
                        //[Put file checking code here]
                        //Send acknowledge ("P");
                        bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"F");
                        ft_timeout.detach();
                        debug("File transfer completed successfully\n");
                        wait(0.25);
                        //Calculate CRC16 value for file
                        IF_calculateCRC16(file_length);
                        
                        display.clear_display();
                        display.write_string("FILE TRANSFER");
                        display.set_position(1,0);
                        display.write_string("COMPLETE");   
                        wait(1);
                        debug("File transfer mode ended\n");
                        IF_end_file_transfer_mode();
                    }
                }
                break;
        }
    }
}


void IF_handle_user_serial_message(char * message, char length, char interface)
{
    char buffer[255];
    sprintf(buffer,message,length);
    for(int i=0; i<length; i++) {
        buffer[i]=message[i];
    }
    buffer[length]=0;
//    if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
//    else debug("Received USB message:%s [%d chars]\n",buffer,length);
    handle_user_serial_message(message,length,interface);
}

void IF_handle_command_serial_message(char message[3], char interface)
{
    char iface [4];
    if(interface) strcpy(iface,"BT");
    else strcpy(iface,"USB");
    char command [26];
    char subcommand[30];
    float dec;
    float l_dec;
    float r_dec;
    int irp_delay;
    char colour_string[7];
    char ret_message[50];
    char send_message = 0;
    char command_status = 0;
    // command_status values:
    // 0 - unrecognised command
    // 1 - command actioned
    // 2 - command blocked
    // 3 - invalid parameters

    subcommand[0]=0;
    command[0]=0;
    switch(message[0]) {

            // MOTOR COMMANDS

        case 1:
            strcpy(command,"SET LEFT MOTOR");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                set_left_motor_speed(dec);
            } else command_status = 2;
            break;
        case 2:
            strcpy(command,"SET RIGHT MOTOR");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                set_right_motor_speed(dec);
                command_status = 1;
            } else command_status = 2;
            break;
        case 3:
            strcpy(command,"SET BOTH MOTORS");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                forward(dec);
            } else command_status = 2;
            break;
        case 4:
            strcpy(command,"BRAKE LEFT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake_left_motor();
            } else command_status = 2;
            break;
        case 5:
            strcpy(command,"BRAKE RIGHT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake_right_motor();
            } else command_status = 2;
            break;
        case 6:
            strcpy(command,"BRAKE BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                brake();
            } else command_status = 2;
            break;
        case 7:
            strcpy(command,"STOP BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                stop();
            } else command_status = 2;
            break;
        case 8:
            strcpy(command,"TURN ON SPOT");
            dec = IF_decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                turn(dec);
            } else command_status = 2;
            break;
        case 9:
            strcpy(command,"SET EACH MOTOR");
            l_dec = IF_decode_float(message[1]);
            r_dec = IF_decode_float(message[2]);
            sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
            if(allow_commands) {
                command_status = 1;
                set_left_motor_speed(l_dec);
                set_right_motor_speed(r_dec);
            } else command_status = 2;
            break;
            // LED COMMANDS

        case 10:
            strcpy(command,"SET LED STATES");
            sprintf(subcommand,"G:%s R:%s",IF_char_to_binary_char(message[1]), IF_char_to_binary_char(message[2]));
            if(allow_commands) {
                command_status = 1;
                set_leds(message[1],message[2]);
            } else command_status = 2;
            break;
        case 11:
            strcpy(command,"SET RED LED STATES");
            sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                set_red_leds(message[1]);
            } else command_status = 2;
            break;
        case 12:
            strcpy(command,"SET GREEN LED STATES");
            sprintf(subcommand,"%s",IF_char_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                set_green_leds(message[1]);
            } else command_status = 2;
            break;
        case 13:
            strcpy(command,"SET LED");
            switch(message[2]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            if(message[1] < 8 && message[2] < 4) {
                sprintf(subcommand,"%d %s",message[1],colour_string);
                if(allow_commands) {
                    command_status = 1;
                    set_led(message[1],message[2]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID CODE]");
                command_status = 3;
            }
            break;
        case 14:
            strcpy(command,"SET CENTER LED STATE");
            switch(message[1]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            if(message[1] < 4) {
                sprintf(subcommand,"%s",colour_string);
                if(allow_commands) {
                    command_status = 1;
                    set_center_led(message[1]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID CODE]");
                command_status = 3;
            }
            break;
        case 15:
            strcpy(command,"SET C.LED BRIGHTNESS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                set_center_led_brightness(dec);
            } else command_status = 2;
            break;
        case 16:
            strcpy(command,"SET MBED LEDS");
            sprintf(subcommand,"%s",IF_nibble_to_binary_char(message[1]));
            if(allow_commands) {
                command_status = 1;
                mbed_led1 = (message[1] & 128) >> 7;
                mbed_led2 = (message[1] & 64) >> 6;
                mbed_led3 = (message[1] & 32) >> 5;
                mbed_led4 = (message[1] & 16) >> 4;
            } else command_status = 2;
            break;
        case 17:
            strcpy(command,"BLINK OUTER LEDS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"FOR %1.5fS",dec);
            if(allow_commands) {
                command_status = 1;
                blink_leds(dec);
            } else command_status = 2;
            break;
        case 18:
            strcpy(command,"SET BASE LED STATE");
            switch(message[1]) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            //Function not yet implemented
            break;
        case 19:
            strcpy(command,"SET CENTER LED ");
            switch(message[1]) {
                case 1:
                    strcpy(colour_string,"RED");
                    break;
                case 2:
                    strcpy(colour_string,"GREEN");
                    break;
                case 3:
                    strcpy(colour_string,"BOTH");
                    break;
                case 0:
                    strcpy(colour_string,"OFF");
                    break;
            }
            dec = IF_decode_unsigned_float(message[2]);
            sprintf(subcommand,"%s @ %1.5f brightness",colour_string,dec);
            if(allow_commands) {
                command_status = 1;
                set_center_led(message[1],dec);
            } else command_status = 2;
            break;

            // DISPLAY COMMANDS

        case 20:
            strcpy(command,"SET DISPLAY ");
            switch(message[1]) {
                case 0:
                    strcpy(subcommand,"CLEAR");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                    } else command_status = 2;
                    break;
                case 1:
                    strcpy(subcommand,"MESSAGE 1");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PC CONNECTION");
                        display.set_position(1,0);
                        display.write_string("STARTED");
                    } else command_status = 2;
                    break;
                case 2:
                    strcpy(subcommand,"MESSAGE 2");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PC CONNECTION");
                        display.set_position(1,0);
                        display.write_string("TERMINATED");
                    } else command_status = 2;
                    break;
                case 3:
                    strcpy(subcommand,"MESSAGE 3");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("ANDROID DEVICE");
                        display.set_position(1,0);
                        display.write_string("CONNECTED");
                    } else command_status = 2;
                    break;
                case 4:
                    strcpy(subcommand,"MESSAGE 4");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("ANDROID DEVICE");
                        display.set_position(1,0);
                        display.write_string("DISCONNECTED");
                    } else command_status = 2;
                    break;
                case 5:
                    strcpy(subcommand,"MESSAGE 5");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PSI CONSOLE");
                        display.set_position(1,0);
                        display.write_string("CONNECTED");
                    } else command_status = 2;
                    break;
                 case 6:
                    strcpy(subcommand,"MESSAGE 6");
                    if(allow_commands) {
                        command_status = 1;
                        display.clear_display();
                        display.home();
                        display.write_string("PSI CONSOLE");
                        display.set_position(1,0);
                        display.write_string("DISCONNECTED");
                    } else command_status = 2;
                    break;    
            }
            break;
        case 21:
            strcpy(command,"SET CURSOR ");
            if(message[1] < 2 && message[2] < 16) {
                sprintf(subcommand,"[%d,%d]",message[1],message[2]);
                if(allow_commands) {
                    display.set_position(message[1],message[2]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID]");
                command_status = 3;
            }
            break;
        case 22:
            strcpy(command,"PRINT CHARACTERS ");
            char print_message[2];
            print_message[0]=message[1];
            print_message[1]=message[2];
            sprintf(subcommand,"[%c,%c]",message[1],message[2]);
            if(allow_commands) {
                display.write_string(print_message,2);
            } else command_status = 2;
            break;
        case 23:
            strcpy(command,"SET DISPLAY B.NESS");
            dec = IF_decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                display.set_backlight_brightness(dec);
            } else command_status = 2;
            break;

        case 30:
            strcpy(command,"SET DEBUG MODE");
            switch(message[1]) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(message[2] & 1) strcat (subcommand,"-PC");
            if(message[2] & 2) strcat (subcommand,"-BT");
            if(message[2] & 4) strcat (subcommand,"-DISP");
            if(allow_commands) {
                command_status = 1;
                debug_mode = message[1];
                debug_output = message[2];
            } else command_status = 2;
            break;
        case 31:
            strcpy(command,"SET DEMO MODE");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(allow_commands) {
                command_status = 1;
                demo_on = message[1] % 2;
                if(demo_on == 1) {
                    user_code_restore_mode = user_code_running;
                    user_code_running = 0;
                } else {
                    user_code_running = user_code_restore_mode;
                }
            } else command_status = 2;
            break;
        case 32:
            strcpy(command,"SET USER CODE");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            if(allow_commands) {
                command_status = 1;
                user_code_running = message[1] % 2;
            } else command_status = 2;
            break;
        case 33:
            strcpy(command,"PAUSE USER CODE");
            dec = IF_decode_unsigned_float(message[1],message[2]) * 10;
            sprintf(subcommand,"FOR %2.3fS",dec);
            if(allow_commands) {
                command_status = 1;
                pause_user_code(dec);
            } else command_status = 2;
            break;

        case 34:
            strcpy(command,"RESET ENCODERS");
            if(allow_commands) {
                command_status = 1;
                reset_encoders();
            } else command_status = 2;
            break;

        case 35:
            strcpy(command,"SET ALLOW COMMANDS");
            switch(message[1] % 2) {
                case 1:
                    strcpy(subcommand,"ON");
                    break;
                case 0:
                    strcpy(subcommand,"OFF");
                    break;
            }
            allow_commands = message[1] % 2;
            command_status = 1;
            break;

        case 36:
            irp_delay = (message[1] << 8) + message[2];
            sprintf(command,"SET IR PULSE DELAY %d MS",irp_delay);
            if(allow_commands) {
                command_status = 1;
                ir_pulse_delay = irp_delay;
            } else command_status = 2;
            break;
        case 37:
            irp_delay = (message[1] << 8) + message[2];
            sprintf(command,"SET BASE IR PULSE DELAY %d MS",irp_delay);
            if(allow_commands) {
                command_status = 1;
                base_ir_pulse_delay = irp_delay;
            } else command_status = 2;
            break;

            // MOTOR REQUESTS
        case 40:
            strcpy(command,"GET LEFT MOTOR SPEED");
            sprintf(ret_message,"%1.5f",motor_left_speed);
            send_message = 1;
            break;

        case 41:
            strcpy(command,"GET RIGHT MOTOR SPEED");
            sprintf(ret_message,"%1.5f",motor_right_speed);
            send_message = 1;
            break;
        case 42:
            strcpy(command,"GET BRAKE STATES");
            sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
            send_message = 1;
            break;
        case 43:
            strcpy(command,"GET MOTOR STATES");
            //sprintf(ret_message,"%d,%d",motor_left_brake,motor_right_brake);
            send_message = 1;
            break;
        case 44:
            strcpy(command,"GET ENCODERS");
            sprintf(ret_message,"%d,%d",left_encoder,right_encoder);
            send_message = 1;
            break;

            // LED REQUESTS
        case 50:
            strcpy(command,"GET LED STATES");
            sprintf(ret_message,"%04x",get_led_states());
            send_message = 1;
            break;

            // GENERAL REQUESTS
        case 60:
            strcpy(command,"GET SOFTWARE VERSION");
            sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
            send_message = 1;
            break;

        case 61:
            strcpy(command,"GET UPTIME");
            sprintf(ret_message,"%6.2f",get_uptime());
            send_message = 1;
            break;

        case 62:
            strcpy(command,"GET ID");
            sprintf(ret_message,"%d",robot_id);
            send_message = 1;
            break;

        case 63:
            strcpy(command,"GET SWITCH BYTE");
            sprintf(ret_message,"%02x",switch_byte);
            send_message = 1;
            break;
        case 64:
            strcpy(command,"GET USER CODE");
            sprintf(ret_message,"%d",user_code_running);
            send_message = 1;
            break;
        case 65:
            strcpy(command,"GET RESPONSE STRING");
            sprintf(ret_message,"PSI");
            send_message = 1;
            break;
        case 66:
            strcpy(command,"GET PROGRAM NAME");
            sprintf(ret_message,"%s",program_name);
            send_message = 1;
            break;
        case 67:
            strcpy(command,"GET AUTHOR NAME");
            sprintf(ret_message,"%s",author_name);
            send_message = 1;
            break;
        case 68:
            strcpy(command,"GET DEBUG MODE");
            sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
            send_message = 1;
            break;
        case 69:
            strcpy(command,"GET SYSTEM WARNINGS");
            sprintf(ret_message,"%d",system_warnings);
            send_message = 1;
            break;


            // Sensors
        case 80:
            strcpy(command,"STORE BG. IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_background_raw_ir_values();
            } else command_status = 2;
            break;
        case 81:
            strcpy(command,"STORE IL. IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_illuminated_raw_ir_values();
            } else command_status = 2;
            break;
        case 82:
            strcpy(command,"STORE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_ir_values();
            } else command_status = 2;
            break;
        case 83:
            strcpy(command,"STORE BG BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_background_base_ir_values();
            } else command_status = 2;
            break;
        case 84:
            strcpy(command,"STORE IL. BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_illuminated_base_ir_values();
            } else command_status = 2;
            break;
        case 85:
            strcpy(command,"STORE BASE IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_base_ir_values();
            } else command_status = 2;
            break;
        case 86:
            strcpy(command,"STORE ALL IR VALUES");
            if(allow_commands) {
                command_status = 1;
                store_ir_values();
                store_base_ir_values();
            } else command_status = 2;
            break;
        case 90:
            sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
            send_message = 1;
            break;
        case 91:
            sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
            send_message = 1;
            break;
        case 92:
            strcpy(command,"GET BG IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_background_raw_ir_value(0),get_background_raw_ir_value(1),get_background_raw_ir_value(2),get_background_raw_ir_value(3),get_background_raw_ir_value(4),get_background_raw_ir_value(5),get_background_raw_ir_value(6),get_background_raw_ir_value(7));
            send_message = 1;
            break;
        case 93:
            strcpy(command,"GET ILLUMINATED IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_illuminated_raw_ir_value(0),get_illuminated_raw_ir_value(1),get_illuminated_raw_ir_value(2),get_illuminated_raw_ir_value(3),get_illuminated_raw_ir_value(4),get_illuminated_raw_ir_value(5),get_illuminated_raw_ir_value(6),get_illuminated_raw_ir_value(7));
            send_message = 1;
            break;
        case 94:
            sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
            send_message = 1;
            break;
        case 95:
            sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
            sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
            send_message = 1;
            break;
        case 96:
            strcpy(command,"GET BG BASE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_background_base_ir_value(0),get_background_base_ir_value(1),get_background_base_ir_value(2),get_background_base_ir_value(3),get_background_base_ir_value(4));
            send_message = 1;
            break;
        case 97:
            strcpy(command,"GET IL BASE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_illuminated_base_ir_value(0),get_illuminated_base_ir_value(1),get_illuminated_base_ir_value(2),get_illuminated_base_ir_value(3),get_illuminated_base_ir_value(4));
            send_message = 1;
            break;
        case 98:
            strcpy(command,"CALCULATE BASE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X",calculate_base_ir_value(0),calculate_base_ir_value(1),calculate_base_ir_value(2),calculate_base_ir_value(3),calculate_base_ir_value(4));
            send_message = 1;
            break;
        case 99:
            strcpy(command,"CALCULATE SIDE IR VALUES");
            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",calculate_side_ir_value(0),calculate_side_ir_value(1),calculate_side_ir_value(2),calculate_side_ir_value(3),calculate_side_ir_value(4),calculate_side_ir_value(5),calculate_side_ir_value(6),calculate_side_ir_value(7));
            send_message = 1;
            break;
        case 100:
            strcpy(command,"START FILE TRANSFER MODE");
            if(allow_commands) {
                command_status = 1;
                IF_start_file_transfer_mode();
                sprintf(ret_message,"OK");
                send_message = 1;
            } else command_status = 2;
            break;
        case 110:
            strcpy(command,"GET FIRMWARE VERSION");
            sprintf(ret_message,"%1.2f",firmware_version);
            send_message = 1;
            break;
        case 111:
            strcpy(command,"GET SERIAL NUMBER");
            sprintf(ret_message,"%2.3f",serial_number);
            send_message = 1;
            break;
        case 112:
            strcpy(command,"GET HAS SIDE IR");
            sprintf(ret_message,"%d",has_side_ir);
            send_message = 1;
            break;
        case 113:
            strcpy(command,"GET HAS BASE IR");
            sprintf(ret_message,"%d",has_base_ir);
            send_message = 1;
            break;
        case 114:
            strcpy(command,"GET HAS ENCODERS");
            sprintf(ret_message,"%d",has_wheel_encoders);
            send_message = 1;
            break;
        case 115:
            strcpy(command,"GET HAS AUDIO");
            sprintf(ret_message,"%d",has_audio_pic);
            send_message = 1;
            break;
        case 116:
            strcpy(command,"GET HAS RECHARGING");
            sprintf(ret_message,"%d",has_recharging_circuit);
            send_message = 1;
            break;
        case 117:
            strcpy(command,"GET HAS COMPASS");
            sprintf(ret_message,"%d",has_compass);
            send_message = 1;
            break;
        case 118:
            strcpy(command,"GET HAS ULTRASONIC");
            sprintf(ret_message,"%d",has_ultrasonic_sensor);
            send_message = 1;
            break;
        case 119:
            strcpy(command,"GET HAS TEMPERATURE");
            sprintf(ret_message,"%d",has_temperature_sensor);
            send_message = 1;
            break;
        case 120:
            strcpy(command,"GET HAS BASE COLOUR");
            sprintf(ret_message,"%d",has_base_colour_sensor);
            send_message = 1;
            break;
        case 121:
            strcpy(command,"GET HAS TOP COLOUR");
            sprintf(ret_message,"%d",has_top_colour_sensor);
            send_message = 1;
            break;
        case 122:
            strcpy(command,"GET HAS RADIO");
            sprintf(ret_message,"%d",has_433_radio);
            send_message = 1;
            break;
        case 123:
            strcpy(command,"GET FIRMWARE H-DESC");
            char byte0 = 0;
            char byte1 = 1;
            if(has_side_ir)byte0+=128;
            if(has_base_ir)byte0+=64;
            if(has_wheel_encoders)byte0+=32;
            if(has_audio_pic)byte0+=16;
            if(has_recharging_circuit)byte0+=8;
            if(has_compass)byte0+=4;
            if(has_ultrasonic_sensor)byte0+=2;
            if(has_temperature_sensor)byte0+=1;
            if(has_base_colour_sensor)byte1+=128;
            if(has_top_colour_sensor)byte1+=64;
            if(has_433_radio)byte1+=32;
            sprintf(ret_message,"%c%c",byte0,byte1);
            send_message = 1;
            break;
        case 124:
            strcpy(command,"GET PCB VERSION");
            sprintf(ret_message,"%1.2f",pcb_version);
            send_message = 1;
            break;
    }


    if(send_message) {
        char message_length = strlen(ret_message);
        switch(interface) {
            case 0:
                pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
                break;
            case 1:
                bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
                break;
        }
        debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
    } else {
        switch(interface) {
            case 0:
                pc.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
                break;
            case 1:
                bt.printf("%c%c",ACKNOWLEDGE_MESSAGE_BYTE,command_status);
                break;
        }
        switch(command_status) {
            case 0:
                debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
                break;
            case 1:
                debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 2:
                debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 3:
                debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
        }
    }
}

char * IF_nibble_to_binary_char(char in)
{
    char * ret = (char*)malloc(sizeof(char)*5);
    for(int i=0; i<4; i++) {
        if(in & (128 >> i)) ret[i]='1';
        else ret[i]='0';
    }
    ret[4]=0;
    return ret;
}

char * IF_char_to_binary_char(char in)
{
    char * ret = (char*)malloc(sizeof(char)*9);
    for(int i=0; i<8; i++) {
        if(in & (128 >> i)) ret[i]='1';
        else ret[i]='0';
    }
    ret[8]=0;
    return ret;
}

float IF_decode_unsigned_float(char byte0, char byte1)
{
    unsigned short sval = (byte0) << 8;
    sval += byte1;
    float scaled = sval / 65535.0f;
    return scaled;
}

float IF_decode_float(char byte0, char byte1)
{
    // MSB is byte 0 is sign, rest is linear spread between 0 and 1
    char sign = byte0 / 128;
    short sval = (byte0 % 128) << 8;
    sval += byte1;
    float scaled = sval / 32767.0f;
    if(sign == 0) scaled = 0-scaled;
    return scaled;
}

float IF_decode_unsigned_float(char byte0)
{
    unsigned short sval = (byte0);
    float scaled = sval / 255.0f;
    return scaled;
}

float IF_decode_float(char byte0)
{
    // MSB is byte 0 is sign, rest is linear spread between 0 and 1
    char sign = byte0 / 128;
    short sval = (byte0 % 128);
    float scaled = sval / 127.0f;
    if(sign == 0) scaled = 0-scaled;
    return scaled;
}

void IF_setup_serial_interfaces()
{
    if(ENABLE_PC_SERIAL) {
        pc.baud(PC_BAUD);
        pc.attach(&IF_pc_rx_callback, Serial::RxIrq);
    }
    if(ENABLE_BLUETOOTH) {
        bt.baud(BLUETOOTH_BAUD);
        bt.attach(&IF_bt_rx_callback, Serial::RxIrq);
    }
}

void IF_pc_rx_command_timeout()
{
    char message_array[6];
    char length = 1 + pc_command_message_byte;
    pc_command_message_started = 0;
    message_array[0] = COMMAND_MESSAGE_BYTE;
    for(int k=0; k<pc_command_message_byte; k++) {
        message_array[k+1] = pc_command_message[k];
    }
    IF_handle_user_serial_message(message_array, length, 0);
}

void IF_bt_rx_command_timeout()
{
    char message_array[6];
    char length = 1 + bt_command_message_byte;
    bt_command_message_started = 0;
    message_array[0] = COMMAND_MESSAGE_BYTE;
    for(int k=0; k<bt_command_message_byte; k++) {
        message_array[k+1] = bt_command_message[k];
    }
    IF_handle_user_serial_message(message_array, length, 1);
}

void IF_pc_rx_callback()
{
    int count = 0;
    char message_array[255];

    while(pc.readable()) {
        char tc = pc.getc();
        message_array[count] = tc;
        count ++;
        if(pc_command_message_started == 1) {
            if(pc_command_message_byte == 3) {
                pc_command_timeout.detach();
                if(tc == COMMAND_MESSAGE_BYTE) {
                    // A complete command message succesfully received, call handler
                    pc_command_message_started = 0;
                    count = 0;
                    IF_handle_command_serial_message(pc_command_message , 0);
                } else {
                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
                    pc_command_message_started = 0;
                    message_array[0] = COMMAND_MESSAGE_BYTE;
                    message_array[1] = pc_command_message[0];
                    message_array[2] = pc_command_message[1];
                    message_array[3] = pc_command_message[2];
                    message_array[4] = tc;
                    count = 5;
                }
            } else {
                pc_command_message[pc_command_message_byte] = tc;
                pc_command_message_byte ++;
            }
        } else {
            if(count == 1) {
                if(tc == COMMAND_MESSAGE_BYTE) {
                    pc_command_timeout.attach(&IF_pc_rx_command_timeout,command_timeout_period);
                    pc_command_message_started = 1;
                    pc_command_message_byte = 0;

                }
            }
        }
    }
    if(!pc_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 0);
}

Timeout bt_message_timeout;
static float bt_message_timeout_period = 0.001; // 1 millisecond
char bt_buffer[255];
int bt_buffer_index = 0;

void IF_bt_message_timeout()
{
    char buffer[255];

    sprintf(buffer, bt_buffer, bt_buffer_index);
    buffer[bt_buffer_index] = 0;
    if(file_transfer_mode == 1) {
        IF_handle_file_transfer_serial_message(bt_buffer, bt_buffer_index, 1);
    } else {
//    debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
        if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
            bt_command_message[0] = buffer[1];
            bt_command_message[1] = buffer[2];
            bt_command_message[2] = buffer[3];
            IF_handle_command_serial_message(bt_command_message , 1);
        } else IF_handle_user_serial_message(bt_buffer, bt_buffer_index, 1);
    }
    bt_buffer_index = 0;
}


//void IF_bt_rx_callback()
//{
//    while(bt.readable()) {
//        char byte = bt.getc();
//
//        bt_buffer[bt_buffer_index] = byte;
//        bt_buffer_index++;
//    }
//
//    bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
//}


void IF_set_filename(char * filename_in){
    strcpy(filename,filename_in);
}
 
unsigned short IF_calculateCRC16(int file_length){
    unsigned short crc16table[256] = { 
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};
        
    //Opens, reads and calculates the CRC16 value for file pointed to by filename
    unsigned short crc_value = 0;
    FILE *fp = fopen(filename,"r");
    char * buffer;
    int limit = 512;
    if(file_length < 512) limit = file_length;
    buffer = (char*) malloc (sizeof(char)*limit);
    int blocks = 1;
    if(file_length > limit) blocks += file_length / limit;
    for(int i=0;i<blocks;i++){
        //Determine size of this block
        int blocksize = limit;
        if(i == blocks-1){
            if((file_length % limit) != 0) blocksize = file_length % limit;   
        }               
        debug("Calculating %d bytes of CRC data...\n",blocksize);
        int result;
        result = fread(buffer,1,blocksize,fp);
        debug("Data read: %d\n",result);
        for(int j=0;j<blocksize;j++){
            int subindex = ((crc_value>>8)^*(char *)(buffer[j]))&0x00FF;
            //debug("J:%d Subindex:%d\n",j,subindex);
            unsigned short table_value = crc16table[subindex];
            crc_value=(crc_value<<8)^table_value;   
        }
    }
    fclose(fp);
    debug("CRC Calculated: %x\n",crc_value);
    return crc_value;               
}

void IF_bt_rx_callback()
{
    int count = 0;
    char message_array[255];

    wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it

    while(bt.readable()) {
        char tc = bt.getc();
        message_array[count] = tc;
        count ++;
        if(bt_command_message_started == 1) {
            if(bt_command_message_byte == 3) {
                bt_command_timeout.detach();
                if(tc == COMMAND_MESSAGE_BYTE) {
                    // A complete command message succesfully received, call handler
                    bt_command_message_started = 0;
                    count = 0;
                    IF_handle_command_serial_message(bt_command_message , 1);
                } else {
                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
                    bt_command_message_started = 0;
                    message_array[0] = COMMAND_MESSAGE_BYTE;
                    message_array[1] = bt_command_message[0];
                    message_array[2] = bt_command_message[1];
                    message_array[3] = bt_command_message[2];
                    message_array[4] = tc;
                    count = 5;
                }
            } else {
                bt_command_timeout.attach(&IF_bt_rx_command_timeout,command_timeout_period);
                bt_command_message[bt_command_message_byte] = tc;
                bt_command_message_byte ++;
            }
        } else {
            if(count == 1) {
                if(tc == COMMAND_MESSAGE_BYTE) {
                    bt_command_message_started = 1;
                    bt_command_message_byte = 0;

                }
            }
        }
    }
    if(!bt_command_message_started && count>0) IF_handle_user_serial_message(message_array, count, 1);
}