UKESF Headstart Summer School / Mbed 2 deprecated ScriptBasedLab

YRL Maze lab made more script-y

Dependencies:   PsiSwarmLab-ScriptingBased mbed

Fork of UKESF_Lab by UKESF Headstart Summer School

main.cpp@17:da524989b637, 2016-01-07 (annotated)

Committer:
alanmillard
Date:
Thu Jan 07 17:58:07 2016 +0000
Revision:
17:da524989b637
Parent:
16:976a1d0ea897
Child:
18:5921c1853e8a
Now stitching two successive partial user messages together, if they are received within 0.1s of each other.

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
alanmillard 17:da524989b637 36 char default_normal_program = 8; // The program to run on turn on (after 'face beacon' program)
alanmillard 17:da524989b637 37 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 38 char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code]
jah128 10:1b09d4bb847b 39 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 40 char main_program_state; // Index of the currently running program
jah128 13:f5994956b1ba 41 char previous_program; // Used to hold previous running program when it is paused for switch press etc
jah128 10:1b09d4bb847b 42 char program_changed = 0; // Flag to update display when program is changed
jah128 12:daa53285b6e4 43 char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary
jah128 10:1b09d4bb847b 44 char success_count = 0; // Flag to indicate the success of a program
jah128 10:1b09d4bb847b 45 char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps
jah128 10:1b09d4bb847b 46 char target_reached = 0; // Flag to indicate if a program target has been reached
jah128 10:1b09d4bb847b 47 char prog_name [17]; // Stores the name of the running program [line 0 on the display]
jah128 10:1b09d4bb847b 48 char prog_info [17]; // Stores information about the current state of the program [line 1 on the display]
jah128 10:1b09d4bb847b 49 char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!]
jah128 10:1b09d4bb847b 50 char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running)
jah128 13:f5994956b1ba 51 char switch_held = 0; // Used for detected when the cursor switch is held to override program choice
jah128 13:f5994956b1ba 52 char choose_program_mode = 0;
jah128 16:976a1d0ea897 53 char program_count = 8;
jah128 13:f5994956b1ba 54 char program_selection;
alanmillard 17:da524989b637 55 int flocking_heading = 0;
jah128 11:7b3ee540ba56 56
jah128 14:f623db1e6184 57 float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55
jah128 14:f623db1e6184 58 float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
jah128 11:7b3ee540ba56 59
jah128 9:085e090e1ec1 60 Ticker main_loop_ticker;
alanmillard 17:da524989b637 61 Ticker flocking_ticker;
alanmillard 17:da524989b637 62
alanmillard 17:da524989b637 63 void flocking_heading_update()
alanmillard 17:da524989b637 64 {
alanmillard 17:da524989b637 65 flocking_heading += 5;
alanmillard 17:da524989b637 66 flocking_heading %= 360;
alanmillard 17:da524989b637 67 }
jah128 9:085e090e1ec1 68
jah128 10:1b09d4bb847b 69 ///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 70 void main_loop()
jah128 9:085e090e1ec1 71 {
jah128 13:f5994956b1ba 72 if(switch_held == 1)switch_held=2;
jah128 14:f623db1e6184 73 if(switch_held == 3 && choose_program_mode == 0) {
jah128 13:f5994956b1ba 74 //The switch has been held right and then released: stop the current program
jah128 13:f5994956b1ba 75 previous_program = main_program_state;
jah128 13:f5994956b1ba 76 program_selection = previous_program;
jah128 13:f5994956b1ba 77 choose_program_mode = 1;
jah128 13:f5994956b1ba 78 set_program(255);
jah128 14:f623db1e6184 79 set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 80 }
jah128 10:1b09d4bb847b 81 if(use_recharging_program == 1)recharging_program();
jah128 9:085e090e1ec1 82 update_display();
jah128 10:1b09d4bb847b 83 if(recharging_state == 0) {
jah128 10:1b09d4bb847b 84 switch(main_program_state) {
jah128 10:1b09d4bb847b 85 case 0: //Case 0 is the initial program: turn to face beacon
jah128 10:1b09d4bb847b 86 if(step_cycle == 0) {
jah128 10:1b09d4bb847b 87 char turn_status = turn_to_bearing(0);
jah128 10:1b09d4bb847b 88 if(turn_status == 0) {
jah128 10:1b09d4bb847b 89 success_count ++;
jah128 10:1b09d4bb847b 90 if(success_count > 1) set_program(default_normal_program);
jah128 10:1b09d4bb847b 91 } else success_count = 0;
jah128 10:1b09d4bb847b 92 }
jah128 10:1b09d4bb847b 93 break;
jah128 10:1b09d4bb847b 94 case 1:
jah128 10:1b09d4bb847b 95 target_reached = 0;
jah128 10:1b09d4bb847b 96 head_to_bearing_program(0);
jah128 10:1b09d4bb847b 97 if(target_reached == 1) set_program(2);
jah128 10:1b09d4bb847b 98 break;
jah128 10:1b09d4bb847b 99 case 2:
jah128 10:1b09d4bb847b 100 target_reached = 0;
jah128 10:1b09d4bb847b 101 head_to_bearing_program(180);
jah128 10:1b09d4bb847b 102 if(target_reached == 1) set_program(1);
jah128 10:1b09d4bb847b 103 break;
jah128 10:1b09d4bb847b 104 case 3:
jah128 10:1b09d4bb847b 105 curved_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 106 break;
jah128 10:1b09d4bb847b 107 case 4:
jah128 10:1b09d4bb847b 108 straight_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 109 break;
jah128 10:1b09d4bb847b 110 case 5:
jah128 13:f5994956b1ba 111 find_space_program(1);
jah128 10:1b09d4bb847b 112 break;
jah128 12:daa53285b6e4 113 case 6:
jah128 13:f5994956b1ba 114 clustering_program(0,1);
jah128 12:daa53285b6e4 115 break;
jah128 16:976a1d0ea897 116 case 7:
jah128 16:976a1d0ea897 117 tag_game_program();
jah128 16:976a1d0ea897 118 break;
alanmillard 17:da524989b637 119 case 8:
alanmillard 17:da524989b637 120 flocking_program();
alanmillard 17:da524989b637 121 break;
jah128 13:f5994956b1ba 122 case 255:
jah128 13:f5994956b1ba 123 stop_program();
jah128 14:f623db1e6184 124 break;
jah128 10:1b09d4bb847b 125 }
jah128 9:085e090e1ec1 126 }
jah128 9:085e090e1ec1 127 step_cycle=1-step_cycle;
jah128 9:085e090e1ec1 128 }
jah128 9:085e090e1ec1 129
jah128 1:f6356cf1cefc 130 ///Place user code here that should be run after initialisation but before the main loop
jah128 6:ff3c66f7372b 131 void user_code_setup()
jah128 13:f5994956b1ba 132 {
jah128 9:085e090e1ec1 133 wait(0.8);
jah128 9:085e090e1ec1 134 display.clear_display();
jah128 9:085e090e1ec1 135 display.set_position(0,0);
jah128 9:085e090e1ec1 136 display.write_string("BEAUTIFUL MEME");
jah128 9:085e090e1ec1 137 display.set_position(1,0);
jah128 9:085e090e1ec1 138 display.write_string(" PROJECT");
jah128 9:085e090e1ec1 139 wait(0.2);
jah128 6:ff3c66f7372b 140 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 141 out("Beautiful Meme Project Demo Code \n");
jah128 6:ff3c66f7372b 142 out("------------------------------------------------------\n");
alanmillard 17:da524989b637 143 while(1);
jah128 6:ff3c66f7372b 144 locate_beacon();
jah128 6:ff3c66f7372b 145 while(beacon_found == 0) {
jah128 9:085e090e1ec1 146 wait(0.5);
jah128 6:ff3c66f7372b 147 locate_beacon();
jah128 6:ff3c66f7372b 148 }
jah128 6:ff3c66f7372b 149 start_infrared_timers();
jah128 9:085e090e1ec1 150 main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
alanmillard 17:da524989b637 151 flocking_ticker.attach(&flocking_heading_update, 1);
jah128 16:976a1d0ea897 152 set_program(0);
jah128 9:085e090e1ec1 153 set_leds(0x00,0x00);
jah128 10:1b09d4bb847b 154 set_center_led(3,0.5);
jah128 9:085e090e1ec1 155 display.clear_display();
jah128 9:085e090e1ec1 156 display.set_position(0,0);
jah128 9:085e090e1ec1 157 display.write_string("BEACON FOUND AT");
jah128 9:085e090e1ec1 158 display.set_position(1,0);
jah128 9:085e090e1ec1 159 char degrees_string[16];
jah128 9:085e090e1ec1 160 sprintf(degrees_string,"%d DEGREES",beacon_heading);
jah128 9:085e090e1ec1 161 display.write_string(degrees_string);
jah128 0:8a5497a2e366 162 }
jah128 0:8a5497a2e366 163
jah128 1:f6356cf1cefc 164 /// Code goes here to handle what should happen when the user switch is pressed
jah128 0:8a5497a2e366 165 void handle_switch_event(char switch_state)
jah128 0:8a5497a2e366 166 {
jah128 6:ff3c66f7372b 167 /// 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 168 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 13:f5994956b1ba 169 if(choose_program_mode == 0) {
jah128 13:f5994956b1ba 170 if(switch_state == 8) switch_held = 1;
jah128 13:f5994956b1ba 171 else if(switch_state == 0 && switch_held == 2) switch_held = 3;
jah128 13:f5994956b1ba 172 else switch_held = 0;
jah128 13:f5994956b1ba 173 } else {
jah128 13:f5994956b1ba 174 // We are in choose program mode
jah128 13:f5994956b1ba 175 if(switch_state == 8) {
jah128 13:f5994956b1ba 176 program_selection ++;
jah128 13:f5994956b1ba 177 if(program_selection > program_count) program_selection = 0;
jah128 13:f5994956b1ba 178 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 13:f5994956b1ba 179 else set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 180 }
jah128 13:f5994956b1ba 181 if(switch_state == 4) {
jah128 14:f623db1e6184 182 if(program_selection == 0) program_selection = program_count;
jah128 14:f623db1e6184 183 else program_selection --;
jah128 13:f5994956b1ba 184 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 13:f5994956b1ba 185 else set_program_info(get_program_name(program_selection));
jah128 13:f5994956b1ba 186 }
jah128 13:f5994956b1ba 187 if(switch_state == 1 || switch_state == 2){
jah128 13:f5994956b1ba 188 if(program_selection == program_count){
jah128 13:f5994956b1ba 189 recharging_state = 1;
jah128 13:f5994956b1ba 190 set_program(previous_program);
jah128 13:f5994956b1ba 191 strcpy(prog_name,"CHARGING PROGRAM");
jah128 13:f5994956b1ba 192 set_program_info("HEAD TO BEACON");
jah128 13:f5994956b1ba 193
jah128 13:f5994956b1ba 194 }
jah128 13:f5994956b1ba 195 else set_program(program_selection);
jah128 13:f5994956b1ba 196 choose_program_mode = 0;
jah128 13:f5994956b1ba 197 switch_held = 0;
jah128 13:f5994956b1ba 198 }
jah128 13:f5994956b1ba 199 }
jah128 14:f623db1e6184 200 //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 201 }
jah128 0:8a5497a2e366 202
jah128 1:f6356cf1cefc 203 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
jah128 6:ff3c66f7372b 204 int main()
jah128 6:ff3c66f7372b 205 {
jah128 1:f6356cf1cefc 206 ///init() in psiswarm.cpp sets up the robot
jah128 0:8a5497a2e366 207 init();
jah128 0:8a5497a2e366 208 user_code_setup();
jah128 0:8a5497a2e366 209 user_code_running = 1;
jah128 0:8a5497a2e366 210 while(1) {
jah128 9:085e090e1ec1 211 wait(1);
jah128 6:ff3c66f7372b 212 }
jah128 6:ff3c66f7372b 213 }
jah128 6:ff3c66f7372b 214
jah128 10:1b09d4bb847b 215 char * get_program_name(int index)
jah128 13:f5994956b1ba 216 {
jah128 10:1b09d4bb847b 217 char * ret_name = new char[17];
jah128 13:f5994956b1ba 218 switch(index) {
jah128 10:1b09d4bb847b 219 case 0:
jah128 10:1b09d4bb847b 220 strcpy(ret_name,"FACE BEACON");
jah128 10:1b09d4bb847b 221 break;
jah128 10:1b09d4bb847b 222 case 1:
jah128 10:1b09d4bb847b 223 strcpy(ret_name,"HEAD TO BEACON");
jah128 10:1b09d4bb847b 224 break;
jah128 10:1b09d4bb847b 225 case 2:
jah128 10:1b09d4bb847b 226 strcpy(ret_name,"HEAD TO SOUTH");
jah128 10:1b09d4bb847b 227 break;
jah128 10:1b09d4bb847b 228 case 3:
jah128 10:1b09d4bb847b 229 strcpy(ret_name,"RANDOM WALK 1");
jah128 10:1b09d4bb847b 230 break;
jah128 10:1b09d4bb847b 231 case 4:
jah128 10:1b09d4bb847b 232 strcpy(ret_name,"RANDOM WALK 2");
jah128 10:1b09d4bb847b 233 break;
jah128 10:1b09d4bb847b 234 case 5:
jah128 10:1b09d4bb847b 235 strcpy(ret_name,"FIND SPACE");
jah128 10:1b09d4bb847b 236 break;
jah128 12:daa53285b6e4 237 case 6:
jah128 12:daa53285b6e4 238 strcpy(ret_name,"CLUSTERING");
jah128 12:daa53285b6e4 239 break;
jah128 16:976a1d0ea897 240 case 7:
jah128 16:976a1d0ea897 241 strcpy(ret_name,"TAG GAME");
jah128 16:976a1d0ea897 242 break;
alanmillard 17:da524989b637 243 case 8:
alanmillard 17:da524989b637 244 strcpy(ret_name,"FLOCKING");
alanmillard 17:da524989b637 245 break;
jah128 13:f5994956b1ba 246 case 255:
jah128 13:f5994956b1ba 247 strcpy(ret_name,"PROGRAM:");
jah128 14:f623db1e6184 248 break;
jah128 10:1b09d4bb847b 249 }
jah128 10:1b09d4bb847b 250 return ret_name;
jah128 10:1b09d4bb847b 251 }
jah128 10:1b09d4bb847b 252
jah128 10:1b09d4bb847b 253 void set_program(int index)
jah128 10:1b09d4bb847b 254 {
jah128 10:1b09d4bb847b 255 main_program_state = index;
jah128 10:1b09d4bb847b 256 program_changed = 1;
jah128 12:daa53285b6e4 257 program_run_init = 1;
jah128 10:1b09d4bb847b 258 strcpy(prog_info,"");
jah128 13:f5994956b1ba 259 strcpy(prog_name,get_program_name(index));
jah128 10:1b09d4bb847b 260 }
jah128 10:1b09d4bb847b 261
jah128 10:1b09d4bb847b 262 void set_program_info(char * info)
jah128 10:1b09d4bb847b 263 {
jah128 10:1b09d4bb847b 264 strcpy(prog_info,info);
jah128 10:1b09d4bb847b 265 program_changed = 1;
jah128 10:1b09d4bb847b 266 }
jah128 9:085e090e1ec1 267
jah128 9:085e090e1ec1 268 void update_display()
jah128 9:085e090e1ec1 269 {
jah128 10:1b09d4bb847b 270 if(program_changed == 1) {
jah128 10:1b09d4bb847b 271 program_changed = 0;
jah128 9:085e090e1ec1 272 display.clear_display();
jah128 13:f5994956b1ba 273
jah128 10:1b09d4bb847b 274 if(display_debug_inf==1) display_debug_info();
jah128 13:f5994956b1ba 275 else {
jah128 10:1b09d4bb847b 276 display.set_position(0,0);
jah128 10:1b09d4bb847b 277 display.write_string(prog_name);
jah128 9:085e090e1ec1 278 }
jah128 10:1b09d4bb847b 279 display.set_position(1,0);
jah128 10:1b09d4bb847b 280 display.write_string(prog_info);
jah128 9:085e090e1ec1 281 }
jah128 9:085e090e1ec1 282 }
jah128 9:085e090e1ec1 283
jah128 9:085e090e1ec1 284 void display_debug_info()
jah128 9:085e090e1ec1 285 {
jah128 9:085e090e1ec1 286 char disp_line[16] = "- - - - - - - -";
jah128 9:085e090e1ec1 287 if(beacon_found==1)disp_line[0]='B';
jah128 10:1b09d4bb847b 288 for(int i=1; i<8; i++) {
jah128 10:1b09d4bb847b 289 if(robots_found[i])disp_line[((i)*2)]=48+i;
jah128 9:085e090e1ec1 290 }
jah128 10:1b09d4bb847b 291 display.set_position(0,0);
jah128 9:085e090e1ec1 292 display.write_string(disp_line);
jah128 9:085e090e1ec1 293 }
jah128 9:085e090e1ec1 294
jah128 6:ff3c66f7372b 295 /// Verbose output
jah128 6:ff3c66f7372b 296 void out(const char* format, ...)
jah128 6:ff3c66f7372b 297 {
jah128 6:ff3c66f7372b 298 char buffer[256];
jah128 6:ff3c66f7372b 299 if (debug_mode) {
jah128 6:ff3c66f7372b 300 va_list vl;
jah128 6:ff3c66f7372b 301 va_start(vl, format);
jah128 6:ff3c66f7372b 302 vsprintf(buffer,format,vl);
jah128 6:ff3c66f7372b 303 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 6:ff3c66f7372b 304 va_end(vl);
jah128 0:8a5497a2e366 305 }
jah128 0:8a5497a2e366 306 }