beacon.cpp

00001 /// PsiSwarm Beautiful Meme Project Source Code
00002 /// Version 0.1
00003 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
00004 /// University of York
00005 
00006 // beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots
00007 
00008 #include "main.h"
00009 
00010 int pulse_step = 1;                     //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8
00011 int low_threshold;                      //Set to be 2x mean background IR
00012 int beacon_threshold;                   //Set to be 4x mean background IR
00013 unsigned short ir_sensor_data[9][8];    // The raw sensor data from all 9x 50ms sample windows
00014 Ticker ir_sample_ticker;                // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot
00015 Ticker ir_emitter_ticker;               // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot
00016 Timeout ir_emitter_timeout;             // Timeout for turning off the IR emitters after 40ms
00017 Timer beacon_debug_timer;               // Timer for debug information only [remove later?]
00018 
00019 char show_ir_debug_info = 0;            // Set to 1 to display (via PC) the list of IR readings & visible robots every timestep
00020 
00021 /// 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.
00022 /// The infrared beacon is set to give a 25ms burst of IR every 250ms.  We should thus see in the sampled radiation 2 blocks
00023 /// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart.
00024 void locate_beacon()
00025 {
00026     int sample_period = (BEACON_PERIOD * 2) / 5;
00027     out("1) Searching for IR beacon...");
00028     unsigned short samples[50][9];
00029     Timer beacon_timer;
00030     beacon_timer.start();
00031     int offset = 0;
00032     //This loop samples the background IR values at 50Hz for 1 second and stores in an array
00033     for(int i=0; i<50; i++) {
00034         store_background_raw_ir_values ();
00035         if(i%2 == 0){
00036             set_center_led(1, 0.5);
00037             set_leds(0xAA,0x55);
00038         }else{
00039             set_center_led(2, 0.5);
00040             set_leds(0x55,0xAA);
00041         }
00042         samples[i][8]=0;
00043         for(int j=0; j<8; j++) {
00044             samples[i][j] = get_background_raw_ir_value(j);
00045             samples[i][8] += get_background_raw_ir_value(j);
00046         }
00047         offset+=sample_period;
00048         while(beacon_timer.read_us() < offset) {}
00049     }
00050 
00051     //Print values: for testing [comment out]
00052     /*
00053     for(int i=0; i<50; i++) {
00054         out("IR %d:",i);
00055         for(int j=0; j<8; j++) {
00056             out("[%d:%d]",j,samples[i][j]);
00057         }
00058         out("  [SUM:%d]\n",samples[i][8]);
00059     }
00060     */
00061 
00062     //Bubble sort sums to find (6) highest values
00063     unsigned short sorted_array[50];
00064     for(int i=0; i<50; i++) {
00065         sorted_array[i]=samples[i][8];
00066     }
00067     for (int c = 0 ; c < 49; c++) {
00068         for (int d = 0 ; d < (50-c-1); d++) {
00069             if (sorted_array[d] > sorted_array[d+1]) {
00070                 unsigned short swap = sorted_array[d];
00071                 sorted_array[d] = sorted_array[d+1];
00072                 sorted_array[d+1] = swap;
00073             }
00074         }
00075     }
00076 
00077     //Print sorted values: for testing [comment out]
00078     /*
00079     out("Sorted values:");
00080     for (int c = 0 ; c < 50 ; c++ ) {
00081         out("%d", sorted_array[c]);
00082         if(c<49)out(",");
00083     }
00084     out("\n");
00085     */
00086 
00087     // Calculate mean background sum value by looking at 44 lowest sum values
00088     int background_mean = 0;
00089     for(int i=0;i<44;i++)background_mean += sorted_array[i];
00090     background_mean /= 44;
00091     
00092     //out("Background mean value: %d\n",background_mean);
00093     
00094     //Our beacon threshold will be 4x the background mean value; find all instances where this occurs
00095     low_threshold = background_mean * 2;
00096     beacon_threshold = background_mean * 4;
00097     char beacon_detected_indices[50];
00098     for(int i=0;i<50;i++){
00099         if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1;
00100         else beacon_detected_indices[i]=0;
00101     }
00102     //Count and display matches
00103     int beacon_detected_count = 0;
00104     //char output_string[251] = "";
00105     for(int i=0;i<50;i++){
00106         if(beacon_detected_indices[i] == 1){
00107             beacon_detected_count++;
00108            // char index_string[6];
00109            // sprintf(index_string,"[%d],",i);   
00110            // strcat(output_string,index_string);
00111         }
00112     }
00113     //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string);
00114     
00115     //We will use this array to store average values for each sensor when the beacon is detected
00116     unsigned short beacon_averages[8];
00117     char beacon_averages_count = 0;
00118     for(int i=0;i<8;i++)beacon_averages[i]=0;
00119     
00120     //Now determine if the beacon is correctly found: must adhere to a set of rules
00121     //Firstly, we should have not less than 4 and not more than 6 positive matches 
00122     if(beacon_detected_count>3 && beacon_detected_count<7){
00123         // Now verify that the positive samples are in valid places...
00124         // Find first positive sample
00125         int first_index = 0;
00126         //out("Here\n",first_index);
00127         
00128         while(beacon_detected_indices[first_index]==0)first_index ++;
00129         
00130         //out("First index:%d\n",first_index);
00131         
00132         
00133         // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high
00134         if(first_index == 0){
00135             if(beacon_detected_indices[49]>0)first_index = 49;
00136             if(beacon_detected_indices[48]>0)first_index = 48;   
00137         }
00138         
00139         beacon_averages_count++;
00140         for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];}
00141         
00142         // Now count the length of the 'block' of positive hits: must be equal to 2 or 3
00143         char block_length = 1;
00144         int end_index = first_index + 1;
00145         if(end_index == 50) end_index = 0;
00146         while(beacon_detected_indices[end_index]>0){
00147             beacon_averages_count++;
00148             for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];}
00149             block_length ++;
00150             end_index ++;
00151             if(end_index == 50) end_index = 0;   
00152         }
00153         if(block_length==2 || block_length == 3){
00154             //We have found the first correct block and it is valid; now calculate its mid-point and check that the second block is also present 500ms later
00155             float mid_point;
00156             char second_block_okay = 0;
00157             if(block_length == 2){
00158                 mid_point = first_index + 0.5;
00159                 char second_block_low = first_index + 25;
00160                 char second_block_high = first_index + 26;
00161                 if(second_block_low > 49) second_block_low -= 50;
00162                 if(second_block_high > 49) second_block_high -= 50;
00163                 beacon_averages_count+=2;
00164                 for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];}
00165                 if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1;
00166             }
00167             if(block_length == 3){
00168                 mid_point = first_index + 1;
00169                 if(mid_point == 50) mid_point = 0;
00170                 char second_block_single = first_index + 26;
00171                 if(second_block_single > 49) second_block_single -= 50;
00172                 beacon_averages_count++;
00173                 for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];}
00174                 if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1;
00175             }   
00176             if(second_block_okay >0){
00177                 beacon_found = 1;
00178                 beacon_heading = get_bearing_from_ir_array(beacon_averages);
00179                 out("Found at %d degrees\n",beacon_heading);
00180                 //for(int i=0;i<8;i++){
00181                 //    beacon_averages[i] /= beacon_averages_count;
00182                 //    out("[%d]",beacon_averages[i]);   
00183                 //}
00184                 out("2) Synchronising...\n");
00185                 // Calculate the offset to the expected start of the next beacon pulse
00186                 int microseconds_offset = (sample_period * mid_point) - sample_period - (sample_period / 4);
00187                 //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us());
00188                 int cycle_period = (BEACON_PERIOD * 10);
00189                 if(microseconds_offset < 0) microseconds_offset += cycle_period;
00190                 //If we have missed the start of the beacon this cycle, wait until the next cycle
00191                 while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){};
00192                 //Now wait until the start of the beacon pulse
00193                 while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){};
00194                 /*
00195                 out("Now:%d",beacon_timer.read_us());
00196                 Timer test_timer;
00197                 test_timer.start();
00198                 for(int i=0;i<50;i++){
00199                     store_background_raw_ir_values ();
00200                     out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2));
00201                     while(test_timer.read_ms() % 10 < 9){};
00202                 }
00203                 */
00204             }else{
00205                 beacon_found = 0;
00206                 out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100));   
00207             }
00208         }else{
00209             beacon_found = 0;
00210             if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index);
00211             if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length);   
00212         }
00213     } else {
00214         beacon_found = 0;
00215         if(beacon_detected_count > 6) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count);
00216         else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count);
00217     }
00218     if(beacon_found == 0){
00219         set_leds(0x00,0x00);
00220         set_center_led(1, 1);
00221         display.clear_display();
00222         display.set_position(0,0);
00223         display.write_string("BEACON NOT FOUND");   
00224     }
00225 }
00226 
00227 // The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to
00228 // It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot
00229 // The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot  
00230 void start_infrared_timers()
00231 {
00232     // At this point we should be exactly at the start of a beacon cycle.
00233     // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse)
00234     // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step
00235     out("3) Starting TDMA infrared timers\n");
00236     beacon_debug_timer.start();
00237     wait_us(BEACON_PERIOD / 10);  
00238     ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD);
00239     wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse
00240     ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD);
00241 }
00242 
00243 
00244 //Return the max value in IR array
00245 unsigned short get_highest_sample(unsigned short * ir_array){
00246     unsigned short highest = 0;
00247     for(int i=0;i<8;i++){
00248         if(ir_array[i]>highest) highest=ir_array[i];
00249     }   
00250     return highest;
00251 }
00252 
00253 //Return the sum total of IR array
00254 unsigned short get_sum_sample(unsigned short * ir_array){
00255     unsigned short sum = 0;
00256     for(int i=0;i<8;i++){
00257         sum+=ir_array[i];
00258     }   
00259     return sum;  
00260 }
00261 
00262 //The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID
00263 //It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters
00264 void emitter_ticker_block(){
00265     //If the time-step (-1) equals my ID, turn on my emitters for 40ms
00266     if(pulse_step-1 == robot_id && disable_ir_emitters == 0){
00267         IF_set_IR_emitter_output(0, 1);
00268         IF_set_IR_emitter_output(1, 1);
00269         ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10);
00270     }  
00271 }
00272 
00273 //Turn off the emitters
00274 void emitter_timeout_block(){
00275      //Turn off IR emitters
00276      IF_set_IR_emitter_output(0, 0);
00277      IF_set_IR_emitter_output(1, 0);
00278 }
00279 
00280 //The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot
00281 //There are 10 time slots in each 500ms period
00282 //Slot 0 is when the beacon is flashing
00283 //Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[]
00284 //Slot 2-8 are for the 7 robots; slot-1 = robot_id
00285 //In slot 9, the robot processes the data [and doesn't store and new readings]
00286 //It checks if the beacon is visible, if any robots are, calculates their bearings if they are, and transfers the background and active IR data for the robot
00287 void sample_ticker_block(){
00288     //If we are in time-step 0 to 8, store the background data in an array
00289     if(pulse_step < 9){
00290     store_background_raw_ir_values ();
00291     for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i);
00292     }else{
00293       //If not, process the data   
00294       for(int i=0;i<9;i++){
00295          unsigned short sum = get_sum_sample(ir_sensor_data[i]);
00296          unsigned short highest = get_highest_sample(ir_sensor_data[i]);
00297          //Check if beacon is visible
00298          if(i==0){
00299              if(sum > beacon_threshold){
00300                  beacon_found = 1;
00301                  beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]);
00302              }else beacon_found = 0;
00303              //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]);
00304          }
00305          if(i==1){
00306             for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j];   
00307          }
00308          if(i>1){
00309             char test_robot = i-1;
00310             if(test_robot == robot_id){
00311                 for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j];   
00312             }else{
00313                 if(sum > low_threshold){
00314                     robots_found[test_robot] = 1;
00315                     //Debug--
00316                     //out("Robot %d: [%d][%d][%d][%d][%d][%d][%d][%d]\n",test_robot,ir_sensor_data[i][0],ir_sensor_data[i][1],ir_sensor_data[i][2],ir_sensor_data[i][3],ir_sensor_data[i][4],ir_sensor_data[i][5],ir_sensor_data[i][6],ir_sensor_data[i][7]);
00317                     robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]);
00318                     robots_distance[test_robot] = highest;
00319                 }else robots_found[test_robot] = 0;   
00320             }  
00321          }
00322       }
00323       if(show_ir_debug_info == 1)display_ir_readings();
00324     }
00325     //Increment pulse step
00326     pulse_step++;
00327     if(pulse_step == 10) pulse_step = 0;
00328 }
00329 
00330 
00331 //Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles
00332 void display_ir_readings()
00333 {
00334     out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms());
00335     if(beacon_found==1){
00336         out("Beacon detected at %d degrees\n",beacon_heading);
00337     }  
00338     for(int j=1;j<8;j++){
00339         if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]);       
00340     }   
00341     out("Reflected values:");
00342     for(int i=0;i<8;i++){
00343         out("[%d,%d]",i,reflected_sensor_data[i]);
00344     }
00345     out("\nBackground values:");
00346     for(int i=0;i<8;i++){
00347         out("[%d,%d]",i,background_sensor_data[i]);
00348     }
00349     out("\n\n");
00350 }
00351 
00352 //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
00353 char turn_to_bearing(int bearing)
00354 {
00355     if(beacon_found == 0){
00356         out("Beacon not found: cannot turn to specific bearing");   
00357         return 2;
00358     }else{
00359         //First calculate the bearing using the angle of beacon relative to robot
00360         int current_bearing = 360 - beacon_heading;
00361         //Now work out turn needed to face intended heading
00362         int target_turn = (bearing - current_bearing) % 360;
00363         //Adjust to take 10% off turn, stops overshoot
00364         target_turn = (target_turn * 9) / 10;
00365         if(target_turn > 180) target_turn -= 360;
00366         if(target_turn < -180) target_turn += 360;
00367         //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that
00368         char beyond_limit = 0;
00369         int turn_limit = BEACON_PERIOD / 358;
00370         if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;};
00371         if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;};
00372         out("Turning %d degrees\n",target_turn);
00373         time_based_turn_degrees(1, target_turn,1);
00374         return beyond_limit;
00375     }
00376 }