Diff: main.cpp

Revision:

10:1b09d4bb847b

Parent:

9:085e090e1ec1

Child:

11:7b3ee540ba56

--- a/main.cpp	Mon Oct 26 11:16:05 2015 +0000
+++ b/main.cpp	Mon Oct 26 23:58:08 2015 +0000
@@ -10,22 +10,21 @@
 ***********************************************************************/
 
 /// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.1
+/// Version 0.2
 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
 /// University of York
 
-
+/// Include main.h - this includes psiswarm.h all the other necessary core files
+#include "main.h"
 
-/// Include psiswarm.h - this includes all the other necessary core files
-
-#include "psiswarm.h"
+char * program_name = "B-Meme";
+char * author_name  = "YRL";
+char * version_name = "0.2";
 
 //  IMPORTANT!!!
 //  Do not call the IR functions at all as they will interfere with the correct operation of this program
 //  Instead, use the values held in the variables below; they are updated every 500ms
 
-
-
 char beacon_found = 0;                   // This will be a 1 when a beacon was detected during the previous 500ms window
 int beacon_heading = 0;                  // This is the heading from the last time a beacon was detected
 char robots_found[8];                    // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
@@ -34,96 +33,61 @@
 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
 
-char * program_name = "B-Meme";
-char * author_name  = "YRL";
-char * version_name = "1.0";
-
+char default_normal_program = 3;         // The program to run on turn on (after 'face beacon' program)
+char use_recharging_program = 1;         // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
 char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
-char display_debug_inf = 1;            // Set to 1 to show debug info on display
-char main_program_state;
-char old_program_state = 255;
-char step_cycle = 0;
-char success_count = 0;
-char target_reached = 0;
-char was_turning = 0;
+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 program_changed = 0;                // Flag to update display when program is changed
+char success_count = 0;                  // Flag to indicate the success of a program
+char step_cycle = 0;                     // Alternates between 0 and 1 in successive time-steps
+char target_reached = 0;                 // Flag to indicate if a program target has been reached
+char prog_name [17];                     // Stores the name of the running program [line 0 on the display]
+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)
 Ticker main_loop_ticker;
 
-
-///This is the main loop for the Beautiful Meme code.  The code block is run once every 500mS* once all the IR samples have been collected.
+///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(use_recharging_program == 1)recharging_program();
     update_display();
-    switch(main_program_state) {
-        case 0: //Case 0 is the initial program: turn to face beacon
-            if(step_cycle == 0) {
-                char turn_status = turn_to_bearing(0);
-                if(turn_status == 0) {
-                    success_count ++;
-                    if(success_count > 1) main_program_state = 1;
-                } else success_count = 0;
-            }
-            break;
-        case 1:
-            target_reached = 0;
-            head_to_bearing_program(0);
-            if(target_reached == 1) main_program_state = 2;
-            break;
-        case 2:
-            target_reached = 0;
-            head_to_bearing_program(180);
-            if(target_reached == 1) main_program_state = 1;
-            break;
+    if(recharging_state == 0) {
+        switch(main_program_state) {
+            case 0: //Case 0 is the initial program: turn to face beacon
+                if(step_cycle == 0) {
+                    char turn_status = turn_to_bearing(0);
+                    if(turn_status == 0) {
+                        success_count ++;
+                        if(success_count > 1) set_program(default_normal_program);
+                    } else success_count = 0;
+                }
+                break;
+            case 1:
+                target_reached = 0;
+                head_to_bearing_program(0);
+                if(target_reached == 1) set_program(2);
+                break;
+            case 2:
+                target_reached = 0;
+                head_to_bearing_program(180);
+                if(target_reached == 1) set_program(1);
+                break;
+            case 3:
+                curved_random_walk_with_interaction_program();
+                break;
+            case 4:
+                straight_random_walk_with_interaction_program();
+                break;
+            case 5:
+                find_space_program();
+                break;
+        }
     }
     step_cycle=1-step_cycle;
 }
 
