File content as of revision 5:6da8daaeb9f7:

#include "robot.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 allow_commands = 1;
Timeout pc_command_timeout;
char acknowledge_messages = 0;

void SerialComms::setup_serial_interfaces()
{
    if(ENABLE_PC_SERIAL) {
        pc.baud(PC_BAUD);
        pc.attach(this,&SerialComms::_pc_rx_callback, Serial::RxIrq);
    }
    if(ENABLE_BLUETOOTH) {
        bt.baud(BLUETOOTH_BAUD);
        bt.attach(this,&SerialComms::_bt_rx_callback, Serial::RxIrq);
    }
}

void SerialComms::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;
    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 = decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                motors.set_left_motor_speed(dec);
            } else command_status = 2;
            break;
        case 2:
            strcpy(command,"SET RIGHT MOTOR");
            dec = decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                motors.set_right_motor_speed(dec);
                command_status = 1;
            } else command_status = 2;
            break;
        case 3:
            strcpy(command,"SET BOTH MOTORS");
            dec = decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                motors.forwards(dec);
            } else command_status = 2;
            break;
        case 4:
            strcpy(command,"BRAKE LEFT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                motors.brake_left();
            } else command_status = 2;
            break;
        case 5:
            strcpy(command,"BRAKE RIGHT MOTOR");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                motors.brake_right();
            } else command_status = 2;
            break;
        case 6:
            strcpy(command,"BRAKE BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                motors.brake();
            } else command_status = 2;
            break;
        case 7:
            strcpy(command,"COAST BOTH MOTORS");
            sprintf(subcommand,"");
            if(allow_commands) {
                command_status = 1;
                motors.coast();
            } else command_status = 2;
            break;
        case 8:
            strcpy(command,"TURN ON SPOT");
            dec = decode_float(message[1],message[2]);
            sprintf(subcommand,"%1.5f",dec);
            if(allow_commands) {
                command_status = 1;
                motors.turn(dec);
            } else command_status = 2;
            break;
        case 9:
            strcpy(command,"SET EACH MOTOR");
            l_dec = decode_float(message[1]);
            r_dec = decode_float(message[2]);
            sprintf(subcommand,"L=%1.3f R=%1.3f",l_dec,r_dec);
            if(allow_commands) {
                command_status = 1;
                motors.set_left_motor_speed(l_dec);
                motors.set_right_motor_speed(r_dec);
            } else command_status = 2;
            break;
        case 10:
            strcpy(command,"SET PWM PERIOD");
            int period = message[1] * 256 + message[2];
            if(period < 10) period == 10;
            sprintf(subcommand,"%d uS",period);
            if(allow_commands) {
                command_status = 1;
                motors.set_pwm_period(period);
            } else command_status = 2;
            break;
        // LED COMMANDS
        case 12:
            strcpy(command,"SET LED ANIMATION");
            sprintf(subcommand,"M:%d S:%d",message[1], message[2]);
            if(allow_commands) {
                command_status = 1;
                led.start_animation(message[1],message[2]);
            } 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;
              //      led.set_led(message[1],message[2]);
                } else command_status = 2;
            } else {
                sprintf(subcommand,"[INVALID CODE]");
                command_status = 3;
            }
            break;
        case 14:
            strcpy(command,"SET MBED LEDS");
            sprintf(subcommand,"%s",_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 15:
            strcpy(command,"SET CASE LED");
            dec = decode_unsigned_float(message[1],message[2]);
            sprintf(subcommand,"FOR %1.5fS",dec);
            if(allow_commands) {
                command_status = 1;
              //  led.blink_leds(dec);
            } else command_status = 2;
            break;
        case 20:
            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 21:
            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;

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

        case 101:
            strcpy(command,"GET RIGHT MOTOR SPEED");
            sprintf(ret_message,"%1.5f",motors.get_right_motor_speed());
            send_message = 1;
            break;
       
            // GENERAL REQUESTS
        case 102:
            strcpy(command,"GET SOFTWARE VERSION");
            sprintf(ret_message,"%1.2f",SOFTWARE_VERSION_CODE);
            send_message = 1;
            break;

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

        case 104:
            strcpy(command,"GET DEBUG MODE");
            sprintf(ret_message,"%1d%1d",debug_mode,debug_output);
            send_message = 1;
            break;
        case 105:
            strcpy(command,"GET HEX IR VALUES");
            sprintf(ret_message,"%02X%02X%02X%02X%02X%02X%02X%02X",sensors.get_adc_value(0),sensors.get_adc_value(1),sensors.get_adc_value(2),sensors.get_adc_value(3),sensors.get_adc_value(4),sensors.get_adc_value(5),sensors.get_adc_value(6),sensors.get_adc_value(7));
            send_message = 1;
            break;
        case 106:
            strcpy(command,"GET HEX STATUS VALUES");
            robot.update_status_message();
            sprintf(ret_message,"%02X%02X%02X%02X%02X%02X%02X%02X",status_message[0],status_message[1],status_message[2],status_message[3],status_message[4],status_message[5],status_message[6],status_message[7]);
            send_message = 1;
            break;
        case 107:
            strcpy(command,"GET IR VALUES");
            //Special case: characters may have zero values so use printf directly
            pc.printf("%c%c%c%c%c%c%c%c%c",IR_MESSAGE_BYTE,sensors.get_adc_value(0),sensors.get_adc_value(1),sensors.get_adc_value(2),sensors.get_adc_value(3),sensors.get_adc_value(4),sensors.get_adc_value(5),sensors.get_adc_value(6),sensors.get_adc_value(7));
            robot.debug("IR Message sent\n");
            send_message = 2;
            break;
        case 108:
            strcpy(command,"GET STATUS VALUES");
            robot.update_status_message();
            //Special case: characters may have zero values so use printf directly
            pc.printf("%c%c%c%c%c%c%c%c%c",STATUS_MESSAGE_BYTE,status_message[0],status_message[1],status_message[2],status_message[3],status_message[4],status_message[5],status_message[6],status_message[7]);
            robot.debug("Status Message sent\n");
            send_message = 2;
            break;
    }


    if(send_message > 0) {
        if(send_message == 1){
        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;
        }
        robot.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 {
        if(acknowledge_messages){
        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:
                robot.debug("Unrecognised %s command message [%02x%02x%02x]\n",iface,message[0],message[1],message[2]);
                break;
            case 1:
                robot.debug("Actioned %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 2:
                robot.debug("Blocked %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
            case 3:
                robot.debug("Invalid %s command message:%s %s [%02x%02x%02x]\n",iface, command, subcommand,message[0],message[1],message[2]);
                break;
        }
    }
}
    


void SerialComms::handle_user_serial_message(char * message, char length, char interface)
{
    robot.debug("U:[L=%d] ",length);
    for(int i=0; i<length; i++) {
        robot.debug("%2X",message[i]);
    }
    robot.debug("\n");
}


// Private functions

void SerialComms::_pc_rx_callback()
{
    int count = 0;
    char message_array[128];
    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;
                    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(this,&SerialComms::_pc_rx_command_timeout,command_timeout_period);
                    pc_command_message_started = 1;
                    pc_command_message_byte = 0;

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


void SerialComms::_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];
    }
    handle_user_serial_message(message_array, length, 0);
}


void SerialComms::_bt_rx_callback()
{

}


float SerialComms::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 SerialComms::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;
}

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

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


char * SerialComms::_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 * SerialComms::_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;
}