Science Memeseum / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCodeNew by James O'Keeffe

beacon.cpp@6:ff3c66f7372b, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 00:46:14 2015 +0000
Revision:
6:ff3c66f7372b
Child:
7:ef9ab01b9e26
Initial version: beacon detection and sync. code, bearing estimation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 6:ff3c66f7372b 1 /// PsiSwarm Beautiful Meme Project Source Code
jah128 6:ff3c66f7372b 2 /// Version 0.1
jah128 6:ff3c66f7372b 3 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 6:ff3c66f7372b 4 /// University of York
jah128 6:ff3c66f7372b 5
jah128 6:ff3c66f7372b 6 // Beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots
jah128 6:ff3c66f7372b 7
jah128 6:ff3c66f7372b 8 #include "psiswarm.h"
jah128 6:ff3c66f7372b 9
jah128 6:ff3c66f7372b 10 int pulse_step = 1; //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8
jah128 6:ff3c66f7372b 11 int low_threshold; //Set to be 2x mean background IR
jah128 6:ff3c66f7372b 12 int beacon_threshold; //Set to be 4x mean background IR
jah128 6:ff3c66f7372b 13 unsigned short ir_sensor_data[9][8]; // The raw sensor data from all 9x 50ms sample windows
jah128 6:ff3c66f7372b 14 Ticker ir_sample_ticker; // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot
jah128 6:ff3c66f7372b 15 Ticker ir_emitter_ticker; // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot
jah128 6:ff3c66f7372b 16 Timeout ir_emitter_timeout; // Timeout for turning off the IR emitters after 40ms
jah128 6:ff3c66f7372b 17 Timer beacon_debug_timer; // Timer for debug information only [remove later?]
jah128 6:ff3c66f7372b 18
jah128 6:ff3c66f7372b 19
jah128 6:ff3c66f7372b 20
jah128 6:ff3c66f7372b 21 /// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in 20ms blocks.
jah128 6:ff3c66f7372b 22 /// The infrared beacon is set to give a 50ms burst of IR every 500ms. We should thus see in the sampled radiation 2 blocks
jah128 6:ff3c66f7372b 23 /// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart.
jah128 6:ff3c66f7372b 24 void locate_beacon()
jah128 6:ff3c66f7372b 25 {
jah128 6:ff3c66f7372b 26 out("1) Searching for IR beacon...");
jah128 6:ff3c66f7372b 27 unsigned short samples[50][9];
jah128 6:ff3c66f7372b 28 Timer beacon_timer;
jah128 6:ff3c66f7372b 29 beacon_timer.start();
jah128 6:ff3c66f7372b 30 int offset = 0;
jah128 6:ff3c66f7372b 31 //This loop samples the background IR values at 50Hz for 1 second and stores in an array
jah128 6:ff3c66f7372b 32 for(int i=0; i<50; i++) {
jah128 6:ff3c66f7372b 33 store_background_raw_ir_values ();
jah128 6:ff3c66f7372b 34 samples[i][8]=0;
jah128 6:ff3c66f7372b 35 for(int j=0; j<8; j++) {
jah128 6:ff3c66f7372b 36 samples[i][j] = get_background_raw_ir_value(j);
jah128 6:ff3c66f7372b 37 samples[i][8] += get_background_raw_ir_value(j);
jah128 6:ff3c66f7372b 38 }
jah128 6:ff3c66f7372b 39 offset+=20000;
jah128 6:ff3c66f7372b 40 while(beacon_timer.read_us() < offset) {}
jah128 6:ff3c66f7372b 41 }
jah128 6:ff3c66f7372b 42
jah128 6:ff3c66f7372b 43 //Print values: for testing [comment out]
jah128 6:ff3c66f7372b 44 /*
jah128 6:ff3c66f7372b 45 for(int i=0; i<50; i++) {
jah128 6:ff3c66f7372b 46 out("IR %d:",i);
jah128 6:ff3c66f7372b 47 for(int j=0; j<8; j++) {
jah128 6:ff3c66f7372b 48 out("[%d:%d]",j,samples[i][j]);
jah128 6:ff3c66f7372b 49 }
jah128 6:ff3c66f7372b 50 out(" [SUM:%d]\n",samples[i][8]);
jah128 6:ff3c66f7372b 51 }
jah128 6:ff3c66f7372b 52 */
jah128 6:ff3c66f7372b 53
jah128 6:ff3c66f7372b 54 //Bubble sort sums to find (6) highest values
jah128 6:ff3c66f7372b 55 unsigned short sorted_array[50];
jah128 6:ff3c66f7372b 56 for(int i=0; i<50; i++) {
jah128 6:ff3c66f7372b 57 sorted_array[i]=samples[i][8];
jah128 6:ff3c66f7372b 58 }
jah128 6:ff3c66f7372b 59 for (int c = 0 ; c < 49; c++) {
jah128 6:ff3c66f7372b 60 for (int d = 0 ; d < (50-c-1); d++) {
jah128 6:ff3c66f7372b 61 if (sorted_array[d] > sorted_array[d+1]) {
jah128 6:ff3c66f7372b 62 unsigned short swap = sorted_array[d];
jah128 6:ff3c66f7372b 63 sorted_array[d] = sorted_array[d+1];
jah128 6:ff3c66f7372b 64 sorted_array[d+1] = swap;
jah128 6:ff3c66f7372b 65 }
jah128 6:ff3c66f7372b 66 }
jah128 6:ff3c66f7372b 67 }
jah128 6:ff3c66f7372b 68
jah128 6:ff3c66f7372b 69 //Print sorted values: for testing [comment out]
jah128 6:ff3c66f7372b 70 /*
jah128 6:ff3c66f7372b 71 out("Sorted values:");
jah128 6:ff3c66f7372b 72 for (int c = 0 ; c < 50 ; c++ ) {
jah128 6:ff3c66f7372b 73 out("%d", sorted_array[c]);
jah128 6:ff3c66f7372b 74 if(c<49)out(",");
jah128 6:ff3c66f7372b 75 }
jah128 6:ff3c66f7372b 76 out("\n");
jah128 6:ff3c66f7372b 77 */
jah128 6:ff3c66f7372b 78
jah128 6:ff3c66f7372b 79 // Calculate mean background sum value by looking at 44 lowest sum values
jah128 6:ff3c66f7372b 80 int background_mean = 0;
jah128 6:ff3c66f7372b 81 for(int i=0;i<44;i++)background_mean += sorted_array[i];
jah128 6:ff3c66f7372b 82 background_mean /= 44;
jah128 6:ff3c66f7372b 83
jah128 6:ff3c66f7372b 84 //out("Background mean value: %d\n",background_mean);
jah128 6:ff3c66f7372b 85
jah128 6:ff3c66f7372b 86 //Our beacon threshold will be 4x the background mean value; find all instances where this occurs
jah128 6:ff3c66f7372b 87 low_threshold = background_mean * 2;
jah128 6:ff3c66f7372b 88 beacon_threshold = background_mean * 4;
jah128 6:ff3c66f7372b 89 char beacon_detected_indices[50];
jah128 6:ff3c66f7372b 90 for(int i=0;i<50;i++){
jah128 6:ff3c66f7372b 91 if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1;
jah128 6:ff3c66f7372b 92 else beacon_detected_indices[i]=0;
jah128 6:ff3c66f7372b 93 }
jah128 6:ff3c66f7372b 94 //Count and display matches
jah128 6:ff3c66f7372b 95 int beacon_detected_count = 0;
jah128 6:ff3c66f7372b 96 char output_string[251] = "";
jah128 6:ff3c66f7372b 97 for(int i=0;i<50;i++){
jah128 6:ff3c66f7372b 98 if(beacon_detected_indices[i] == 1){
jah128 6:ff3c66f7372b 99 beacon_detected_count++;
jah128 6:ff3c66f7372b 100 char index_string[6];
jah128 6:ff3c66f7372b 101 sprintf(index_string,"[%d],",i);
jah128 6:ff3c66f7372b 102 strcat(output_string,index_string);
jah128 6:ff3c66f7372b 103 }
jah128 6:ff3c66f7372b 104 }
jah128 6:ff3c66f7372b 105 //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string);
jah128 6:ff3c66f7372b 106
jah128 6:ff3c66f7372b 107 //We will use this array to store average values for each sensor when the beacon is detected
jah128 6:ff3c66f7372b 108 unsigned short beacon_averages[8];
jah128 6:ff3c66f7372b 109 char beacon_averages_count = 0;
jah128 6:ff3c66f7372b 110 for(int i=0;i<8;i++)beacon_averages[i]=0;
jah128 6:ff3c66f7372b 111
jah128 6:ff3c66f7372b 112 //Now determine if the beacon is correctly found: must adhere to a set of rules
jah128 6:ff3c66f7372b 113 //Firstly, we should have not less than 4 and not more than 6 positive matches
jah128 6:ff3c66f7372b 114 if(beacon_detected_count>3 && beacon_detected_count<7){
jah128 6:ff3c66f7372b 115 // Now verify that the positive samples are in valid places...
jah128 6:ff3c66f7372b 116 // Find first positive sample
jah128 6:ff3c66f7372b 117 int first_index = 0;
jah128 6:ff3c66f7372b 118 //out("Here\n",first_index);
jah128 6:ff3c66f7372b 119
jah128 6:ff3c66f7372b 120 while(beacon_detected_indices[first_index]==0)first_index ++;
jah128 6:ff3c66f7372b 121
jah128 6:ff3c66f7372b 122 //out("First index:%d\n",first_index);
jah128 6:ff3c66f7372b 123
jah128 6:ff3c66f7372b 124
jah128 6:ff3c66f7372b 125 // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high
jah128 6:ff3c66f7372b 126 if(first_index == 0){
jah128 6:ff3c66f7372b 127 if(beacon_detected_indices[49]>0)first_index = 49;
jah128 6:ff3c66f7372b 128 if(beacon_detected_indices[48]>0)first_index = 48;
jah128 6:ff3c66f7372b 129 }
jah128 6:ff3c66f7372b 130
jah128 6:ff3c66f7372b 131 beacon_averages_count++;
jah128 6:ff3c66f7372b 132 for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];}
jah128 6:ff3c66f7372b 133
jah128 6:ff3c66f7372b 134 // Now count the length of the 'block' of positive hits: must be equal to 2 or 3
jah128 6:ff3c66f7372b 135 char block_length = 1;
jah128 6:ff3c66f7372b 136 int end_index = first_index + 1;
jah128 6:ff3c66f7372b 137 if(end_index == 50) end_index = 0;
jah128 6:ff3c66f7372b 138 while(beacon_detected_indices[end_index]>0){
jah128 6:ff3c66f7372b 139 beacon_averages_count++;
jah128 6:ff3c66f7372b 140 for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];}
jah128 6:ff3c66f7372b 141 block_length ++;
jah128 6:ff3c66f7372b 142 end_index ++;
jah128 6:ff3c66f7372b 143 if(end_index == 50) end_index = 0;
jah128 6:ff3c66f7372b 144 }
jah128 6:ff3c66f7372b 145 if(block_length==2 || block_length == 3){
jah128 6:ff3c66f7372b 146 //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
jah128 6:ff3c66f7372b 147 float mid_point;
jah128 6:ff3c66f7372b 148 char second_block_okay = 0;
jah128 6:ff3c66f7372b 149 if(block_length == 2){
jah128 6:ff3c66f7372b 150 mid_point = first_index + 0.5;
jah128 6:ff3c66f7372b 151 char second_block_low = first_index + 25;
jah128 6:ff3c66f7372b 152 char second_block_high = first_index + 26;
jah128 6:ff3c66f7372b 153 if(second_block_low > 49) second_block_low -= 50;
jah128 6:ff3c66f7372b 154 if(second_block_high > 49) second_block_high -= 50;
jah128 6:ff3c66f7372b 155 beacon_averages_count+=2;
jah128 6:ff3c66f7372b 156 for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];}
jah128 6:ff3c66f7372b 157 if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1;
jah128 6:ff3c66f7372b 158 }
jah128 6:ff3c66f7372b 159 if(block_length == 3){
jah128 6:ff3c66f7372b 160 mid_point = first_index + 1;
jah128 6:ff3c66f7372b 161 if(mid_point == 50) mid_point = 0;
jah128 6:ff3c66f7372b 162 char second_block_single = first_index + 26;
jah128 6:ff3c66f7372b 163 if(second_block_single > 49) second_block_single -= 50;
jah128 6:ff3c66f7372b 164 beacon_averages_count++;
jah128 6:ff3c66f7372b 165 for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];}
jah128 6:ff3c66f7372b 166 if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1;
jah128 6:ff3c66f7372b 167 }
jah128 6:ff3c66f7372b 168 if(second_block_okay >0){
jah128 6:ff3c66f7372b 169 beacon_found = 1;
jah128 6:ff3c66f7372b 170 beacon_heading = get_bearing_from_ir_array(beacon_averages);
jah128 6:ff3c66f7372b 171 out("Found at %d degrees\n",beacon_heading);
jah128 6:ff3c66f7372b 172 //for(int i=0;i<8;i++){
jah128 6:ff3c66f7372b 173 // beacon_averages[i] /= beacon_averages_count;
jah128 6:ff3c66f7372b 174 // out("[%d]",beacon_averages[i]);
jah128 6:ff3c66f7372b 175 //}
jah128 6:ff3c66f7372b 176 out("2) Synchronising...\n");
jah128 6:ff3c66f7372b 177 // Calculate the offset to the expected start of the next beacon pulse
jah128 6:ff3c66f7372b 178 int microseconds_offset = (20000 * mid_point) - 25000;
jah128 6:ff3c66f7372b 179 //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us());
jah128 6:ff3c66f7372b 180 if(microseconds_offset < 0) microseconds_offset += 500000;
jah128 6:ff3c66f7372b 181 //If we have missed the start of the beacon this cycle, wait until the next cycle
jah128 6:ff3c66f7372b 182 while(beacon_timer.read_us()%500000 > microseconds_offset){};
jah128 6:ff3c66f7372b 183 //Now wait until the start of the beacon pulse
jah128 6:ff3c66f7372b 184 while(beacon_timer.read_us()%500000 < microseconds_offset){};
jah128 6:ff3c66f7372b 185 /*
jah128 6:ff3c66f7372b 186 out("Now:%d",beacon_timer.read_us());
jah128 6:ff3c66f7372b 187 Timer test_timer;
jah128 6:ff3c66f7372b 188 test_timer.start();
jah128 6:ff3c66f7372b 189 for(int i=0;i<50;i++){
jah128 6:ff3c66f7372b 190 store_background_raw_ir_values ();
jah128 6:ff3c66f7372b 191 out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2));
jah128 6:ff3c66f7372b 192 while(test_timer.read_ms() % 10 < 9){};
jah128 6:ff3c66f7372b 193 }
jah128 6:ff3c66f7372b 194 */
jah128 6:ff3c66f7372b 195 }else{
jah128 6:ff3c66f7372b 196 beacon_found = 0;
jah128 6:ff3c66f7372b 197 out("Beacon not found: a matching second block 500ms after first block not detected\n");
jah128 6:ff3c66f7372b 198 }
jah128 6:ff3c66f7372b 199 }else{
jah128 6:ff3c66f7372b 200 beacon_found = 0;
jah128 6:ff3c66f7372b 201 if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index);
jah128 6:ff3c66f7372b 202 if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length);
jah128 6:ff3c66f7372b 203 }
jah128 6:ff3c66f7372b 204 } else {
jah128 6:ff3c66f7372b 205 beacon_found = 0;
jah128 6:ff3c66f7372b 206 if(beacon_detected_count > 7) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count);
jah128 6:ff3c66f7372b 207 else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count);
jah128 6:ff3c66f7372b 208 }
jah128 6:ff3c66f7372b 209 wait(1);
jah128 6:ff3c66f7372b 210 }
jah128 6:ff3c66f7372b 211
jah128 6:ff3c66f7372b 212 // The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to
jah128 6:ff3c66f7372b 213 // It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot
jah128 6:ff3c66f7372b 214 // The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot
jah128 6:ff3c66f7372b 215 void start_infrared_timers()
jah128 6:ff3c66f7372b 216 {
jah128 6:ff3c66f7372b 217 // At this point we should be exactly at the start of a beacon cycle.
jah128 6:ff3c66f7372b 218 // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse)
jah128 6:ff3c66f7372b 219 // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step
jah128 6:ff3c66f7372b 220 out("3) Starting TDMA infrared timers\n");
jah128 6:ff3c66f7372b 221 beacon_debug_timer.start();
jah128 6:ff3c66f7372b 222 wait_us(4000); //Adjusted down for PC message
jah128 6:ff3c66f7372b 223 ir_emitter_ticker.attach_us(emitter_ticker_block,50000);
jah128 6:ff3c66f7372b 224 wait_us(20000); //Wait for middle of pulse
jah128 6:ff3c66f7372b 225 ir_sample_ticker.attach_us(sample_ticker_block,50000);
jah128 6:ff3c66f7372b 226 }
jah128 6:ff3c66f7372b 227
jah128 6:ff3c66f7372b 228
jah128 6:ff3c66f7372b 229 //Return the max value in IR array
jah128 6:ff3c66f7372b 230 unsigned short get_highest_sample(unsigned short * ir_array){
jah128 6:ff3c66f7372b 231 unsigned short highest = 0;
jah128 6:ff3c66f7372b 232 for(int i=0;i<8;i++){
jah128 6:ff3c66f7372b 233 if(ir_array[i]>highest) highest=ir_array[i];
jah128 6:ff3c66f7372b 234 }
jah128 6:ff3c66f7372b 235 return highest;
jah128 6:ff3c66f7372b 236 }
jah128 6:ff3c66f7372b 237
jah128 6:ff3c66f7372b 238 //Return the sum total of IR array
jah128 6:ff3c66f7372b 239 unsigned short get_sum_sample(unsigned short * ir_array){
jah128 6:ff3c66f7372b 240 unsigned short sum = 0;
jah128 6:ff3c66f7372b 241 for(int i=0;i<8;i++){
jah128 6:ff3c66f7372b 242 sum+=ir_array[i];
jah128 6:ff3c66f7372b 243 }
jah128 6:ff3c66f7372b 244 return sum;
jah128 6:ff3c66f7372b 245 }
jah128 6:ff3c66f7372b 246
jah128 6:ff3c66f7372b 247 //The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID
jah128 6:ff3c66f7372b 248 //It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters
jah128 6:ff3c66f7372b 249 void emitter_ticker_block(){
jah128 6:ff3c66f7372b 250 //If the time-step (-1) equals my ID, turn on my emitters for 40ms
jah128 6:ff3c66f7372b 251 if(pulse_step-1 == robot_id){
jah128 6:ff3c66f7372b 252 IF_set_IR_emitter_output(0, 1);
jah128 6:ff3c66f7372b 253 IF_set_IR_emitter_output(1, 1);
jah128 6:ff3c66f7372b 254 ir_emitter_timeout.attach_us(emitter_timeout_block,40000);
jah128 6:ff3c66f7372b 255 }
jah128 6:ff3c66f7372b 256 }
jah128 6:ff3c66f7372b 257
jah128 6:ff3c66f7372b 258 //Turn off the emitters
jah128 6:ff3c66f7372b 259 void emitter_timeout_block(){
jah128 6:ff3c66f7372b 260 //Turn off IR emitters
jah128 6:ff3c66f7372b 261 IF_set_IR_emitter_output(0, 0);
jah128 6:ff3c66f7372b 262 IF_set_IR_emitter_output(1, 0);
jah128 6:ff3c66f7372b 263 }
jah128 6:ff3c66f7372b 264
jah128 6:ff3c66f7372b 265 //The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot
jah128 6:ff3c66f7372b 266 //There are 10 time slots in each 500ms period
jah128 6:ff3c66f7372b 267 //Slot 0 is when the beacon is flashing
jah128 6:ff3c66f7372b 268 //Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[]
jah128 6:ff3c66f7372b 269 //Slot 2-8 are for the 7 robots; slot-1 = robot_id
jah128 6:ff3c66f7372b 270 //In slot 9, the robot processes the data [and doesn't store and new readings]
jah128 6:ff3c66f7372b 271 //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
jah128 6:ff3c66f7372b 272 void sample_ticker_block(){
jah128 6:ff3c66f7372b 273 //If we are in time-step 0 to 8, store the background data in an array
jah128 6:ff3c66f7372b 274 if(pulse_step < 9){
jah128 6:ff3c66f7372b 275 store_background_raw_ir_values ();
jah128 6:ff3c66f7372b 276 for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i);
jah128 6:ff3c66f7372b 277 }else{
jah128 6:ff3c66f7372b 278 //If not, process the data
jah128 6:ff3c66f7372b 279 for(int i=0;i<9;i++){
jah128 6:ff3c66f7372b 280 unsigned short sum = get_sum_sample(ir_sensor_data[i]);
jah128 6:ff3c66f7372b 281 unsigned short highest = get_highest_sample(ir_sensor_data[i]);
jah128 6:ff3c66f7372b 282 //Check if beacon is visible
jah128 6:ff3c66f7372b 283 if(i==0){
jah128 6:ff3c66f7372b 284 if(sum > beacon_threshold){
jah128 6:ff3c66f7372b 285 beacon_found = 1;
jah128 6:ff3c66f7372b 286 beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]);
jah128 6:ff3c66f7372b 287 }else beacon_found = 0;
jah128 6:ff3c66f7372b 288 //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]);
jah128 6:ff3c66f7372b 289 }
jah128 6:ff3c66f7372b 290 if(i==1){
jah128 6:ff3c66f7372b 291 for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j];
jah128 6:ff3c66f7372b 292 }
jah128 6:ff3c66f7372b 293 if(i>1){
jah128 6:ff3c66f7372b 294 char test_robot = i-1;
jah128 6:ff3c66f7372b 295 if(test_robot == robot_id){
jah128 6:ff3c66f7372b 296 for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j];
jah128 6:ff3c66f7372b 297 }else{
jah128 6:ff3c66f7372b 298 if(sum > low_threshold){
jah128 6:ff3c66f7372b 299 robots_found[test_robot] = 1;
jah128 6:ff3c66f7372b 300 robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]);
jah128 6:ff3c66f7372b 301 robots_distance[test_robot] = highest;
jah128 6:ff3c66f7372b 302 }else robots_found[test_robot] = 0;
jah128 6:ff3c66f7372b 303 }
jah128 6:ff3c66f7372b 304 }
jah128 6:ff3c66f7372b 305 }
jah128 6:ff3c66f7372b 306 display_ir_readings();
jah128 6:ff3c66f7372b 307 }
jah128 6:ff3c66f7372b 308 //Increment pulse step
jah128 6:ff3c66f7372b 309 pulse_step++;
jah128 6:ff3c66f7372b 310 if(pulse_step == 10) pulse_step = 0;
jah128 6:ff3c66f7372b 311 }
jah128 6:ff3c66f7372b 312
jah128 6:ff3c66f7372b 313
jah128 6:ff3c66f7372b 314 //Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles
jah128 6:ff3c66f7372b 315 void display_ir_readings()
jah128 6:ff3c66f7372b 316 {
jah128 6:ff3c66f7372b 317 out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms());
jah128 6:ff3c66f7372b 318 if(beacon_found==1){
jah128 6:ff3c66f7372b 319 out("Beacon detected at %d degrees\n",beacon_heading);
jah128 6:ff3c66f7372b 320 }
jah128 6:ff3c66f7372b 321 for(int j=1;j<8;j++){
jah128 6:ff3c66f7372b 322 if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]);
jah128 6:ff3c66f7372b 323 }
jah128 6:ff3c66f7372b 324 out("Reflected values:");
jah128 6:ff3c66f7372b 325 for(int i=0;i<8;i++){
jah128 6:ff3c66f7372b 326 out("[%d,%d]",i,reflected_sensor_data[i]);
jah128 6:ff3c66f7372b 327 }
jah128 6:ff3c66f7372b 328 out("\nBackground values:");
jah128 6:ff3c66f7372b 329 for(int i=0;i<8;i++){
jah128 6:ff3c66f7372b 330 out("[%d,%d]",i,background_sensor_data[i]);
jah128 6:ff3c66f7372b 331 }
jah128 6:ff3c66f7372b 332 out("\n\n");
jah128 6:ff3c66f7372b 333 }