Diff: beacon.cpp

Revision:

9:085e090e1ec1

Parent:

7:ef9ab01b9e26

Child:

10:1b09d4bb847b

--- a/beacon.cpp	Thu Oct 22 15:36:16 2015 +0000
+++ b/beacon.cpp	Mon Oct 26 11:16:05 2015 +0000
@@ -6,6 +6,7 @@
 // Beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots
 
 #include "psiswarm.h"
+#include "main.h"
 
 int pulse_step = 1;                     //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8
 int low_threshold;                      //Set to be 2x mean background IR
@@ -18,11 +19,12 @@
 
 
 
-/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in 20ms blocks.
+/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in [BEACON_PERIOD / 2.5] (20ms) blocks.
 /// The infrared beacon is set to give a 50ms burst of IR every 500ms.  We should thus see in the sampled radiation 2 blocks
 /// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart.
 void locate_beacon()
 {
+    int sample_period = (BEACON_PERIOD * 2) / 5;
     out("1) Searching for IR beacon...");
     unsigned short samples[50][9];
     Timer beacon_timer;
@@ -31,12 +33,19 @@
     //This loop samples the background IR values at 50Hz for 1 second and stores in an array
     for(int i=0; i<50; i++) {
         store_background_raw_ir_values ();
+        if(i%2 == 0){
+            set_center_led(1, 0.5);
+            set_leds(0xAA,0x55);
+        }else{
+            set_center_led(2, 0.5);
+            set_leds(0x55,0xAA);
+        }
         samples[i][8]=0;
         for(int j=0; j<8; j++) {
             samples[i][j] = get_background_raw_ir_value(j);
             samples[i][8] += get_background_raw_ir_value(j);
         }
-        offset+=20000;
+        offset+=sample_period;
         while(beacon_timer.read_us() < offset) {}
     }
 
@@ -93,13 +102,13 @@
     }
     //Count and display matches
     int beacon_detected_count = 0;
-    char output_string[251] = "";
+    //char output_string[251] = "";
     for(int i=0;i<50;i++){
         if(beacon_detected_indices[i] == 1){
             beacon_detected_count++;
-            char index_string[6];
-            sprintf(index_string,"[%d],",i);   
-            strcat(output_string,index_string);
+           // char index_string[6];
+           // sprintf(index_string,"[%d],",i);   
+           // strcat(output_string,index_string);
         }
     }
     //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string);
@@ -175,13 +184,14 @@
                 //}
                 out("2) Synchronising...\n");
                 // Calculate the offset to the expected start of the next beacon pulse
-                int microseconds_offset = (20000 * mid_point) - 25000;
+                int microseconds_offset = (sample_period * mid_point) - sample_period - (sample_period / 4);
                 //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us());
-                if(microseconds_offset < 0) microseconds_offset += 500000;
+                int cycle_period = (BEACON_PERIOD * 10);
+                if(microseconds_offset < 0) microseconds_offset += cycle_period;
                 //If we have missed the start of the beacon this cycle, wait until the next cycle
-                while(beacon_timer.read_us()%500000 > microseconds_offset){};
+                while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){};
                 //Now wait until the start of the beacon pulse
-                while(beacon_timer.read_us()%500000 < microseconds_offset){};
+                while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){};
                 /*
                 out("Now:%d",beacon_timer.read_us());
                 Timer test_timer;
@@ -194,7 +204,7 @@
                 */
             }else{
                 beacon_found = 0;
-                out("Beacon not found: a matching second block 500ms after first block not detected\n");   
+                out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100));   
             }
         }else{
             beacon_found = 0;
@@ -206,7 +216,13 @@
         if(beacon_detected_count > 6) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count);
         else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count);
     }
-    wait(1);
+    if(beacon_found == 0){
+        set_leds(0x00,0x00);
+        set_center_led(1, 1);
+        display.clear_display();
+        display.set_position(0,0);
+        display.write_string("BEACON NOT FOUND");   
+    }
 }
 
 // The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to
@@ -219,10 +235,10 @@
     // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step
     out("3) Starting TDMA infrared timers\n");
     beacon_debug_timer.start();
-    wait_us(4000);  //Adjusted down for PC message
-    ir_emitter_ticker.attach_us(emitter_ticker_block,50000);
-    wait_us(20000); //Wait for middle of pulse
-    ir_sample_ticker.attach_us(sample_ticker_block,50000);
+    wait_us(BEACON_PERIOD / 10);  
+    ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD);
+    wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse
+    ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD);
 }
 
 
@@ -251,7 +267,7 @@
     if(pulse_step-1 == robot_id){
         IF_set_IR_emitter_output(0, 1);
         IF_set_IR_emitter_output(1, 1);
-        ir_emitter_timeout.attach_us(emitter_timeout_block,40000);
+        ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10);
     }  
 }
 
@@ -332,4 +348,28 @@
         out("[%d,%d]",i,background_sensor_data[i]);
     }
     out("\n\n");
+}
+
+//Returns a 0 if turn is likely to complete in a single timestep, 1 if it is beyond range for single timestep and 2 if the beacon is not found so bearing unknown
+char turn_to_bearing(int bearing)
+{
+    if(beacon_found == 0){
+        out("Beacon not found: cannot turn to specific bearing");   
+        return 2;
+    }else{
+        //First calculate the bearing using the angle of beacon relative to robot
+        int current_bearing = 360 - beacon_heading;
+        //Now work out turn needed to face intended heading
+        int target_turn = (bearing - current_bearing) % 360;
+        if(target_turn > 180) target_turn -= 360;
+        if(target_turn < -180) target_turn += 360;
+        //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that
+        char beyond_limit = 0;
+        int turn_limit = BEACON_PERIOD / 358;
+        if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;};
+        if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;};
+        out("Turning %d degrees\n",target_turn);
+        time_based_turn_degrees(1, target_turn,1);
+        return beyond_limit;
+    }
 }
\ No newline at end of file