![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
main.cpp
- Committer:
- jah128
- Date:
- 2015-10-26
- Revision:
- 10:1b09d4bb847b
- Parent:
- 9:085e090e1ec1
- Child:
- 11:7b3ee540ba56
File content as of revision 10:1b09d4bb847b:
/*********************************************************************** ** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ ** ** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ ** ** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ ** ** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ ** ** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ ** ** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ ** ************************************************************************ **(C) Dr James Hilder - York Robotics Laboratory - University of York ** ***********************************************************************/ /// PsiSwarm Beautiful Meme Project Source Code /// 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" 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 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 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 = 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 250mS* [with 4Hz beacon] once all the IR samples have been collected. void main_loop() { if(use_recharging_program == 1)recharging_program(); update_display(); 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; } ///Place user code here that should be run after initialisation but before the main loop void user_code_setup() { 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); set_program(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); } /// 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 //pc.printf("User switch code block: %d\n",switch_state); } /// 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(); user_code_running = 1; while(1) { 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,"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(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); } }