![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
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?
User | Revision | Line number | New 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 | } |