Diff: main.cpp

Revision:

13:f5994956b1ba

Parent:

12:daa53285b6e4

Child:

14:f623db1e6184

--- a/main.cpp	Tue Oct 27 13:03:10 2015 +0000
+++ b/main.cpp	Tue Oct 27 22:49:41 2015 +0000
@@ -38,6 +38,7 @@
 char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
 char display_debug_inf = 0;              // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
 char main_program_state;                 // Index of the currently running program
+char previous_program;                   // Used to hold previous running program when it is paused for switch press etc
 char program_changed = 0;                // Flag to update display when program is changed
 char program_run_init = 0;               // Flag to tell program to run its initialisation on first loop, if neccessary
 char success_count = 0;                  // Flag to indicate the success of a program
@@ -47,6 +48,10 @@
 char prog_info [17];                     // Stores information about the current state of the program [line 1 on the display]
 char disable_ir_emitters = 0;            // Used to disable IR emission during charging etc [use with caution!]
 char recharging_state = 0;               // Stores the state of the recharging program (0 is not currently running)
+char switch_held = 0;                    // Used for detected when the cursor switch is held to override program choice
+char choose_program_mode = 0;
+char program_count = 7;
+char program_selection;
 
 float battery_low_threshold = 3.63;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
 float battery_high_threshold = 3.97;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
@@ -56,6 +61,14 @@
 ///This is the main loop for the Beautiful Meme code.  The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected.
 void main_loop()
 {
+    if(switch_held == 1)switch_held=2;
+    if(switch_held == 3) {
+        //The switch has been held right and then released:  stop the current program
+        previous_program = main_program_state;
+        program_selection = previous_program;
+        choose_program_mode = 1;
+        set_program(255);
+    }
     if(use_recharging_program == 1)recharging_program();
     update_display();
     if(recharging_state == 0) {
@@ -86,11 +99,13 @@
                 straight_random_walk_with_interaction_program();
                 break;
             case 5:
-                find_space_program();
+                find_space_program(1);
                 break;
             case 6:
-                clustering_program(0);
+                clustering_program(0,1);
                 break;
+            case 255:
+                stop_program();
         }
     }
     step_cycle=1-step_cycle;
@@ -98,7 +113,8 @@
 
 ///Place user code here that should be run after initialisation but before the main loop
 void user_code_setup()
-{  
+{
+    out("70 degrees: %d\n",get_time_based_turn_time(1, 70));
     wait(0.8);
     display.clear_display();
     display.set_position(0,0);
@@ -116,7 +132,7 @@
     }
     start_infrared_timers();
     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
-    set_program(0);
+    set_program(5);
     set_leds(0x00,0x00);
     set_center_led(3,0.5);
     display.clear_display();
@@ -133,8 +149,37 @@
 {
     /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed
     /// NB For maximum compatability it is recommended to minimise reliance on center button press
-
-    //pc.printf("User switch code block: %d\n",switch_state);
+    if(choose_program_mode == 0) {
+        if(switch_state == 8) switch_held = 1;
+        else if(switch_state == 0 && switch_held == 2) switch_held = 3;
+        else switch_held = 0;
+    } else {
+        // We are in choose program mode
+        if(switch_state == 8) {
+            program_selection ++;
+            if(program_selection > program_count) program_selection = 0;
+            if(program_selection == program_count) set_program_info("RECHARGE");
+            else set_program_info(get_program_name(program_selection));
+        }
+        if(switch_state == 4) {
+            program_selection --;
+            if(program_selection == 0) program_selection = program_count;   
+            if(program_selection == program_count) set_program_info("RECHARGE");
+            else set_program_info(get_program_name(program_selection));
+        }
+        if(switch_state == 1 || switch_state == 2){
+            if(program_selection == program_count){
+                  recharging_state = 1;
+                  set_program(previous_program);
+                  strcpy(prog_name,"CHARGING PROGRAM");
+                  set_program_info("HEAD TO BEACON");  
+                  
+            }   
+            else set_program(program_selection);
+            choose_program_mode = 0; 
+            switch_held = 0;
+        }
+    }
 }
 
 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
@@ -150,9 +195,9 @@
 }
 
 char * get_program_name(int index)
-{   
+{
     char * ret_name = new char[17];
-    switch(index){
+    switch(index) {
         case 0:
             strcpy(ret_name,"FACE BEACON");
             break;
@@ -174,6 +219,8 @@
         case 6:
             strcpy(ret_name,"CLUSTERING");
             break;
+        case 255:
+            strcpy(ret_name,"PROGRAM:");
     }
     return ret_name;
 }
@@ -184,7 +231,7 @@
     program_changed = 1;
     program_run_init = 1;
     strcpy(prog_info,"");
-    strcpy(prog_name,get_program_name(index)); 
+    strcpy(prog_name,get_program_name(index));
 }
 
 void set_program_info(char * info)
@@ -198,9 +245,9 @@
     if(program_changed == 1) {
         program_changed = 0;
         display.clear_display();
-        
+
         if(display_debug_inf==1) display_debug_info();
-        else{
+        else {
             display.set_position(0,0);
             display.write_string(prog_name);
         }