![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
main.cpp
- Committer:
- jhok500
- Date:
- 2017-03-02
- Revision:
- 20:fe7eb85cf3a1
- Parent:
- 19:b5788427db67
- Child:
- 21:efe191c96cbb
File content as of revision 20:fe7eb85cf3a1:
/*********************************************************************** ** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ ** ** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ ** ** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ ** ** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ ** ** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ ** ** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ ** ************************************************************************ **(C) Dr James Hilder - York Robotics Laboratory - University of York ** ***********************************************************************/ /// PsiSwarm Beautiful Meme Project Source Code /// Version 0.21 /// 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" char * program_name = "B-Meme"; char * author_name = "YRL"; char * version_name = "0.21"; // 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 int robots_heading[8]; // These are the headings from the last time the respective robots were detected unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected 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 toggle_program_stage = 0; char default_normal_program = 5; // The program to run on turn on (after 'face beacon' program) char use_recharging_program = 0; // 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 = 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 previous_program; // Used to hold previous running program when it is paused for switch press etc char program_changed = 0; // Flag to update display when program is changed char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary 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) char switch_held = 0; // Used for detected when the cursor switch is held to override program choice char choose_program_mode = 0; char program_count = 8; char program_selection; Ticker toggle_program_ticker; float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55 float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0 Ticker main_loop_ticker; ///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(switch_held == 1)switch_held=2; if(switch_held == 3 && choose_program_mode == 0) { //The switch has been held right and then released: stop the current program previous_program = main_program_state; program_selection = previous_program; choose_program_mode = 1; set_program(255); set_program_info(get_program_name(program_selection)); } if(use_recharging_program == 1)recharging_program(); update_display(); if(recharging_state == 0) { // remove after testing //main_program_state = 8; 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(1); break; case 6: clustering_program(0,1); break; case 7: tag_game_program(); break; case 8: mockFlocking(); break; case 254: init(); break; case 255: stop_program(); break; } } step_cycle=1-step_cycle; } ///Place user code here that should be run after initialisation but before the main loop void user_code_setup() { // Note for Alex: // // This is the code we have been working on for the beautiful meme project // // If you were to comment out (or delete...) all the code here, the robot will just // sit in an 'idle' loop when you turn it on - but should respond to any command codes // that are sent. 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(); } //toggle_program_ticker.attach(toggle_program,20); start_infrared_timers(); main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10); set_program(4);//for debug ...was 0 ---------- set_leds(0x00,0x00); set_center_led(3,0.5); 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); } void toggle_program(){ toggle_program_stage ++; if(toggle_program_stage == 3) toggle_program_stage = 0; switch(toggle_program_stage){ case 0: set_program(5); break; case 1: set_program(6); break; case 2: set_program(7);break; case 3: set_program(7); break; } } /// Code goes here to handle what should happen when the user switch is pressed void handle_switch_event(char switch_state) { /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed /// NB For maximum compatability it is recommended to minimise reliance on center button press if(choose_program_mode == 0) { if(switch_state == 8) switch_held = 1; else if(switch_state == 0 && switch_held == 2) switch_held = 3; else switch_held = 0; } else { // We are in choose program mode if(switch_state == 8) { program_selection ++; if(program_selection > program_count) program_selection = 0; if(program_selection == program_count) set_program_info("RECHARGE"); else set_program_info(get_program_name(program_selection)); } if(switch_state == 4) { if(program_selection == 0) program_selection = program_count; else program_selection --; if(program_selection == program_count) set_program_info("RECHARGE"); else set_program_info(get_program_name(program_selection)); } if(switch_state == 1 || switch_state == 2){ if(program_selection == program_count){ recharging_state = 1; set_program(previous_program); strcpy(prog_name,"CHARGING PROGRAM"); set_program_info("HEAD TO BEACON"); } else set_program(program_selection); choose_program_mode = 0; switch_held = 0; } } //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info); } void handle_user_serial_message(char * message, char length, char interface) { // This is where user code for handling a (non-system) serial message should go // // message = pointer to message char array // length = length of message // interface = 0 for PC serial connection, 1 for Bluetooth char buffer[255]; sprintf(buffer,message,length); for(int i=0; i<length; i++) { buffer[i]=message[i]; } buffer[length]=0; if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length); else debug("Received USB message:%s [%d chars]\n",buffer,length); display.clear_display(); display.set_position(0,0); display.write_string(buffer); //"A" = 0x41 if(buffer[0] == 'A'){ /* display.clear_display(); display.set_position(0,0); display.write_string("Gotcha!");*/ //set_program(255); // STOP main_program_state = 255; } //"B" = 0x42 else if(buffer[0] == 'B'){ //set_program(1); // Head to beacon main_program_state = 1; } //"C" = 0x43 else if(buffer[0] == 'C'){ //set_program(2); // Head to beacon + 180 main_program_state = 2; } //"D" = 0x44 else if(buffer[0] == 'D'){ //set_program(8); // Flocking main_program_state = 8; } else if(buffer[0] == 'E'){ //set_program(4); // Random Walk main_program_state = 4; } else if(buffer[0] == 'F'){ //set_program(6); // Aggregation main_program_state = 6; } else if(buffer[0] == 'G'){ //set_program(5); // Find Space main_program_state = 5; } else if(buffer[0] == 'H'){ //set_program(6); // Clustering main_program_state = 6; } else if(buffer[0] == 'I'){ //set_program(7); // Tag main_program_state = 7; } else { } } /// The main routine: it is recommended to leave this function alone and add user code to the above functions int main() { ///init() in psiswarm.cpp sets up the robot init(); user_code_setup(); //char prog_index; //char * p_index = &prog_index; user_code_running = 1; while(1) { /* prog_index = pc.getc(); switch(prog_index){ case 0x61: display.clear_display(); display.set_position(0,0); display.write_string("Facing Beacon"); break; default: display.clear_display(); display.set_position(0,0); display.write_string(p_index); pc.putc(prog_index); break; }*/ wait(1); } } 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,"WAITING COMMAND");///gus, was random walk2 break; case 5: strcpy(ret_name,"FIND SPACE"); break; case 6: strcpy(ret_name,"CLUSTERING"); break; case 7: strcpy(ret_name,"TAG GAME"); break; case 255: strcpy(ret_name,"PROGRAM:"); break; } return ret_name; } void set_program(int index) { main_program_state = index; program_changed = 1; program_run_init = 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(program_changed == 1) { program_changed = 0; display.clear_display(); 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); } } void display_debug_info() { char disp_line[16] = "- - - - - - - -"; if(beacon_found==1)disp_line[0]='B'; for(int i=1; i<8; i++) { if(robots_found[i])disp_line[((i)*2)]=48+i; } display.set_position(0,0); display.write_string(disp_line); } /// Verbose output void out(const char* format, ...) { char buffer[256]; if (debug_mode) { va_list vl; va_start(vl, format); vsprintf(buffer,format,vl); if(user_code_debug == 1) pc.printf("%s", buffer); va_end(vl); } }