Diff: main.cpp

Revision:

9:085e090e1ec1

Parent:

8:00558287a4ef

Child:

10:1b09d4bb847b

--- a/main.cpp	Thu Oct 22 15:36:16 2015 +0000
+++ b/main.cpp	Mon Oct 26 11:16:05 2015 +0000
@@ -14,13 +14,18 @@
 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
 /// University of York
 
+
+
 /// Include psiswarm.h - this includes all the other necessary core files
+
 #include "psiswarm.h"
 
 //  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,41 +39,123 @@
 char * version_name = "1.0";
 
 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;
+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.
+void main_loop()
+{
+    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;
+    }
+    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()
 {
-    for(int i=0;i<10;i++){
-        time_based_turn_degrees(1, 90);
-        wait(1);   
-    }
-    
+    wait(0.8);
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("BEAUTIFUL MEME");
+    display.set_position(1,0);
+    display.write_string("   PROJECT");
+    wait(0.2);
     out("------------------------------------------------------\n");
     out("Beautiful Meme Project Demo Code                      \n");
     out("------------------------------------------------------\n");
     locate_beacon();
     while(beacon_found == 0) {
+        wait(0.5);
         locate_beacon();
     }
-
     start_infrared_timers();
+    main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
+    main_program_state = 1;
+    set_leds(0x00,0x00);
+    set_center_led(3,1);
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("BEACON FOUND AT");
+    display.set_position(1,0);
+    char degrees_string[16];
+    sprintf(degrees_string,"%d DEGREES",beacon_heading);
+    display.write_string(degrees_string);
 }
 
-///This function is the loop where user code should be placed
-void user_code_loop()
-{
-
-    ///Do not place code within a loop, but consider this function to be a loop that is always run
-    ///unless the user code is externally paused (such as by debug or recharging system)
-
-    //pc.printf("User loop code block\n");
-
-    
-    wait(0.5);
-}
-
-
 /// Code goes here to handle what should happen when the user switch is pressed
 void handle_switch_event(char switch_state)
 {
@@ -86,12 +173,43 @@
     user_code_setup();
     user_code_running = 1;
     while(1) {
-        if(user_code_running)user_code_loop();
-        if(demo_on) demo_mode();
-        //wait_us(5);
+        wait(1);
     }
 }
 
+
+void update_display()
+{
+    if(main_program_state != old_program_state) {
+        old_program_state = main_program_state;
+        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();
+}
+
+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;
+    }
+    display.set_position(1,0);
+    display.write_string(disp_line);
+}
+
 /// Verbose output
 void out(const char* format, ...)
 {