Diff: main.cpp

Revision:

18:3fa5cd53ed45

Parent:

17:f26a5805cde3

Child:

19:b5788427db67

--- a/main.cpp	Fri Jan 15 18:27:53 2016 +0000
+++ b/main.cpp	Thu Feb 23 09:51:40 2017 +0000
@@ -144,11 +144,11 @@
     while(beacon_found == 0) {
         wait(0.5);
         locate_beacon();
-    }
-    toggle_program_ticker.attach(toggle_program,20);
+    } 
+    //toggle_program_ticker.attach(toggle_program,20);
     start_infrared_timers();
     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
-    set_program(0);
+    set_program(4);//for debug ...was 0 ----------
     set_leds(0x00,0x00);
     set_center_led(3,0.5);
     display.clear_display();
@@ -209,15 +209,78 @@
     }
     //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info);
 }
-
+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
+    
+    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);
+    
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string(buffer);
+    
+    //"A" = 0x41
+    if(buffer[0] == 0x41){
+/*        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("Gotcha!");*/
+        
+        set_program(1); // Head to beacon
+    }
+    //"B" = 0x42
+    else if(buffer[0] == 0x42){
+        set_program(2); // Head inverse to beacon
+        }
+    //"C" = 0x43
+    else if(buffer[0] == 0x43){
+        set_program(5); // Find space
+        }
+    //"D" = 0x44
+    else if(buffer[0] == 0x44){
+        set_program(6); // Clustering
+        }
+    else
+    {
+/*        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("NOOO!");*/
+        }
+}
 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
 int main()
 {
     ///init() in psiswarm.cpp sets up the robot
     init();
     user_code_setup();
+    //char prog_index;
+    //char * p_index = &prog_index;
     user_code_running = 1;
     while(1) {
+/*        prog_index = pc.getc();
+        switch(prog_index){
+        case 0x61:
+            display.clear_display();
+            display.set_position(0,0);
+            display.write_string("Facing Beacon");
+            break;
+        default:
+            display.clear_display();
+            display.set_position(0,0);
+            display.write_string(p_index);
+            pc.putc(prog_index);
+            break;
+        }*/
         wait(1);
     }
 }
@@ -239,7 +302,7 @@
             strcpy(ret_name,"RANDOM WALK 1");
             break;
         case 4:
-            strcpy(ret_name,"RANDOM WALK 2");
+            strcpy(ret_name,"WAITING COMMAND");///gus, was random walk2
             break;
         case 5:
             strcpy(ret_name,"FIND SPACE");