James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

main.cpp@20:fe7eb85cf3a1, 2017-03-02 (annotated)

Committer:
jhok500
Date:
Thu Mar 02 13:54:12 2017 +0000
Revision:
20:fe7eb85cf3a1
Parent:
19:b5788427db67
Child:
21:efe191c96cbb
buttoncodeDev

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 6:ff3c66f7372b 1 /***********************************************************************
jah128 0:8a5497a2e366 2 ** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ **
jah128 0:8a5497a2e366 3 ** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ **
jah128 0:8a5497a2e366 4 ** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ **
jah128 0:8a5497a2e366 5 ** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ **
jah128 0:8a5497a2e366 6 ** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ **
jah128 0:8a5497a2e366 7 ** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ **
jah128 0:8a5497a2e366 8 ************************************************************************
jah128 0:8a5497a2e366 9 **(C) Dr James Hilder - York Robotics Laboratory - University of York **
jah128 0:8a5497a2e366 10 ***********************************************************************/
jah128 0:8a5497a2e366 11
jah128 6:ff3c66f7372b 12 /// PsiSwarm Beautiful Meme Project Source Code
jah128 16:976a1d0ea897 13 /// Version 0.21
jah128 6:ff3c66f7372b 14 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 6:ff3c66f7372b 15 /// University of York
jah128 0:8a5497a2e366 16
jah128 10:1b09d4bb847b 17 /// Include main.h - this includes psiswarm.h all the other necessary core files
jah128 10:1b09d4bb847b 18 #include "main.h"
jah128 9:085e090e1ec1 19
jah128 10:1b09d4bb847b 20 char * program_name = "B-Meme";
jah128 10:1b09d4bb847b 21 char * author_name = "YRL";
jah128 16:976a1d0ea897 22 char * version_name = "0.21";
jah128 0:8a5497a2e366 23
jah128 6:ff3c66f7372b 24 // IMPORTANT!!!
jah128 6:ff3c66f7372b 25 // Do not call the IR functions at all as they will interfere with the correct operation of this program
jah128 6:ff3c66f7372b 26 // Instead, use the values held in the variables below; they are updated every 500ms
jah128 6:ff3c66f7372b 27
jah128 6:ff3c66f7372b 28 char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window
jah128 6:ff3c66f7372b 29 int beacon_heading = 0; // This is the heading from the last time a beacon was detected
jah128 6:ff3c66f7372b 30 char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
jah128 7:ef9ab01b9e26 31 int robots_heading[8]; // These are the headings from the last time the respective robots were detected
jah128 6:ff3c66f7372b 32 unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected
jah128 6:ff3c66f7372b 33 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
jah128 6:ff3c66f7372b 34 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
jah128 6:ff3c66f7372b 35
jah128 17:f26a5805cde3 36
jah128 17:f26a5805cde3 37 char toggle_program_stage = 0;
jah128 17:f26a5805cde3 38 char default_normal_program = 5; // The program to run on turn on (after 'face beacon' program)
jah128 17:f26a5805cde3 39 char use_recharging_program = 0; // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
jah128 6:ff3c66f7372b 40 char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code]
jah128 10:1b09d4bb847b 41 char display_debug_inf = 0; // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
jah128 10:1b09d4bb847b 42 char main_program_state; // Index of the currently running program
jah128 13:f5994956b1ba 43 char previous_program; // Used to hold previous running program when it is paused for switch press etc
jah128 10:1b09d4bb847b 44 char program_changed = 0; // Flag to update display when program is changed
jah128 12:daa53285b6e4 45 char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary
jah128 10:1b09d4bb847b 46 char success_count = 0; // Flag to indicate the success of a program
jah128 10:1b09d4bb847b 47 char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps
jah128 10:1b09d4bb847b 48 char target_reached = 0; // Flag to indicate if a program target has been reached
jah128 10:1b09d4bb847b 49 char prog_name [17]; // Stores the name of the running program [line 0 on the display]
jah128 10:1b09d4bb847b 50 char prog_info [17]; // Stores information about the current state of the program [line 1 on the display]
jah128 10:1b09d4bb847b 51 char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!]
jah128 10:1b09d4bb847b 52 char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running)
jah128 13:f5994956b1ba 53 char switch_held = 0; // Used for detected when the cursor switch is held to override program choice
jah128 13:f5994956b1ba 54 char choose_program_mode = 0;
jah128 16:976a1d0ea897 55 char program_count = 8;
jah128 13:f5994956b1ba 56 char program_selection;
jah128 17:f26a5805cde3 57 Ticker toggle_program_ticker;
jah128 11:7b3ee540ba56 58
jah128 14:f623db1e6184 59 float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55
jah128 14:f623db1e6184 60 float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
jah128 11:7b3ee540ba56 61
jah128 9:085e090e1ec1 62 Ticker main_loop_ticker;
jah128 9:085e090e1ec1 63
JamesSW 19:b5788427db67 64
jah128 10:1b09d4bb847b 65 ///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.
jah128 9:085e090e1ec1 66 void main_loop()
jah128 9:085e090e1ec1 67 {
jah128 13:f5994956b1ba 68 if(switch_held == 1)switch_held=2;
jah128 14:f623db1e6184 69 if(switch_held == 3 && choose_program_mode == 0) {
jah128 13:f5994956b1ba 70 //The switch has been held right and then released: stop the current program
jah128 13:f5994956b1ba 71 previous_program = main_program_state;
jah128 13:f5994956b1ba 72 program_selection = previous_program;
jah128 13:f5994956b1ba 73 choose_program_mode = 1;
jah128 13:f5994956b1ba 74 set_program(255);
jah128 14:f623db1e6184 75 set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 76 }
jah128 10:1b09d4bb847b 77 if(use_recharging_program == 1)recharging_program();
jah128 9:085e090e1ec1 78 update_display();
jah128 10:1b09d4bb847b 79 if(recharging_state == 0) {
JamesSW 19:b5788427db67 80 // remove after testing
jhok500 20:fe7eb85cf3a1 81 //main_program_state = 8;
jah128 10:1b09d4bb847b 82 switch(main_program_state) {
jah128 10:1b09d4bb847b 83 case 0: //Case 0 is the initial program: turn to face beacon
jah128 10:1b09d4bb847b 84 if(step_cycle == 0) {
jah128 10:1b09d4bb847b 85 char turn_status = turn_to_bearing(0);
jah128 10:1b09d4bb847b 86 if(turn_status == 0) {
jah128 10:1b09d4bb847b 87 success_count ++;
jah128 10:1b09d4bb847b 88 if(success_count > 1) set_program(default_normal_program);
jah128 10:1b09d4bb847b 89 } else success_count = 0;
jah128 10:1b09d4bb847b 90 }
jah128 10:1b09d4bb847b 91 break;
jah128 10:1b09d4bb847b 92 case 1:
jah128 10:1b09d4bb847b 93 target_reached = 0;
jah128 10:1b09d4bb847b 94 head_to_bearing_program(0);
jah128 10:1b09d4bb847b 95 if(target_reached == 1) set_program(2);
jah128 10:1b09d4bb847b 96 break;
jah128 10:1b09d4bb847b 97 case 2:
jah128 10:1b09d4bb847b 98 target_reached = 0;
jah128 10:1b09d4bb847b 99 head_to_bearing_program(180);
jah128 10:1b09d4bb847b 100 if(target_reached == 1) set_program(1);
jah128 10:1b09d4bb847b 101 break;
jah128 10:1b09d4bb847b 102 case 3:
jah128 10:1b09d4bb847b 103 curved_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 104 break;
jah128 10:1b09d4bb847b 105 case 4:
jah128 10:1b09d4bb847b 106 straight_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 107 break;
jah128 10:1b09d4bb847b 108 case 5:
jah128 13:f5994956b1ba 109 find_space_program(1);
jah128 10:1b09d4bb847b 110 break;
jah128 12:daa53285b6e4 111 case 6:
jah128 13:f5994956b1ba 112 clustering_program(0,1);
jah128 12:daa53285b6e4 113 break;
jah128 16:976a1d0ea897 114 case 7:
jah128 16:976a1d0ea897 115 tag_game_program();
jah128 16:976a1d0ea897 116 break;
JamesSW 19:b5788427db67 117 case 8:
JamesSW 19:b5788427db67 118 mockFlocking();
JamesSW 19:b5788427db67 119 break;
JamesSW 19:b5788427db67 120 case 254:
JamesSW 19:b5788427db67 121 init();
JamesSW 19:b5788427db67 122 break;
jah128 13:f5994956b1ba 123 case 255:
jah128 13:f5994956b1ba 124 stop_program();
jah128 14:f623db1e6184 125 break;
jah128 10:1b09d4bb847b 126 }
jah128 9:085e090e1ec1 127 }
jah128 9:085e090e1ec1 128 step_cycle=1-step_cycle;
jah128 9:085e090e1ec1 129 }
jah128 9:085e090e1ec1 130
jah128 1:f6356cf1cefc 131 ///Place user code here that should be run after initialisation but before the main loop
jah128 6:ff3c66f7372b 132 void user_code_setup()
jah128 13:f5994956b1ba 133 {
jah128 17:f26a5805cde3 134 // Note for Alex:
jah128 17:f26a5805cde3 135 //
jah128 17:f26a5805cde3 136 // This is the code we have been working on for the beautiful meme project
jah128 17:f26a5805cde3 137 //
jah128 17:f26a5805cde3 138 // If you were to comment out (or delete...) all the code here, the robot will just
jah128 17:f26a5805cde3 139 // sit in an 'idle' loop when you turn it on - but should respond to any command codes
jah128 17:f26a5805cde3 140 // that are sent.
jah128 17:f26a5805cde3 141
jah128 9:085e090e1ec1 142 wait(0.8);
jah128 9:085e090e1ec1 143 display.clear_display();
jah128 9:085e090e1ec1 144 display.set_position(0,0);
jah128 9:085e090e1ec1 145 display.write_string("BEAUTIFUL MEME");
jah128 9:085e090e1ec1 146 display.set_position(1,0);
jah128 9:085e090e1ec1 147 display.write_string(" PROJECT");
jah128 9:085e090e1ec1 148 wait(0.2);
jah128 6:ff3c66f7372b 149 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 150 out("Beautiful Meme Project Demo Code \n");
jah128 6:ff3c66f7372b 151 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 152 locate_beacon();
jah128 6:ff3c66f7372b 153 while(beacon_found == 0) {
jah128 9:085e090e1ec1 154 wait(0.5);
jah128 6:ff3c66f7372b 155 locate_beacon();
GusZ92 18:3fa5cd53ed45 156 }
GusZ92 18:3fa5cd53ed45 157 //toggle_program_ticker.attach(toggle_program,20);
jah128 6:ff3c66f7372b 158 start_infrared_timers();
jah128 9:085e090e1ec1 159 main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
GusZ92 18:3fa5cd53ed45 160 set_program(4);//for debug ...was 0 ----------
jah128 9:085e090e1ec1 161 set_leds(0x00,0x00);
jah128 10:1b09d4bb847b 162 set_center_led(3,0.5);
jah128 9:085e090e1ec1 163 display.clear_display();
jah128 9:085e090e1ec1 164 display.set_position(0,0);
jah128 9:085e090e1ec1 165 display.write_string("BEACON FOUND AT");
jah128 9:085e090e1ec1 166 display.set_position(1,0);
jah128 9:085e090e1ec1 167 char degrees_string[16];
jah128 9:085e090e1ec1 168 sprintf(degrees_string,"%d DEGREES",beacon_heading);
jah128 9:085e090e1ec1 169 display.write_string(degrees_string);
jah128 0:8a5497a2e366 170 }
jah128 0:8a5497a2e366 171
jah128 17:f26a5805cde3 172 void toggle_program(){
jah128 17:f26a5805cde3 173 toggle_program_stage ++;
jah128 17:f26a5805cde3 174 if(toggle_program_stage == 3) toggle_program_stage = 0;
jah128 17:f26a5805cde3 175 switch(toggle_program_stage){
jah128 17:f26a5805cde3 176 case 0: set_program(5); break;
jah128 17:f26a5805cde3 177 case 1: set_program(6); break;
jah128 17:f26a5805cde3 178 case 2: set_program(7);break;
jah128 17:f26a5805cde3 179 case 3: set_program(7); break;
jah128 17:f26a5805cde3 180 }
jah128 17:f26a5805cde3 181 }
jah128 17:f26a5805cde3 182
jah128 1:f6356cf1cefc 183 /// Code goes here to handle what should happen when the user switch is pressed
jah128 0:8a5497a2e366 184 void handle_switch_event(char switch_state)
jah128 0:8a5497a2e366 185 {
jah128 6:ff3c66f7372b 186 /// 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
jah128 1:f6356cf1cefc 187 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 13:f5994956b1ba 188 if(choose_program_mode == 0) {
jah128 13:f5994956b1ba 189 if(switch_state == 8) switch_held = 1;
jah128 13:f5994956b1ba 190 else if(switch_state == 0 && switch_held == 2) switch_held = 3;
jah128 13:f5994956b1ba 191 else switch_held = 0;
jah128 13:f5994956b1ba 192 } else {
jah128 13:f5994956b1ba 193 // We are in choose program mode
jah128 13:f5994956b1ba 194 if(switch_state == 8) {
jah128 13:f5994956b1ba 195 program_selection ++;
jah128 13:f5994956b1ba 196 if(program_selection > program_count) program_selection = 0;
jah128 13:f5994956b1ba 197 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 13:f5994956b1ba 198 else set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 199 }
jah128 13:f5994956b1ba 200 if(switch_state == 4) {
jah128 14:f623db1e6184 201 if(program_selection == 0) program_selection = program_count;
jah128 14:f623db1e6184 202 else program_selection --;
jah128 13:f5994956b1ba 203 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 13:f5994956b1ba 204 else set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 205 }
jah128 13:f5994956b1ba 206 if(switch_state == 1 || switch_state == 2){
jah128 13:f5994956b1ba 207 if(program_selection == program_count){
jah128 13:f5994956b1ba 208 recharging_state = 1;
jah128 13:f5994956b1ba 209 set_program(previous_program);
jah128 13:f5994956b1ba 210 strcpy(prog_name,"CHARGING PROGRAM");
jah128 13:f5994956b1ba 211 set_program_info("HEAD TO BEACON");
jah128 13:f5994956b1ba 212
jah128 13:f5994956b1ba 213 }
jah128 13:f5994956b1ba 214 else set_program(program_selection);
jah128 13:f5994956b1ba 215 choose_program_mode = 0;
jah128 13:f5994956b1ba 216 switch_held = 0;
jah128 13:f5994956b1ba 217 }
jah128 13:f5994956b1ba 218 }
jah128 14:f623db1e6184 219 //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);
jah128 0:8a5497a2e366 220 }
GusZ92 18:3fa5cd53ed45 221 void handle_user_serial_message(char * message, char length, char interface)
GusZ92 18:3fa5cd53ed45 222 {
GusZ92 18:3fa5cd53ed45 223 // This is where user code for handling a (non-system) serial message should go
GusZ92 18:3fa5cd53ed45 224 //
GusZ92 18:3fa5cd53ed45 225 // message = pointer to message char array
GusZ92 18:3fa5cd53ed45 226 // length = length of message
GusZ92 18:3fa5cd53ed45 227 // interface = 0 for PC serial connection, 1 for Bluetooth
GusZ92 18:3fa5cd53ed45 228
GusZ92 18:3fa5cd53ed45 229 char buffer[255];
GusZ92 18:3fa5cd53ed45 230 sprintf(buffer,message,length);
GusZ92 18:3fa5cd53ed45 231 for(int i=0; i<length; i++) {
GusZ92 18:3fa5cd53ed45 232 buffer[i]=message[i];
GusZ92 18:3fa5cd53ed45 233 }
GusZ92 18:3fa5cd53ed45 234 buffer[length]=0;
GusZ92 18:3fa5cd53ed45 235 if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
GusZ92 18:3fa5cd53ed45 236 else debug("Received USB message:%s [%d chars]\n",buffer,length);
GusZ92 18:3fa5cd53ed45 237
GusZ92 18:3fa5cd53ed45 238 display.clear_display();
GusZ92 18:3fa5cd53ed45 239 display.set_position(0,0);
GusZ92 18:3fa5cd53ed45 240 display.write_string(buffer);
GusZ92 18:3fa5cd53ed45 241
GusZ92 18:3fa5cd53ed45 242 //"A" = 0x41
jhok500 20:fe7eb85cf3a1 243 if(buffer[0] == 'A'){
GusZ92 18:3fa5cd53ed45 244 /* display.clear_display();
GusZ92 18:3fa5cd53ed45 245 display.set_position(0,0);
GusZ92 18:3fa5cd53ed45 246 display.write_string("Gotcha!");*/
GusZ92 18:3fa5cd53ed45 247
jhok500 20:fe7eb85cf3a1 248 //set_program(255); // STOP
jhok500 20:fe7eb85cf3a1 249 main_program_state = 255;
GusZ92 18:3fa5cd53ed45 250 }
GusZ92 18:3fa5cd53ed45 251 //"B" = 0x42
jhok500 20:fe7eb85cf3a1 252 else if(buffer[0] == 'B'){
jhok500 20:fe7eb85cf3a1 253 //set_program(1); // Head to beacon
jhok500 20:fe7eb85cf3a1 254 main_program_state = 1;
GusZ92 18:3fa5cd53ed45 255 }
GusZ92 18:3fa5cd53ed45 256 //"C" = 0x43
jhok500 20:fe7eb85cf3a1 257 else if(buffer[0] == 'C'){
jhok500 20:fe7eb85cf3a1 258 //set_program(2); // Head to beacon + 180
jhok500 20:fe7eb85cf3a1 259 main_program_state = 2;
GusZ92 18:3fa5cd53ed45 260 }
GusZ92 18:3fa5cd53ed45 261 //"D" = 0x44
jhok500 20:fe7eb85cf3a1 262 else if(buffer[0] == 'D'){
jhok500 20:fe7eb85cf3a1 263 //set_program(8); // Flocking
jhok500 20:fe7eb85cf3a1 264 main_program_state = 8;
jhok500 20:fe7eb85cf3a1 265 }
jhok500 20:fe7eb85cf3a1 266 else if(buffer[0] == 'E'){
jhok500 20:fe7eb85cf3a1 267 //set_program(4); // Random Walk
jhok500 20:fe7eb85cf3a1 268 main_program_state = 4;
jhok500 20:fe7eb85cf3a1 269 }
jhok500 20:fe7eb85cf3a1 270 else if(buffer[0] == 'F'){
jhok500 20:fe7eb85cf3a1 271 //set_program(6); // Aggregation
jhok500 20:fe7eb85cf3a1 272 main_program_state = 6;
jhok500 20:fe7eb85cf3a1 273 }
jhok500 20:fe7eb85cf3a1 274 else if(buffer[0] == 'G'){
jhok500 20:fe7eb85cf3a1 275 //set_program(5); // Find Space
jhok500 20:fe7eb85cf3a1 276 main_program_state = 5;
jhok500 20:fe7eb85cf3a1 277 }
jhok500 20:fe7eb85cf3a1 278 else if(buffer[0] == 'H'){
jhok500 20:fe7eb85cf3a1 279 //set_program(6); // Clustering
jhok500 20:fe7eb85cf3a1 280 main_program_state = 6;
jhok500 20:fe7eb85cf3a1 281 }
jhok500 20:fe7eb85cf3a1 282 else if(buffer[0] == 'I'){
jhok500 20:fe7eb85cf3a1 283 //set_program(7); // Tag
jhok500 20:fe7eb85cf3a1 284 main_program_state = 7;
GusZ92 18:3fa5cd53ed45 285 }
GusZ92 18:3fa5cd53ed45 286 else
GusZ92 18:3fa5cd53ed45 287 {
JamesSW 19:b5788427db67 288
GusZ92 18:3fa5cd53ed45 289 }
GusZ92 18:3fa5cd53ed45 290 }
jah128 1:f6356cf1cefc 291 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
jah128 6:ff3c66f7372b 292 int main()
jah128 6:ff3c66f7372b 293 {
jah128 1:f6356cf1cefc 294 ///init() in psiswarm.cpp sets up the robot
jah128 0:8a5497a2e366 295 init();
jah128 0:8a5497a2e366 296 user_code_setup();
GusZ92 18:3fa5cd53ed45 297 //char prog_index;
GusZ92 18:3fa5cd53ed45 298 //char * p_index = &prog_index;
jah128 0:8a5497a2e366 299 user_code_running = 1;
jah128 0:8a5497a2e366 300 while(1) {
GusZ92 18:3fa5cd53ed45 301 /* prog_index = pc.getc();
GusZ92 18:3fa5cd53ed45 302 switch(prog_index){
GusZ92 18:3fa5cd53ed45 303 case 0x61:
GusZ92 18:3fa5cd53ed45 304 display.clear_display();
GusZ92 18:3fa5cd53ed45 305 display.set_position(0,0);
GusZ92 18:3fa5cd53ed45 306 display.write_string("Facing Beacon");
GusZ92 18:3fa5cd53ed45 307 break;
GusZ92 18:3fa5cd53ed45 308 default:
GusZ92 18:3fa5cd53ed45 309 display.clear_display();
GusZ92 18:3fa5cd53ed45 310 display.set_position(0,0);
GusZ92 18:3fa5cd53ed45 311 display.write_string(p_index);
GusZ92 18:3fa5cd53ed45 312 pc.putc(prog_index);
GusZ92 18:3fa5cd53ed45 313 break;
GusZ92 18:3fa5cd53ed45 314 }*/
jah128 9:085e090e1ec1 315 wait(1);
jah128 6:ff3c66f7372b 316 }
jah128 6:ff3c66f7372b 317 }
jah128 6:ff3c66f7372b 318
jah128 10:1b09d4bb847b 319 char * get_program_name(int index)
jah128 13:f5994956b1ba 320 {
jah128 10:1b09d4bb847b 321 char * ret_name = new char[17];
jah128 13:f5994956b1ba 322 switch(index) {
jah128 10:1b09d4bb847b 323 case 0:
jah128 10:1b09d4bb847b 324 strcpy(ret_name,"FACE BEACON");
jah128 10:1b09d4bb847b 325 break;
jah128 10:1b09d4bb847b 326 case 1:
jah128 10:1b09d4bb847b 327 strcpy(ret_name,"HEAD TO BEACON");
jah128 10:1b09d4bb847b 328 break;
jah128 10:1b09d4bb847b 329 case 2:
jah128 10:1b09d4bb847b 330 strcpy(ret_name,"HEAD TO SOUTH");
jah128 10:1b09d4bb847b 331 break;
jah128 10:1b09d4bb847b 332 case 3:
jah128 10:1b09d4bb847b 333 strcpy(ret_name,"RANDOM WALK 1");
jah128 10:1b09d4bb847b 334 break;
jah128 10:1b09d4bb847b 335 case 4:
GusZ92 18:3fa5cd53ed45 336 strcpy(ret_name,"WAITING COMMAND");///gus, was random walk2
jah128 10:1b09d4bb847b 337 break;
jah128 10:1b09d4bb847b 338 case 5:
jah128 10:1b09d4bb847b 339 strcpy(ret_name,"FIND SPACE");
jah128 10:1b09d4bb847b 340 break;
jah128 12:daa53285b6e4 341 case 6:
jah128 12:daa53285b6e4 342 strcpy(ret_name,"CLUSTERING");
jah128 12:daa53285b6e4 343 break;
jah128 16:976a1d0ea897 344 case 7:
jah128 16:976a1d0ea897 345 strcpy(ret_name,"TAG GAME");
jah128 16:976a1d0ea897 346 break;
jah128 13:f5994956b1ba 347 case 255:
jah128 13:f5994956b1ba 348 strcpy(ret_name,"PROGRAM:");
jah128 14:f623db1e6184 349 break;
jah128 10:1b09d4bb847b 350 }
jah128 10:1b09d4bb847b 351 return ret_name;
jah128 10:1b09d4bb847b 352 }
jah128 10:1b09d4bb847b 353
jah128 10:1b09d4bb847b 354 void set_program(int index)
jah128 10:1b09d4bb847b 355 {
jah128 10:1b09d4bb847b 356 main_program_state = index;
jah128 10:1b09d4bb847b 357 program_changed = 1;
jah128 12:daa53285b6e4 358 program_run_init = 1;
jah128 10:1b09d4bb847b 359 strcpy(prog_info,"");
jah128 13:f5994956b1ba 360 strcpy(prog_name,get_program_name(index));
jah128 10:1b09d4bb847b 361 }
jah128 10:1b09d4bb847b 362
jah128 10:1b09d4bb847b 363 void set_program_info(char * info)
jah128 10:1b09d4bb847b 364 {
jah128 10:1b09d4bb847b 365 strcpy(prog_info,info);
jah128 10:1b09d4bb847b 366 program_changed = 1;
jah128 10:1b09d4bb847b 367 }
jah128 9:085e090e1ec1 368
jah128 9:085e090e1ec1 369 void update_display()
jah128 9:085e090e1ec1 370 {
jah128 10:1b09d4bb847b 371 if(program_changed == 1) {
jah128 10:1b09d4bb847b 372 program_changed = 0;
jah128 9:085e090e1ec1 373 display.clear_display();
jah128 13:f5994956b1ba 374
jah128 10:1b09d4bb847b 375 if(display_debug_inf==1) display_debug_info();
jah128 13:f5994956b1ba 376 else {
jah128 10:1b09d4bb847b 377 display.set_position(0,0);
jah128 10:1b09d4bb847b 378 display.write_string(prog_name);
jah128 9:085e090e1ec1 379 }
jah128 10:1b09d4bb847b 380 display.set_position(1,0);
jah128 10:1b09d4bb847b 381 display.write_string(prog_info);
jah128 9:085e090e1ec1 382 }
jah128 9:085e090e1ec1 383 }
jah128 9:085e090e1ec1 384
jah128 9:085e090e1ec1 385 void display_debug_info()
jah128 9:085e090e1ec1 386 {
jah128 9:085e090e1ec1 387 char disp_line[16] = "- - - - - - - -";
jah128 9:085e090e1ec1 388 if(beacon_found==1)disp_line[0]='B';
jah128 10:1b09d4bb847b 389 for(int i=1; i<8; i++) {
jah128 10:1b09d4bb847b 390 if(robots_found[i])disp_line[((i)*2)]=48+i;
jah128 9:085e090e1ec1 391 }
jah128 10:1b09d4bb847b 392 display.set_position(0,0);
jah128 9:085e090e1ec1 393 display.write_string(disp_line);
jah128 9:085e090e1ec1 394 }
jah128 9:085e090e1ec1 395
jah128 6:ff3c66f7372b 396 /// Verbose output
jah128 6:ff3c66f7372b 397 void out(const char* format, ...)
jah128 6:ff3c66f7372b 398 {
jah128 6:ff3c66f7372b 399 char buffer[256];
jah128 6:ff3c66f7372b 400 if (debug_mode) {
jah128 6:ff3c66f7372b 401 va_list vl;
jah128 6:ff3c66f7372b 402 va_start(vl, format);
jah128 6:ff3c66f7372b 403 vsprintf(buffer,format,vl);
jah128 6:ff3c66f7372b 404 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 6:ff3c66f7372b 405 va_end(vl);
jah128 0:8a5497a2e366 406 }
jah128 0:8a5497a2e366 407 }