James Hilder / Mbed 2 deprecated PsiSwarm_V41

Psi Swarm Code V0.41 [With Beautiful Meme program]

Dependencies:   PsiSwarmLibrary mbed

Fork of BeautifulMemeProjectBT by Alan Millard

BeautifulMeme/bmeme.cpp@30:513457c1ad12, 2016-03-15 (annotated)

Committer:
jah128
Date:
Tue Mar 15 00:58:43 2016 +0000
Revision:
30:513457c1ad12
Added serial handling for Psi Console

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 30:513457c1ad12 1 /// PsiSwarm Beautiful Meme Project Source Code
jah128 30:513457c1ad12 2 /// Version 0.41
jah128 30:513457c1ad12 3 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 30:513457c1ad12 4 /// University of York
jah128 30:513457c1ad12 5
jah128 30:513457c1ad12 6 /// Include bmeme.h - this includes main.h, psiswarm.h all the other necessary core files
jah128 30:513457c1ad12 7
jah128 30:513457c1ad12 8 #include "bmeme.h"
jah128 30:513457c1ad12 9
jah128 30:513457c1ad12 10 // IMPORTANT!!!
jah128 30:513457c1ad12 11 // Do not call the IR functions at all as they will interfere with the correct operation of this program
jah128 30:513457c1ad12 12 // Instead, use the values held in the variables below; they are updated every 500ms
jah128 30:513457c1ad12 13
jah128 30:513457c1ad12 14 char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window
jah128 30:513457c1ad12 15 int beacon_heading = 0; // This is the heading from the last time a beacon was detected
jah128 30:513457c1ad12 16 char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
jah128 30:513457c1ad12 17 int robots_heading[8]; // These are the headings from the last time the respective robots were detected
jah128 30:513457c1ad12 18 unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected
jah128 30:513457c1ad12 19 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
jah128 30:513457c1ad12 20 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
jah128 30:513457c1ad12 21
jah128 30:513457c1ad12 22 char default_normal_program = 8; // The program to run on turn on (after 'face beacon' program)
jah128 30:513457c1ad12 23 char use_recharging_program = 0; // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
jah128 30:513457c1ad12 24 char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code]
jah128 30:513457c1ad12 25 char display_debug_inf = 0; // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
jah128 30:513457c1ad12 26 char main_program_state; // Index of the currently running program
jah128 30:513457c1ad12 27 char previous_program; // Used to hold previous running program when it is paused for switch press etc
jah128 30:513457c1ad12 28 char program_changed = 0; // Flag to update display when program is changed
jah128 30:513457c1ad12 29 char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary
jah128 30:513457c1ad12 30 char success_count = 0; // Flag to indicate the success of a program
jah128 30:513457c1ad12 31 char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps
jah128 30:513457c1ad12 32 char target_reached = 0; // Flag to indicate if a program target has been reached
jah128 30:513457c1ad12 33 char prog_name [17]; // Stores the name of the running program [line 0 on the display]
jah128 30:513457c1ad12 34 char prog_info [17]; // Stores information about the current state of the program [line 1 on the display]
jah128 30:513457c1ad12 35 char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!]
jah128 30:513457c1ad12 36 char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running)
jah128 30:513457c1ad12 37 char switch_held = 0; // Used for detected when the cursor switch is held to override program choice
jah128 30:513457c1ad12 38 char choose_program_mode = 0;
jah128 30:513457c1ad12 39 char program_count = 8;
jah128 30:513457c1ad12 40 char program_selection;
jah128 30:513457c1ad12 41 int flocking_headings[8]; // Beacon heading of each robot
jah128 30:513457c1ad12 42
jah128 30:513457c1ad12 43 float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55
jah128 30:513457c1ad12 44 float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
jah128 30:513457c1ad12 45
jah128 30:513457c1ad12 46 Ticker main_loop_ticker;
jah128 30:513457c1ad12 47
jah128 30:513457c1ad12 48 ///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 30:513457c1ad12 49 void bmeme_main_loop()
jah128 30:513457c1ad12 50 {
jah128 30:513457c1ad12 51 if(switch_held == 1)switch_held=2;
jah128 30:513457c1ad12 52 if(switch_held == 3 && choose_program_mode == 0) {
jah128 30:513457c1ad12 53 //The switch has been held right and then released: stop the current program
jah128 30:513457c1ad12 54 previous_program = main_program_state;
jah128 30:513457c1ad12 55 program_selection = previous_program;
jah128 30:513457c1ad12 56 choose_program_mode = 1;
jah128 30:513457c1ad12 57 set_program(255);
jah128 30:513457c1ad12 58 set_program_info(get_program_name(program_selection));
jah128 30:513457c1ad12 59 }
jah128 30:513457c1ad12 60 if(use_recharging_program == 1)recharging_program();
jah128 30:513457c1ad12 61 update_display();
jah128 30:513457c1ad12 62 if(recharging_state == 0) {
jah128 30:513457c1ad12 63 switch(main_program_state) {
jah128 30:513457c1ad12 64 case 0: //Case 0 is the initial program: turn to face beacon
jah128 30:513457c1ad12 65 if(step_cycle == 0) {
jah128 30:513457c1ad12 66 char turn_status = turn_to_bearing(0);
jah128 30:513457c1ad12 67 if(turn_status == 0) {
jah128 30:513457c1ad12 68 success_count ++;
jah128 30:513457c1ad12 69 if(success_count > 1) set_program(default_normal_program);
jah128 30:513457c1ad12 70 } else success_count = 0;
jah128 30:513457c1ad12 71 }
jah128 30:513457c1ad12 72 break;
jah128 30:513457c1ad12 73 case 1:
jah128 30:513457c1ad12 74 target_reached = 0;
jah128 30:513457c1ad12 75 head_to_bearing_program(0);
jah128 30:513457c1ad12 76 if(target_reached == 1) set_program(2);
jah128 30:513457c1ad12 77 break;
jah128 30:513457c1ad12 78 case 2:
jah128 30:513457c1ad12 79 target_reached = 0;
jah128 30:513457c1ad12 80 head_to_bearing_program(180);
jah128 30:513457c1ad12 81 if(target_reached == 1) set_program(1);
jah128 30:513457c1ad12 82 break;
jah128 30:513457c1ad12 83 case 3:
jah128 30:513457c1ad12 84 curved_random_walk_with_interaction_program();
jah128 30:513457c1ad12 85 break;
jah128 30:513457c1ad12 86 case 4:
jah128 30:513457c1ad12 87 straight_random_walk_with_interaction_program();
jah128 30:513457c1ad12 88 break;
jah128 30:513457c1ad12 89 case 5:
jah128 30:513457c1ad12 90 find_space_program(1);
jah128 30:513457c1ad12 91 break;
jah128 30:513457c1ad12 92 case 6:
jah128 30:513457c1ad12 93 clustering_program(0,1);
jah128 30:513457c1ad12 94 break;
jah128 30:513457c1ad12 95 case 7:
jah128 30:513457c1ad12 96 tag_game_program();
jah128 30:513457c1ad12 97 break;
jah128 30:513457c1ad12 98 case 8:
jah128 30:513457c1ad12 99 flocking_program();
jah128 30:513457c1ad12 100 break;
jah128 30:513457c1ad12 101 case 255:
jah128 30:513457c1ad12 102 stop_program();
jah128 30:513457c1ad12 103 break;
jah128 30:513457c1ad12 104 }
jah128 30:513457c1ad12 105 }
jah128 30:513457c1ad12 106 step_cycle=1-step_cycle;
jah128 30:513457c1ad12 107 }
jah128 30:513457c1ad12 108
jah128 30:513457c1ad12 109 ///Place user code here that should be run after initialisation but before the main loop
jah128 30:513457c1ad12 110 void bmeme_user_code_setup()
jah128 30:513457c1ad12 111 {
jah128 30:513457c1ad12 112 program_name = "B-Meme";
jah128 30:513457c1ad12 113 author_name = "YRL";
jah128 30:513457c1ad12 114 version_name = "0.4";
jah128 30:513457c1ad12 115
jah128 30:513457c1ad12 116 display.clear_display();
jah128 30:513457c1ad12 117 display.set_position(0,0);
jah128 30:513457c1ad12 118 display.write_string("BEAUTIFUL MEME");
jah128 30:513457c1ad12 119 display.set_position(1,0);
jah128 30:513457c1ad12 120 display.write_string(" PROJECT");
jah128 30:513457c1ad12 121 wait(0.2);
jah128 30:513457c1ad12 122 out("------------------------------------------------------\n");
jah128 30:513457c1ad12 123 out("Beautiful Meme Project Demo Code \n");
jah128 30:513457c1ad12 124 out("------------------------------------------------------\n");
jah128 30:513457c1ad12 125 locate_beacon();
jah128 30:513457c1ad12 126 while(beacon_found == 0) {
jah128 30:513457c1ad12 127 wait(0.5);
jah128 30:513457c1ad12 128 locate_beacon();
jah128 30:513457c1ad12 129 }
jah128 30:513457c1ad12 130 start_infrared_timers();
jah128 30:513457c1ad12 131 main_loop_ticker.attach_us(&bmeme_main_loop,BEACON_PERIOD * 10);
jah128 30:513457c1ad12 132 set_program(0);
jah128 30:513457c1ad12 133 set_leds(0x00,0x00);
jah128 30:513457c1ad12 134 set_center_led(3,0.5);
jah128 30:513457c1ad12 135 display.clear_display();
jah128 30:513457c1ad12 136 display.set_position(0,0);
jah128 30:513457c1ad12 137 display.write_string("BEACON FOUND AT");
jah128 30:513457c1ad12 138 display.set_position(1,0);
jah128 30:513457c1ad12 139 char degrees_string[16];
jah128 30:513457c1ad12 140 sprintf(degrees_string,"%d DEGREES",beacon_heading);
jah128 30:513457c1ad12 141 display.write_string(degrees_string);
jah128 30:513457c1ad12 142 }
jah128 30:513457c1ad12 143
jah128 30:513457c1ad12 144 /// Code goes here to handle what should happen when the user switch is pressed
jah128 30:513457c1ad12 145 void bmeme_handle_switch_event(char switch_state)
jah128 30:513457c1ad12 146 {
jah128 30:513457c1ad12 147 /// 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 30:513457c1ad12 148 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 30:513457c1ad12 149 if(choose_program_mode == 0) {
jah128 30:513457c1ad12 150 if(switch_state == 8) switch_held = 1;
jah128 30:513457c1ad12 151 else if(switch_state == 0 && switch_held == 2) switch_held = 3;
jah128 30:513457c1ad12 152 else switch_held = 0;
jah128 30:513457c1ad12 153 } else {
jah128 30:513457c1ad12 154 // We are in choose program mode
jah128 30:513457c1ad12 155 if(switch_state == 8) {
jah128 30:513457c1ad12 156 program_selection ++;
jah128 30:513457c1ad12 157 if(program_selection > program_count) program_selection = 0;
jah128 30:513457c1ad12 158 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 30:513457c1ad12 159 else set_program_info(get_program_name(program_selection));
jah128 30:513457c1ad12 160 }
jah128 30:513457c1ad12 161 if(switch_state == 4) {
jah128 30:513457c1ad12 162 if(program_selection == 0) program_selection = program_count;
jah128 30:513457c1ad12 163 else program_selection --;
jah128 30:513457c1ad12 164 if(program_selection == program_count) set_program_info("RECHARGE");
jah128 30:513457c1ad12 165 else set_program_info(get_program_name(program_selection));
jah128 30:513457c1ad12 166 }
jah128 30:513457c1ad12 167 if(switch_state == 1 || switch_state == 2){
jah128 30:513457c1ad12 168 if(program_selection == program_count){
jah128 30:513457c1ad12 169 recharging_state = 1;
jah128 30:513457c1ad12 170 set_program(previous_program);
jah128 30:513457c1ad12 171 strcpy(prog_name,"CHARGING PROGRAM");
jah128 30:513457c1ad12 172 set_program_info("HEAD TO BEACON");
jah128 30:513457c1ad12 173
jah128 30:513457c1ad12 174 }
jah128 30:513457c1ad12 175 else set_program(program_selection);
jah128 30:513457c1ad12 176 choose_program_mode = 0;
jah128 30:513457c1ad12 177 switch_held = 0;
jah128 30:513457c1ad12 178 }
jah128 30:513457c1ad12 179 }
jah128 30:513457c1ad12 180 //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 30:513457c1ad12 181 }
jah128 30:513457c1ad12 182
jah128 30:513457c1ad12 183 char * get_program_name(int index)
jah128 30:513457c1ad12 184 {
jah128 30:513457c1ad12 185 char * ret_name = new char[17];
jah128 30:513457c1ad12 186 switch(index) {
jah128 30:513457c1ad12 187 case 0:
jah128 30:513457c1ad12 188 strcpy(ret_name,"FACE BEACON");
jah128 30:513457c1ad12 189 break;
jah128 30:513457c1ad12 190 case 1:
jah128 30:513457c1ad12 191 strcpy(ret_name,"HEAD TO BEACON");
jah128 30:513457c1ad12 192 break;
jah128 30:513457c1ad12 193 case 2:
jah128 30:513457c1ad12 194 strcpy(ret_name,"HEAD TO SOUTH");
jah128 30:513457c1ad12 195 break;
jah128 30:513457c1ad12 196 case 3:
jah128 30:513457c1ad12 197 strcpy(ret_name,"RANDOM WALK 1");
jah128 30:513457c1ad12 198 break;
jah128 30:513457c1ad12 199 case 4:
jah128 30:513457c1ad12 200 strcpy(ret_name,"RANDOM WALK 2");
jah128 30:513457c1ad12 201 break;
jah128 30:513457c1ad12 202 case 5:
jah128 30:513457c1ad12 203 strcpy(ret_name,"FIND SPACE");
jah128 30:513457c1ad12 204 break;
jah128 30:513457c1ad12 205 case 6:
jah128 30:513457c1ad12 206 strcpy(ret_name,"CLUSTERING");
jah128 30:513457c1ad12 207 break;
jah128 30:513457c1ad12 208 case 7:
jah128 30:513457c1ad12 209 strcpy(ret_name,"TAG GAME");
jah128 30:513457c1ad12 210 break;
jah128 30:513457c1ad12 211 case 8:
jah128 30:513457c1ad12 212 strcpy(ret_name,"FLOCKING");
jah128 30:513457c1ad12 213 break;
jah128 30:513457c1ad12 214 case 255:
jah128 30:513457c1ad12 215 strcpy(ret_name,"PROGRAM:");
jah128 30:513457c1ad12 216 break;
jah128 30:513457c1ad12 217 }
jah128 30:513457c1ad12 218 return ret_name;
jah128 30:513457c1ad12 219 }
jah128 30:513457c1ad12 220
jah128 30:513457c1ad12 221 void set_program(int index)
jah128 30:513457c1ad12 222 {
jah128 30:513457c1ad12 223 main_program_state = index;
jah128 30:513457c1ad12 224 program_changed = 1;
jah128 30:513457c1ad12 225 program_run_init = 1;
jah128 30:513457c1ad12 226 strcpy(prog_info,"");
jah128 30:513457c1ad12 227 strcpy(prog_name,get_program_name(index));
jah128 30:513457c1ad12 228 }
jah128 30:513457c1ad12 229
jah128 30:513457c1ad12 230 void set_program_info(char * info)
jah128 30:513457c1ad12 231 {
jah128 30:513457c1ad12 232 strcpy(prog_info,info);
jah128 30:513457c1ad12 233 program_changed = 1;
jah128 30:513457c1ad12 234 }
jah128 30:513457c1ad12 235
jah128 30:513457c1ad12 236 void update_display()
jah128 30:513457c1ad12 237 {
jah128 30:513457c1ad12 238 if(program_changed == 1) {
jah128 30:513457c1ad12 239 program_changed = 0;
jah128 30:513457c1ad12 240 display.clear_display();
jah128 30:513457c1ad12 241
jah128 30:513457c1ad12 242 if(display_debug_inf==1) display_debug_info();
jah128 30:513457c1ad12 243 else {
jah128 30:513457c1ad12 244 display.set_position(0,0);
jah128 30:513457c1ad12 245 display.write_string(prog_name);
jah128 30:513457c1ad12 246 }
jah128 30:513457c1ad12 247 display.set_position(1,0);
jah128 30:513457c1ad12 248 display.write_string(prog_info);
jah128 30:513457c1ad12 249 }
jah128 30:513457c1ad12 250 }
jah128 30:513457c1ad12 251
jah128 30:513457c1ad12 252 void display_debug_info()
jah128 30:513457c1ad12 253 {
jah128 30:513457c1ad12 254 char disp_line[16] = "- - - - - - - -";
jah128 30:513457c1ad12 255 if(beacon_found==1)disp_line[0]='B';
jah128 30:513457c1ad12 256 for(int i=1; i<8; i++) {
jah128 30:513457c1ad12 257 if(robots_found[i])disp_line[((i)*2)]=48+i;
jah128 30:513457c1ad12 258 }
jah128 30:513457c1ad12 259 display.set_position(0,0);
jah128 30:513457c1ad12 260 display.write_string(disp_line);
jah128 30:513457c1ad12 261 }
jah128 30:513457c1ad12 262
jah128 30:513457c1ad12 263 /// Verbose output
jah128 30:513457c1ad12 264 void out(const char* format, ...)
jah128 30:513457c1ad12 265 {
jah128 30:513457c1ad12 266 char buffer[256];
jah128 30:513457c1ad12 267 if (debug_mode) {
jah128 30:513457c1ad12 268 va_list vl;
jah128 30:513457c1ad12 269 va_start(vl, format);
jah128 30:513457c1ad12 270 vsprintf(buffer,format,vl);
jah128 30:513457c1ad12 271 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 30:513457c1ad12 272 va_end(vl);
jah128 30:513457c1ad12 273 }
jah128 30:513457c1ad12 274 }
jah128 30:513457c1ad12 275
jah128 30:513457c1ad12 276
jah128 30:513457c1ad12 277 void bmeme_handle_user_serial_message(char * message, char length, char interface)
jah128 30:513457c1ad12 278 {
jah128 30:513457c1ad12 279 // This is where user code for handling a (non-system) serial message should go
jah128 30:513457c1ad12 280 //
jah128 30:513457c1ad12 281 // message = pointer to message char array
jah128 30:513457c1ad12 282 // length = length of message
jah128 30:513457c1ad12 283 // interface = 0 for PC serial connection, 1 for Bluetooth
jah128 30:513457c1ad12 284
jah128 30:513457c1ad12 285 if(interface) {
jah128 30:513457c1ad12 286 if(length == 8) {
jah128 30:513457c1ad12 287 for(int i = 0; i < length; i++) {
jah128 30:513457c1ad12 288 // Convert single byte value into a beacon heading in the range +/-180 degrees
jah128 30:513457c1ad12 289 float beacon_heading = message[i];
jah128 30:513457c1ad12 290 float degrees_per_value = 256.0f / 360.0f;
jah128 30:513457c1ad12 291
jah128 30:513457c1ad12 292 if(beacon_heading != 0)
jah128 30:513457c1ad12 293 beacon_heading /= degrees_per_value;
jah128 30:513457c1ad12 294
jah128 30:513457c1ad12 295 beacon_heading -= 180;
jah128 30:513457c1ad12 296
jah128 30:513457c1ad12 297 flocking_headings[i] = beacon_heading;
jah128 30:513457c1ad12 298
jah128 30:513457c1ad12 299 debug("%d, ", flocking_headings[i]);
jah128 30:513457c1ad12 300 //debug("%f, ", beacon_heading);
jah128 30:513457c1ad12 301 }
jah128 30:513457c1ad12 302
jah128 30:513457c1ad12 303 debug("\n");
jah128 30:513457c1ad12 304 }
jah128 30:513457c1ad12 305 }
jah128 30:513457c1ad12 306 }