![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: main.cpp
- Revision:
- 9:085e090e1ec1
- Parent:
- 8:00558287a4ef
- Child:
- 10:1b09d4bb847b
--- a/main.cpp Thu Oct 22 15:36:16 2015 +0000 +++ b/main.cpp Mon Oct 26 11:16:05 2015 +0000 @@ -14,13 +14,18 @@ /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis /// University of York + + /// Include psiswarm.h - this includes all the other necessary core files + #include "psiswarm.h" // IMPORTANT!!! // Do not call the IR functions at all as they will interfere with the correct operation of this program // Instead, use the values held in the variables below; they are updated every 500ms + + char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window int beacon_heading = 0; // This is the heading from the last time a beacon was detected char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window @@ -34,41 +39,123 @@ char * version_name = "1.0"; char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code] +char display_debug_inf = 1; // Set to 1 to show debug info on display +char main_program_state; +char old_program_state = 255; +char step_cycle = 0; +char success_count = 0; +char target_reached = 0; +char was_turning = 0; +Ticker main_loop_ticker; + + +///This is the main loop for the Beautiful Meme code. The code block is run once every 500mS* once all the IR samples have been collected. +void main_loop() +{ + update_display(); + switch(main_program_state) { + case 0: //Case 0 is the initial program: turn to face beacon + if(step_cycle == 0) { + char turn_status = turn_to_bearing(0); + if(turn_status == 0) { + success_count ++; + if(success_count > 1) main_program_state = 1; + } else success_count = 0; + } + break; + case 1: + target_reached = 0; + head_to_bearing_program(0); + if(target_reached == 1) main_program_state = 2; + break; + case 2: + target_reached = 0; + head_to_bearing_program(180); + if(target_reached == 1) main_program_state = 1; + break; + } + step_cycle=1-step_cycle; +} + + +//The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it +void head_to_bearing_program(int target_bearing) +{ + if(step_cycle == 0 || was_turning == 0) { + // Check if we are heading in the right bearing (+- 25 degrees) + int current_bearing = (360 - beacon_heading) % 360; + // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other + char bearing_ok = 0; + int lower_bound = target_bearing - 25; + int upper_bound = target_bearing + 25; + if(lower_bound < 0) { + if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1; + } else if(upper_bound > 359) { + if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1; + } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1; + // Check if there is an obstacle in front of us + if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1; + else { + // Now move forward if we are facing correct bearing, otherwise turn + if(bearing_ok == 1) { + //Check if anything is in front of us to determine speed - if it is, move slowly + int t_time = 6 * BEACON_PERIOD; + float t_speed = 1.0; + if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) { + t_time = 4 * BEACON_PERIOD; + t_speed = 0.6; + } + if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) { + t_time = 3 * BEACON_PERIOD; + t_speed = 0.4; + } + if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) { + t_time = 2 * BEACON_PERIOD; + t_speed = 0.2; + } + time_based_forward(t_speed,t_time,0); + was_turning = 0; + } else { + turn_to_bearing(target_bearing); + was_turning = 1; + } + } + } +} ///Place user code here that should be run after initialisation but before the main loop void user_code_setup() { - for(int i=0;i<10;i++){ - time_based_turn_degrees(1, 90); - wait(1); - } - + wait(0.8); + display.clear_display(); + display.set_position(0,0); + display.write_string("BEAUTIFUL MEME"); + display.set_position(1,0); + display.write_string(" PROJECT"); + wait(0.2); out("------------------------------------------------------\n"); out("Beautiful Meme Project Demo Code \n"); out("------------------------------------------------------\n"); locate_beacon(); while(beacon_found == 0) { + wait(0.5); locate_beacon(); } - start_infrared_timers(); + main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10); + main_program_state = 1; + set_leds(0x00,0x00); + set_center_led(3,1); + display.clear_display(); + display.set_position(0,0); + display.write_string("BEACON FOUND AT"); + display.set_position(1,0); + char degrees_string[16]; + sprintf(degrees_string,"%d DEGREES",beacon_heading); + display.write_string(degrees_string); } -///This function is the loop where user code should be placed -void user_code_loop() -{ - - ///Do not place code within a loop, but consider this function to be a loop that is always run - ///unless the user code is externally paused (such as by debug or recharging system) - - //pc.printf("User loop code block\n"); - - - wait(0.5); -} - - /// Code goes here to handle what should happen when the user switch is pressed void handle_switch_event(char switch_state) { @@ -86,12 +173,43 @@ user_code_setup(); user_code_running = 1; while(1) { - if(user_code_running)user_code_loop(); - if(demo_on) demo_mode(); - //wait_us(5); + wait(1); } } + +void update_display() +{ + if(main_program_state != old_program_state) { + old_program_state = main_program_state; + display.clear_display(); + display.set_position(0,0); + switch(main_program_state) { + case 0: + display.write_string("P:FACE BEACON"); + break; + case 1: + display.write_string("P:HEAD TO BEACON"); + break; + case 2: + display.write_string("P:HEAD TO SOUTH"); + break; + } + } + if(display_debug_inf==1) display_debug_info(); +} + +void display_debug_info() +{ + char disp_line[16] = "- - - - - - - -"; + if(beacon_found==1)disp_line[0]='B'; + for(int i=0; i<7; i++) { + if(robots_found[i])disp_line[((i+1)*2)]=49+i; + } + display.set_position(1,0); + display.write_string(disp_line); +} + /// Verbose output void out(const char* format, ...) {