C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

Revision:
3:7c0d1f581757
Parent:
2:c6986ee3c7c5
Child:
5:3cdd1a37cdd7
--- a/serial.cpp	Fri Mar 11 13:51:24 2016 +0000
+++ b/serial.cpp	Tue Mar 15 00:58:09 2016 +0000
@@ -44,36 +44,6 @@
 
 
 
-void handle_user_serial_message(char * message, char length, char interface)
-{
-    // This is where user code for handling a (non-system) serial message should go
-    //
-    // message = pointer to message char array
-    // length = length of message
-    // interface = 0 for PC serial connection, 1 for Bluetooth
-
-    if(interface) {
-        if(length == 8) {
-            for(int i = 0; i < length; i++) {
-                // Convert single byte value into a beacon heading in the range +/-180 degrees
-                float beacon_heading = message[i];
-                float degrees_per_value = 256.0f / 360.0f;
-
-                if(beacon_heading != 0)
-                    beacon_heading /= degrees_per_value;
-
-                beacon_heading -= 180;
-
-                flocking_headings[i] = beacon_heading;
-
-                debug("%d, ", flocking_headings[i]);
-                //debug("%f, ", beacon_heading);
-            }
-
-            debug("\n");
-        }
-    }
-}
 
 void IF_start_file_transfer_mode()
 {
@@ -364,7 +334,6 @@
             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;
@@ -570,6 +539,28 @@
                         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:
@@ -909,6 +900,94 @@
                 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;
     }
 
 
@@ -1113,17 +1192,19 @@
     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++;
-    }
+//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);
+//}
 
-    bt_message_timeout.attach(&IF_bt_message_timeout, bt_message_timeout_period);
-}
 
 void IF_set_filename(char * filename_in){
     strcpy(filename,filename_in);
@@ -1195,49 +1276,49 @@
     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);
-//}
\ No newline at end of file
+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);
+}
\ No newline at end of file