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