![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
beacon.cpp
- Committer:
- jah128
- Date:
- 2015-10-22
- Revision:
- 6:ff3c66f7372b
- Child:
- 7:ef9ab01b9e26
File content as of revision 6:ff3c66f7372b:
/// PsiSwarm Beautiful Meme Project Source Code /// Version 0.1 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis /// University of York // Beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots #include "psiswarm.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 int beacon_threshold; //Set to be 4x mean background IR unsigned short ir_sensor_data[9][8]; // The raw sensor data from all 9x 50ms sample windows Ticker ir_sample_ticker; // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot Ticker ir_emitter_ticker; // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot Timeout ir_emitter_timeout; // Timeout for turning off the IR emitters after 40ms Timer beacon_debug_timer; // Timer for debug information only [remove later?] /// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in 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() { out("1) Searching for IR beacon..."); unsigned short samples[50][9]; Timer beacon_timer; beacon_timer.start(); int offset = 0; //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 (); 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; while(beacon_timer.read_us() < offset) {} } //Print values: for testing [comment out] /* for(int i=0; i<50; i++) { out("IR %d:",i); for(int j=0; j<8; j++) { out("[%d:%d]",j,samples[i][j]); } out(" [SUM:%d]\n",samples[i][8]); } */ //Bubble sort sums to find (6) highest values unsigned short sorted_array[50]; for(int i=0; i<50; i++) { sorted_array[i]=samples[i][8]; } for (int c = 0 ; c < 49; c++) { for (int d = 0 ; d < (50-c-1); d++) { if (sorted_array[d] > sorted_array[d+1]) { unsigned short swap = sorted_array[d]; sorted_array[d] = sorted_array[d+1]; sorted_array[d+1] = swap; } } } //Print sorted values: for testing [comment out] /* out("Sorted values:"); for (int c = 0 ; c < 50 ; c++ ) { out("%d", sorted_array[c]); if(c<49)out(","); } out("\n"); */ // Calculate mean background sum value by looking at 44 lowest sum values int background_mean = 0; for(int i=0;i<44;i++)background_mean += sorted_array[i]; background_mean /= 44; //out("Background mean value: %d\n",background_mean); //Our beacon threshold will be 4x the background mean value; find all instances where this occurs low_threshold = background_mean * 2; beacon_threshold = background_mean * 4; char beacon_detected_indices[50]; for(int i=0;i<50;i++){ if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1; else beacon_detected_indices[i]=0; } //Count and display matches int beacon_detected_count = 0; 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); } } //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string); //We will use this array to store average values for each sensor when the beacon is detected unsigned short beacon_averages[8]; char beacon_averages_count = 0; for(int i=0;i<8;i++)beacon_averages[i]=0; //Now determine if the beacon is correctly found: must adhere to a set of rules //Firstly, we should have not less than 4 and not more than 6 positive matches if(beacon_detected_count>3 && beacon_detected_count<7){ // Now verify that the positive samples are in valid places... // Find first positive sample int first_index = 0; //out("Here\n",first_index); while(beacon_detected_indices[first_index]==0)first_index ++; //out("First index:%d\n",first_index); // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high if(first_index == 0){ if(beacon_detected_indices[49]>0)first_index = 49; if(beacon_detected_indices[48]>0)first_index = 48; } beacon_averages_count++; for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];} // Now count the length of the 'block' of positive hits: must be equal to 2 or 3 char block_length = 1; int end_index = first_index + 1; if(end_index == 50) end_index = 0; while(beacon_detected_indices[end_index]>0){ beacon_averages_count++; for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];} block_length ++; end_index ++; if(end_index == 50) end_index = 0; } if(block_length==2 || block_length == 3){ //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 float mid_point; char second_block_okay = 0; if(block_length == 2){ mid_point = first_index + 0.5; char second_block_low = first_index + 25; char second_block_high = first_index + 26; if(second_block_low > 49) second_block_low -= 50; if(second_block_high > 49) second_block_high -= 50; beacon_averages_count+=2; for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];} if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1; } if(block_length == 3){ mid_point = first_index + 1; if(mid_point == 50) mid_point = 0; char second_block_single = first_index + 26; if(second_block_single > 49) second_block_single -= 50; beacon_averages_count++; for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];} if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1; } if(second_block_okay >0){ beacon_found = 1; beacon_heading = get_bearing_from_ir_array(beacon_averages); out("Found at %d degrees\n",beacon_heading); //for(int i=0;i<8;i++){ // beacon_averages[i] /= beacon_averages_count; // out("[%d]",beacon_averages[i]); //} out("2) Synchronising...\n"); // Calculate the offset to the expected start of the next beacon pulse int microseconds_offset = (20000 * mid_point) - 25000; //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; //If we have missed the start of the beacon this cycle, wait until the next cycle while(beacon_timer.read_us()%500000 > microseconds_offset){}; //Now wait until the start of the beacon pulse while(beacon_timer.read_us()%500000 < microseconds_offset){}; /* out("Now:%d",beacon_timer.read_us()); Timer test_timer; test_timer.start(); for(int i=0;i<50;i++){ store_background_raw_ir_values (); out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2)); while(test_timer.read_ms() % 10 < 9){}; } */ }else{ beacon_found = 0; out("Beacon not found: a matching second block 500ms after first block not detected\n"); } }else{ beacon_found = 0; if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index); if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length); } } else { beacon_found = 0; if(beacon_detected_count > 7) 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); } // The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to // It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot // The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot void start_infrared_timers() { // At this point we should be exactly at the start of a beacon cycle. // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse) // 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); } //Return the max value in IR array unsigned short get_highest_sample(unsigned short * ir_array){ unsigned short highest = 0; for(int i=0;i<8;i++){ if(ir_array[i]>highest) highest=ir_array[i]; } return highest; } //Return the sum total of IR array unsigned short get_sum_sample(unsigned short * ir_array){ unsigned short sum = 0; for(int i=0;i<8;i++){ sum+=ir_array[i]; } return sum; } //The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID //It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters void emitter_ticker_block(){ //If the time-step (-1) equals my ID, turn on my emitters for 40ms 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); } } //Turn off the emitters void emitter_timeout_block(){ //Turn off IR emitters IF_set_IR_emitter_output(0, 0); IF_set_IR_emitter_output(1, 0); } //The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot //There are 10 time slots in each 500ms period //Slot 0 is when the beacon is flashing //Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[] //Slot 2-8 are for the 7 robots; slot-1 = robot_id //In slot 9, the robot processes the data [and doesn't store and new readings] //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 void sample_ticker_block(){ //If we are in time-step 0 to 8, store the background data in an array if(pulse_step < 9){ store_background_raw_ir_values (); for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i); }else{ //If not, process the data for(int i=0;i<9;i++){ unsigned short sum = get_sum_sample(ir_sensor_data[i]); unsigned short highest = get_highest_sample(ir_sensor_data[i]); //Check if beacon is visible if(i==0){ if(sum > beacon_threshold){ beacon_found = 1; beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]); }else beacon_found = 0; //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]); } if(i==1){ for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j]; } if(i>1){ char test_robot = i-1; if(test_robot == robot_id){ for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j]; }else{ if(sum > low_threshold){ robots_found[test_robot] = 1; robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]); robots_distance[test_robot] = highest; }else robots_found[test_robot] = 0; } } } display_ir_readings(); } //Increment pulse step pulse_step++; if(pulse_step == 10) pulse_step = 0; } //Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles void display_ir_readings() { out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms()); if(beacon_found==1){ out("Beacon detected at %d degrees\n",beacon_heading); } for(int j=1;j<8;j++){ if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]); } out("Reflected values:"); for(int i=0;i<8;i++){ out("[%d,%d]",i,reflected_sensor_data[i]); } out("\nBackground values:"); for(int i=0;i<8;i++){ out("[%d,%d]",i,background_sensor_data[i]); } out("\n\n"); }