![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: main.cpp
- Revision:
- 10:1b09d4bb847b
- Parent:
- 9:085e090e1ec1
- Child:
- 11:7b3ee540ba56
--- a/main.cpp Mon Oct 26 11:16:05 2015 +0000 +++ b/main.cpp Mon Oct 26 23:58:08 2015 +0000 @@ -10,22 +10,21 @@ ***********************************************************************/ /// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.1 +/// Version 0.2 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis /// University of York - +/// Include main.h - this includes psiswarm.h all the other necessary core files +#include "main.h" -/// Include psiswarm.h - this includes all the other necessary core files - -#include "psiswarm.h" +char * program_name = "B-Meme"; +char * author_name = "YRL"; +char * version_name = "0.2"; // 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,96 +33,61 @@ unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on -char * program_name = "B-Meme"; -char * author_name = "YRL"; -char * version_name = "1.0"; - +char default_normal_program = 3; // The program to run on turn on (after 'face beacon' program) +char use_recharging_program = 1; // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold 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; +char display_debug_inf = 0; // Set to 1 to show debug info about beacon\robots on display [instead of running program info] +char main_program_state; // Index of the currently running program +char program_changed = 0; // Flag to update display when program is changed +char success_count = 0; // Flag to indicate the success of a program +char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps +char target_reached = 0; // Flag to indicate if a program target has been reached +char prog_name [17]; // Stores the name of the running program [line 0 on the display] +char prog_info [17]; // Stores information about the current state of the program [line 1 on the display] +char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!] +char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running) 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. +///This is the main loop for the Beautiful Meme code. The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected. void main_loop() { + if(use_recharging_program == 1)recharging_program(); 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; + if(recharging_state == 0) { + 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) set_program(default_normal_program); + } else success_count = 0; + } + break; + case 1: + target_reached = 0; + head_to_bearing_program(0); + if(target_reached == 1) set_program(2); + break; + case 2: + target_reached = 0; + head_to_bearing_program(180); + if(target_reached == 1) set_program(1); + break; + case 3: + curved_random_walk_with_interaction_program(); + break; + case 4: + straight_random_walk_with_interaction_program(); + break; + case 5: + find_space_program(); + 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() { @@ -144,9 +108,9 @@ } start_infrared_timers(); main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10); - main_program_state = 1; + set_program(0); set_leds(0x00,0x00); - set_center_led(3,1); + set_center_led(3,0.5); display.clear_display(); display.set_position(0,0); display.write_string("BEACON FOUND AT"); @@ -177,36 +141,70 @@ } } +char * get_program_name(int index) +{ + char * ret_name = new char[17]; + switch(index){ + case 0: + strcpy(ret_name,"FACE BEACON"); + break; + case 1: + strcpy(ret_name,"HEAD TO BEACON"); + break; + case 2: + strcpy(ret_name,"HEAD TO SOUTH"); + break; + case 3: + strcpy(ret_name,"RANDOM WALK 1"); + break; + case 4: + strcpy(ret_name,"RANDOM WALK 2"); + break; + case 5: + strcpy(ret_name,"FIND SPACE"); + break; + } + return ret_name; +} + +void set_program(int index) +{ + main_program_state = index; + program_changed = 1; + strcpy(prog_info,""); + strcpy(prog_name,get_program_name(index)); +} + +void set_program_info(char * info) +{ + strcpy(prog_info,info); + program_changed = 1; +} void update_display() { - if(main_program_state != old_program_state) { - old_program_state = main_program_state; + if(program_changed == 1) { + program_changed = 0; 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(); + else{ + display.set_position(0,0); + display.write_string(prog_name); } + display.set_position(1,0); + display.write_string(prog_info); } - 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; + for(int i=1; i<8; i++) { + if(robots_found[i])disp_line[((i)*2)]=48+i; } - display.set_position(1,0); + display.set_position(0,0); display.write_string(disp_line); }