Science Memeseum / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCodeNew by James O'Keeffe

main.cpp@10:1b09d4bb847b, 2015-10-26 (annotated)

Committer:
jah128
Date:
Mon Oct 26 23:58:08 2015 +0000
Revision:
10:1b09d4bb847b
Parent:
9:085e090e1ec1
Child:
11:7b3ee540ba56
Reordered files; updated display

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 10:1b09d4bb847b 13 /// Version 0.2
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 10:1b09d4bb847b 22 char * version_name = "0.2";
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 10:1b09d4bb847b 36 char default_normal_program = 3; // The program to run on turn on (after 'face beacon' program)
jah128 10:1b09d4bb847b 37 char use_recharging_program = 1; // 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 10:1b09d4bb847b 41 char program_changed = 0; // Flag to update display when program is changed
jah128 10:1b09d4bb847b 42 char success_count = 0; // Flag to indicate the success of a program
jah128 10:1b09d4bb847b 43 char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps
jah128 10:1b09d4bb847b 44 char target_reached = 0; // Flag to indicate if a program target has been reached
jah128 10:1b09d4bb847b 45 char prog_name [17]; // Stores the name of the running program [line 0 on the display]
jah128 10:1b09d4bb847b 46 char prog_info [17]; // Stores information about the current state of the program [line 1 on the display]
jah128 10:1b09d4bb847b 47 char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!]
jah128 10:1b09d4bb847b 48 char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running)
jah128 9:085e090e1ec1 49 Ticker main_loop_ticker;
jah128 9:085e090e1ec1 50
jah128 10:1b09d4bb847b 51 ///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 52 void main_loop()
jah128 9:085e090e1ec1 53 {
jah128 10:1b09d4bb847b 54 if(use_recharging_program == 1)recharging_program();
jah128 9:085e090e1ec1 55 update_display();
jah128 10:1b09d4bb847b 56 if(recharging_state == 0) {
jah128 10:1b09d4bb847b 57 switch(main_program_state) {
jah128 10:1b09d4bb847b 58 case 0: //Case 0 is the initial program: turn to face beacon
jah128 10:1b09d4bb847b 59 if(step_cycle == 0) {
jah128 10:1b09d4bb847b 60 char turn_status = turn_to_bearing(0);
jah128 10:1b09d4bb847b 61 if(turn_status == 0) {
jah128 10:1b09d4bb847b 62 success_count ++;
jah128 10:1b09d4bb847b 63 if(success_count > 1) set_program(default_normal_program);
jah128 10:1b09d4bb847b 64 } else success_count = 0;
jah128 10:1b09d4bb847b 65 }
jah128 10:1b09d4bb847b 66 break;
jah128 10:1b09d4bb847b 67 case 1:
jah128 10:1b09d4bb847b 68 target_reached = 0;
jah128 10:1b09d4bb847b 69 head_to_bearing_program(0);
jah128 10:1b09d4bb847b 70 if(target_reached == 1) set_program(2);
jah128 10:1b09d4bb847b 71 break;
jah128 10:1b09d4bb847b 72 case 2:
jah128 10:1b09d4bb847b 73 target_reached = 0;
jah128 10:1b09d4bb847b 74 head_to_bearing_program(180);
jah128 10:1b09d4bb847b 75 if(target_reached == 1) set_program(1);
jah128 10:1b09d4bb847b 76 break;
jah128 10:1b09d4bb847b 77 case 3:
jah128 10:1b09d4bb847b 78 curved_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 79 break;
jah128 10:1b09d4bb847b 80 case 4:
jah128 10:1b09d4bb847b 81 straight_random_walk_with_interaction_program();
jah128 10:1b09d4bb847b 82 break;
jah128 10:1b09d4bb847b 83 case 5:
jah128 10:1b09d4bb847b 84 find_space_program();
jah128 10:1b09d4bb847b 85 break;
jah128 10:1b09d4bb847b 86 }
jah128 9:085e090e1ec1 87 }
jah128 9:085e090e1ec1 88 step_cycle=1-step_cycle;
jah128 9:085e090e1ec1 89 }
jah128 9:085e090e1ec1 90
jah128 1:f6356cf1cefc 91 ///Place user code here that should be run after initialisation but before the main loop
jah128 6:ff3c66f7372b 92 void user_code_setup()
jah128 6:ff3c66f7372b 93 {
jah128 9:085e090e1ec1 94 wait(0.8);
jah128 9:085e090e1ec1 95 display.clear_display();
jah128 9:085e090e1ec1 96 display.set_position(0,0);
jah128 9:085e090e1ec1 97 display.write_string("BEAUTIFUL MEME");
jah128 9:085e090e1ec1 98 display.set_position(1,0);
jah128 9:085e090e1ec1 99 display.write_string(" PROJECT");
jah128 9:085e090e1ec1 100 wait(0.2);
jah128 6:ff3c66f7372b 101 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 102 out("Beautiful Meme Project Demo Code \n");
jah128 6:ff3c66f7372b 103 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 104 locate_beacon();
jah128 6:ff3c66f7372b 105 while(beacon_found == 0) {
jah128 9:085e090e1ec1 106 wait(0.5);
jah128 6:ff3c66f7372b 107 locate_beacon();
jah128 6:ff3c66f7372b 108 }
jah128 6:ff3c66f7372b 109 start_infrared_timers();
jah128 9:085e090e1ec1 110 main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
jah128 10:1b09d4bb847b 111 set_program(0);
jah128 9:085e090e1ec1 112 set_leds(0x00,0x00);
jah128 10:1b09d4bb847b 113 set_center_led(3,0.5);
jah128 9:085e090e1ec1 114 display.clear_display();
jah128 9:085e090e1ec1 115 display.set_position(0,0);
jah128 9:085e090e1ec1 116 display.write_string("BEACON FOUND AT");
jah128 9:085e090e1ec1 117 display.set_position(1,0);
jah128 9:085e090e1ec1 118 char degrees_string[16];
jah128 9:085e090e1ec1 119 sprintf(degrees_string,"%d DEGREES",beacon_heading);
jah128 9:085e090e1ec1 120 display.write_string(degrees_string);
jah128 0:8a5497a2e366 121 }
jah128 0:8a5497a2e366 122
jah128 1:f6356cf1cefc 123 /// Code goes here to handle what should happen when the user switch is pressed
jah128 0:8a5497a2e366 124 void handle_switch_event(char switch_state)
jah128 0:8a5497a2e366 125 {
jah128 6:ff3c66f7372b 126 /// 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 127 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 6:ff3c66f7372b 128
jah128 6:ff3c66f7372b 129 //pc.printf("User switch code block: %d\n",switch_state);
jah128 0:8a5497a2e366 130 }
jah128 0:8a5497a2e366 131
jah128 1:f6356cf1cefc 132 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
jah128 6:ff3c66f7372b 133 int main()
jah128 6:ff3c66f7372b 134 {
jah128 1:f6356cf1cefc 135 ///init() in psiswarm.cpp sets up the robot
jah128 0:8a5497a2e366 136 init();
jah128 0:8a5497a2e366 137 user_code_setup();
jah128 0:8a5497a2e366 138 user_code_running = 1;
jah128 0:8a5497a2e366 139 while(1) {
jah128 9:085e090e1ec1 140 wait(1);
jah128 6:ff3c66f7372b 141 }
jah128 6:ff3c66f7372b 142 }
jah128 6:ff3c66f7372b 143
jah128 10:1b09d4bb847b 144 char * get_program_name(int index)
jah128 10:1b09d4bb847b 145 {
jah128 10:1b09d4bb847b 146 char * ret_name = new char[17];
jah128 10:1b09d4bb847b 147 switch(index){
jah128 10:1b09d4bb847b 148 case 0:
jah128 10:1b09d4bb847b 149 strcpy(ret_name,"FACE BEACON");
jah128 10:1b09d4bb847b 150 break;
jah128 10:1b09d4bb847b 151 case 1:
jah128 10:1b09d4bb847b 152 strcpy(ret_name,"HEAD TO BEACON");
jah128 10:1b09d4bb847b 153 break;
jah128 10:1b09d4bb847b 154 case 2:
jah128 10:1b09d4bb847b 155 strcpy(ret_name,"HEAD TO SOUTH");
jah128 10:1b09d4bb847b 156 break;
jah128 10:1b09d4bb847b 157 case 3:
jah128 10:1b09d4bb847b 158 strcpy(ret_name,"RANDOM WALK 1");
jah128 10:1b09d4bb847b 159 break;
jah128 10:1b09d4bb847b 160 case 4:
jah128 10:1b09d4bb847b 161 strcpy(ret_name,"RANDOM WALK 2");
jah128 10:1b09d4bb847b 162 break;
jah128 10:1b09d4bb847b 163 case 5:
jah128 10:1b09d4bb847b 164 strcpy(ret_name,"FIND SPACE");
jah128 10:1b09d4bb847b 165 break;
jah128 10:1b09d4bb847b 166 }
jah128 10:1b09d4bb847b 167 return ret_name;
jah128 10:1b09d4bb847b 168 }
jah128 10:1b09d4bb847b 169
jah128 10:1b09d4bb847b 170 void set_program(int index)
jah128 10:1b09d4bb847b 171 {
jah128 10:1b09d4bb847b 172 main_program_state = index;
jah128 10:1b09d4bb847b 173 program_changed = 1;
jah128 10:1b09d4bb847b 174 strcpy(prog_info,"");
jah128 10:1b09d4bb847b 175 strcpy(prog_name,get_program_name(index));
jah128 10:1b09d4bb847b 176 }
jah128 10:1b09d4bb847b 177
jah128 10:1b09d4bb847b 178 void set_program_info(char * info)
jah128 10:1b09d4bb847b 179 {
jah128 10:1b09d4bb847b 180 strcpy(prog_info,info);
jah128 10:1b09d4bb847b 181 program_changed = 1;
jah128 10:1b09d4bb847b 182 }
jah128 9:085e090e1ec1 183
jah128 9:085e090e1ec1 184 void update_display()
jah128 9:085e090e1ec1 185 {
jah128 10:1b09d4bb847b 186 if(program_changed == 1) {
jah128 10:1b09d4bb847b 187 program_changed = 0;
jah128 9:085e090e1ec1 188 display.clear_display();
jah128 10:1b09d4bb847b 189
jah128 10:1b09d4bb847b 190 if(display_debug_inf==1) display_debug_info();
jah128 10:1b09d4bb847b 191 else{
jah128 10:1b09d4bb847b 192 display.set_position(0,0);
jah128 10:1b09d4bb847b 193 display.write_string(prog_name);
jah128 9:085e090e1ec1 194 }
jah128 10:1b09d4bb847b 195 display.set_position(1,0);
jah128 10:1b09d4bb847b 196 display.write_string(prog_info);
jah128 9:085e090e1ec1 197 }
jah128 9:085e090e1ec1 198 }
jah128 9:085e090e1ec1 199
jah128 9:085e090e1ec1 200 void display_debug_info()
jah128 9:085e090e1ec1 201 {
jah128 9:085e090e1ec1 202 char disp_line[16] = "- - - - - - - -";
jah128 9:085e090e1ec1 203 if(beacon_found==1)disp_line[0]='B';
jah128 10:1b09d4bb847b 204 for(int i=1; i<8; i++) {
jah128 10:1b09d4bb847b 205 if(robots_found[i])disp_line[((i)*2)]=48+i;
jah128 9:085e090e1ec1 206 }
jah128 10:1b09d4bb847b 207 display.set_position(0,0);
jah128 9:085e090e1ec1 208 display.write_string(disp_line);
jah128 9:085e090e1ec1 209 }
jah128 9:085e090e1ec1 210
jah128 6:ff3c66f7372b 211 /// Verbose output
jah128 6:ff3c66f7372b 212 void out(const char* format, ...)
jah128 6:ff3c66f7372b 213 {
jah128 6:ff3c66f7372b 214 char buffer[256];
jah128 6:ff3c66f7372b 215 if (debug_mode) {
jah128 6:ff3c66f7372b 216 va_list vl;
jah128 6:ff3c66f7372b 217 va_start(vl, format);
jah128 6:ff3c66f7372b 218 vsprintf(buffer,format,vl);
jah128 6:ff3c66f7372b 219 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 6:ff3c66f7372b 220 va_end(vl);
jah128 0:8a5497a2e366 221 }
jah128 0:8a5497a2e366 222 }