YRL Maze lab made more script-y
Dependencies: PsiSwarmLab-ScriptingBased mbed
Fork of UKESF_Lab by
Diff: main.cpp
- Revision:
- 30:513457c1ad12
- Parent:
- 28:46d650381972
- Child:
- 31:7fa2c47d73a2
--- a/main.cpp Thu Mar 03 23:22:01 2016 +0000 +++ b/main.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -9,149 +9,32 @@ **(C) Dr James Hilder - York Robotics Laboratory - University of York ** ***********************************************************************/ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// PsiSwarm Blank Example Code +/// Version 0.41 +/// James Hilder, Alan Millard, Alexander Horsfield, 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 * program_name = "Blank"; char * author_name = "YRL"; -char * version_name = "0.4"; - -// 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 * version_name = "0.41"; -char default_normal_program = 8; // 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; -int flocking_headings[8]; // Beacon heading of each robot - -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() +void user_code_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) { - 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: - flocking_program(); - break; - case 255: - stop_program(); - break; - } - } - step_cycle=1-step_cycle; + wait(1); } ///Place user code here that should be run after initialisation but before the main loop void user_code_setup() { - wait(0.8); - //Do nothing forever: - while(1)wait(1.0); + wait(1); 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); + display.write_string("No Code"); + + //bmeme_user_code_setup(); } /// Code goes here to handle what should happen when the user switch is pressed @@ -159,38 +42,19 @@ { /// 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); + + //bmeme_handle_switch_event(switch_state); +} + +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 + + //bmeme_handle_user_serial_message(message, length, interface); } /// The main routine: it is recommended to leave this function alone and add user code to the above functions @@ -201,99 +65,6 @@ 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; - case 6: - strcpy(ret_name,"CLUSTERING"); - break; - case 7: - strcpy(ret_name,"TAG GAME"); - break; - case 8: - strcpy(ret_name,"FLOCKING"); - 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); + user_code_loop(); } } \ No newline at end of file