Example code for the UKESF Headstart robotics lab; code to make the PsiSwarm robot move forward and turn.
Dependencies: PsiSwarm-Headstart mbed
Fork of PsiSwarm_V41 by
Diff: BeautifulMeme/beacon.cpp
- Revision:
- 31:7fa2c47d73a2
- Parent:
- 30:513457c1ad12
--- a/BeautifulMeme/beacon.cpp Tue Mar 15 00:58:43 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,376 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.41 -/// 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 "bmeme.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?] - -char show_ir_debug_info = 0; // Set to 1 to display (via PC) the list of IR readings & visible robots every timestep - -/// 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. -/// 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() -{ - int sample_period = (BEACON_PERIOD * 2) / 5; - 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 (); - if(i%2 == 0){ - set_center_led(1, 0.5); - set_leds(0xAA,0x55); - }else{ - set_center_led(2, 0.5); - set_leds(0x55,0xAA); - } - 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+=sample_period; - 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 = (sample_period * mid_point) - sample_period - (sample_period / 4); - //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us()); - int cycle_period = (BEACON_PERIOD * 10); - if(microseconds_offset < 0) microseconds_offset += cycle_period; - //If we have missed the start of the beacon this cycle, wait until the next cycle - while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){}; - //Now wait until the start of the beacon pulse - while(beacon_timer.read_us()% (cycle_period) < 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 %dms after first block not detected\n",(BEACON_PERIOD / 100)); - } - }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 > 6) 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); - } - if(beacon_found == 0){ - set_leds(0x00,0x00); - set_center_led(1, 1); - display.clear_display(); - display.set_position(0,0); - display.write_string("BEACON NOT FOUND"); - } -} - -// 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(BEACON_PERIOD / 10); - ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD); - wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse - ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD); -} - - -//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 && disable_ir_emitters == 0){ - IF_set_IR_emitter_output(0, 1); - IF_set_IR_emitter_output(1, 1); - ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10); - } -} - -//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; - //Debug-- - //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]); - robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]); - robots_distance[test_robot] = highest; - }else robots_found[test_robot] = 0; - } - } - } - if(show_ir_debug_info == 1)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"); -} - -//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 -char turn_to_bearing(int bearing) -{ - if(beacon_found == 0){ - out("Beacon not found: cannot turn to specific bearing\n"); - return 2; - }else{ - //First calculate the bearing using the angle of beacon relative to robot - int current_bearing = 360 - beacon_heading; - //Now work out turn needed to face intended heading - int target_turn = (bearing - current_bearing) % 360; - //Adjust to take 10% off turn, stops overshoot - target_turn = (target_turn * 9) / 10; - if(target_turn > 180) target_turn -= 360; - if(target_turn < -180) target_turn += 360; - //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that - char beyond_limit = 0; - int turn_limit = BEACON_PERIOD / 358; - if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;}; - if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;}; - out("Turning %d degrees\n",target_turn); - time_based_turn_degrees(1, target_turn,1); - return beyond_limit; - } -} \ No newline at end of file