James Hilder / Mbed 2 deprecated PsiSwarm_V41

Psi Swarm Code V0.41 [With Beautiful Meme program]

Dependencies:   PsiSwarmLibrary mbed

Fork of BeautifulMemeProjectBT by Alan Millard

BeautifulMeme/beacon.cpp@30:513457c1ad12, 2016-03-15 (annotated)

Committer:
jah128
Date:
Tue Mar 15 00:58:43 2016 +0000
Revision:
30:513457c1ad12
Added serial handling for Psi Console

Who changed what in which revision?

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