-
-//The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
-void head_to_bearing_program(int target_bearing)
-{
-    if(step_cycle == 0 || was_turning == 0) {
-        // Check if we are heading in the right bearing (+- 25 degrees)
-        int current_bearing = (360 - beacon_heading) % 360;
-        // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
-        char bearing_ok = 0;
-        int lower_bound = target_bearing - 25;
-        int upper_bound = target_bearing + 25;
-        if(lower_bound < 0) {
-            if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
-        } else if(upper_bound > 359) {
-            if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
-        } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
-        // Check if there is an obstacle in front of us
-        if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
-        else {
-            // Now move forward if we are facing correct bearing, otherwise turn
-            if(bearing_ok == 1) {
-                //Check if anything is in front of us to determine speed - if it is, move slowly
-                int t_time = 6 * BEACON_PERIOD;
-                float t_speed = 1.0;
-                if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
-                    t_time = 4 * BEACON_PERIOD;
-                    t_speed = 0.6;
-                }
-                if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
-                    t_time = 3 * BEACON_PERIOD;
-                    t_speed = 0.4;
-                }
-                if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
-                    t_time = 2 * BEACON_PERIOD;
-                    t_speed = 0.2;
-                }
-                time_based_forward(t_speed,t_time,0);
-                was_turning = 0;
-            } else {
-                turn_to_bearing(target_bearing);
-                was_turning = 1;
-            }
-        }
-    }
-}
-
-
 ///Place user code here that should be run after initialisation but before the main loop
 void user_code_setup()
 {
@@ -144,9 +108,9 @@
     }
     start_infrared_timers();
     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
-    main_program_state = 1;
+    set_program(0);
     set_leds(0x00,0x00);
-    set_center_led(3,1);
+    set_center_led(3,0.5);
     display.clear_display();
     display.set_position(0,0);
     display.write_string("BEACON FOUND AT");
@@ -177,36 +141,70 @@
     }
 }
 
+char * get_program_name(int index)
+{   
+    char * ret_name = new char[17];
+    switch(index){
+        case 0:
+            strcpy(ret_name,"FACE BEACON");
+            break;
+        case 1:
+            strcpy(ret_name,"HEAD TO BEACON");
+            break;
+        case 2:
+            strcpy(ret_name,"HEAD TO SOUTH");
+            break;
+        case 3:
+            strcpy(ret_name,"RANDOM WALK 1");
+            break;
+        case 4:
+            strcpy(ret_name,"RANDOM WALK 2");
+            break;
+        case 5:
+            strcpy(ret_name,"FIND SPACE");
+            break;
+    }
+    return ret_name;
+}
+
+void set_program(int index)
+{
+    main_program_state = index;
+    program_changed = 1;
+    strcpy(prog_info,"");
+    strcpy(prog_name,get_program_name(index)); 
+}
+
+void set_program_info(char * info)
+{
+    strcpy(prog_info,info);
+    program_changed = 1;
+}
 
 void update_display()
 {
-    if(main_program_state != old_program_state) {
-        old_program_state = main_program_state;
+    if(program_changed == 1) {
+        program_changed = 0;
         display.clear_display();
-        display.set_position(0,0);
-        switch(main_program_state) {
-            case 0:
-                display.write_string("P:FACE BEACON");
-                break;
-            case 1:
-                display.write_string("P:HEAD TO BEACON");
-                break;
-            case 2:
-                display.write_string("P:HEAD TO SOUTH");
-                break;
+        
+        if(display_debug_inf==1) display_debug_info();
+        else{
+            display.set_position(0,0);
+            display.write_string(prog_name);
         }
+        display.set_position(1,0);
+        display.write_string(prog_info);
     }
-    if(display_debug_inf==1) display_debug_info();
 }
 
 void display_debug_info()
 {
     char disp_line[16] = "- - - - - - - -";
     if(beacon_found==1)disp_line[0]='B';
-    for(int i=0; i<7; i++) {
-        if(robots_found[i])disp_line[((i+1)*2)]=49+i;
+    for(int i=1; i<8; i++) {
+        if(robots_found[i])disp_line[((i)*2)]=48+i;
     }
-    display.set_position(1,0);
+    display.set_position(0,0);
     display.write_string(disp_line);
 }