![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: beacon.cpp
- Revision:
- 9:085e090e1ec1
- Parent:
- 7:ef9ab01b9e26
- Child:
- 10:1b09d4bb847b
diff -r 00558287a4ef -r 085e090e1ec1 beacon.cpp --- a/beacon.cpp Thu Oct 22 15:36:16 2015 +0000 +++ b/beacon.cpp Mon Oct 26 11:16:05 2015 +0000 @@ -6,6 +6,7 @@ // Beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots #include "psiswarm.h" +#include "main.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 @@ -18,11 +19,12 @@ -/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in 20ms blocks. +/// 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; @@ -31,12 +33,19 @@ //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+=20000; + offset+=sample_period; while(beacon_timer.read_us() < offset) {} } @@ -93,13 +102,13 @@ } //Count and display matches int beacon_detected_count = 0; - char output_string[251] = ""; + //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); + // 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); @@ -175,13 +184,14 @@ //} out("2) Synchronising...\n"); // Calculate the offset to the expected start of the next beacon pulse - int microseconds_offset = (20000 * mid_point) - 25000; + 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()); - if(microseconds_offset < 0) microseconds_offset += 500000; + 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()%500000 > microseconds_offset){}; + while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){}; //Now wait until the start of the beacon pulse - while(beacon_timer.read_us()%500000 < microseconds_offset){}; + while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){}; /* out("Now:%d",beacon_timer.read_us()); Timer test_timer; @@ -194,7 +204,7 @@ */ }else{ beacon_found = 0; - out("Beacon not found: a matching second block 500ms after first block not detected\n"); + out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100)); } }else{ beacon_found = 0; @@ -206,7 +216,13 @@ 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); } - wait(1); + 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 @@ -219,10 +235,10 @@ // 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); + 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); } @@ -251,7 +267,7 @@ 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); + ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10); } } @@ -332,4 +348,28 @@ 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"); + 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; + 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