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@9:085e090e1ec1, 2015-10-26 (annotated)

Committer:
jah128
Date:
Mon Oct 26 11:16:05 2015 +0000
Revision:
9:085e090e1ec1
Parent:
8:00558287a4ef
Child:
10:1b09d4bb847b
Fork for variable frequency beacon

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 6:ff3c66f7372b 13 /// Version 0.1
jah128 6:ff3c66f7372b 14 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 6:ff3c66f7372b 15 /// University of York
jah128 0:8a5497a2e366 16
jah128 9:085e090e1ec1 17
jah128 9:085e090e1ec1 18
jah128 1:f6356cf1cefc 19 /// Include psiswarm.h - this includes all the other necessary core files
jah128 9:085e090e1ec1 20
jah128 0:8a5497a2e366 21 #include "psiswarm.h"
jah128 0:8a5497a2e366 22
jah128 6:ff3c66f7372b 23 // IMPORTANT!!!
jah128 6:ff3c66f7372b 24 // Do not call the IR functions at all as they will interfere with the correct operation of this program
jah128 6:ff3c66f7372b 25 // Instead, use the values held in the variables below; they are updated every 500ms
jah128 6:ff3c66f7372b 26
jah128 9:085e090e1ec1 27
jah128 9:085e090e1ec1 28
jah128 6:ff3c66f7372b 29 char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window
jah128 6:ff3c66f7372b 30 int beacon_heading = 0; // This is the heading from the last time a beacon was detected
jah128 6:ff3c66f7372b 31 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 32 int robots_heading[8]; // These are the headings from the last time the respective robots were detected
jah128 6:ff3c66f7372b 33 unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected
jah128 6:ff3c66f7372b 34 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
jah128 6:ff3c66f7372b 35 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
jah128 6:ff3c66f7372b 36
jah128 6:ff3c66f7372b 37 char * program_name = "B-Meme";
jah128 4:25039ea5eb09 38 char * author_name = "YRL";
jah128 1:f6356cf1cefc 39 char * version_name = "1.0";
jah128 0:8a5497a2e366 40
jah128 6:ff3c66f7372b 41 char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code]
jah128 9:085e090e1ec1 42 char display_debug_inf = 1; // Set to 1 to show debug info on display
jah128 9:085e090e1ec1 43 char main_program_state;
jah128 9:085e090e1ec1 44 char old_program_state = 255;
jah128 9:085e090e1ec1 45 char step_cycle = 0;
jah128 9:085e090e1ec1 46 char success_count = 0;
jah128 9:085e090e1ec1 47 char target_reached = 0;
jah128 9:085e090e1ec1 48 char was_turning = 0;
jah128 9:085e090e1ec1 49 Ticker main_loop_ticker;
jah128 9:085e090e1ec1 50
jah128 9:085e090e1ec1 51
jah128 9:085e090e1ec1 52 ///This is the main loop for the Beautiful Meme code. The code block is run once every 500mS* once all the IR samples have been collected.
jah128 9:085e090e1ec1 53 void main_loop()
jah128 9:085e090e1ec1 54 {
jah128 9:085e090e1ec1 55 update_display();
jah128 9:085e090e1ec1 56 switch(main_program_state) {
jah128 9:085e090e1ec1 57 case 0: //Case 0 is the initial program: turn to face beacon
jah128 9:085e090e1ec1 58 if(step_cycle == 0) {
jah128 9:085e090e1ec1 59 char turn_status = turn_to_bearing(0);
jah128 9:085e090e1ec1 60 if(turn_status == 0) {
jah128 9:085e090e1ec1 61 success_count ++;
jah128 9:085e090e1ec1 62 if(success_count > 1) main_program_state = 1;
jah128 9:085e090e1ec1 63 } else success_count = 0;
jah128 9:085e090e1ec1 64 }
jah128 9:085e090e1ec1 65 break;
jah128 9:085e090e1ec1 66 case 1:
jah128 9:085e090e1ec1 67 target_reached = 0;
jah128 9:085e090e1ec1 68 head_to_bearing_program(0);
jah128 9:085e090e1ec1 69 if(target_reached == 1) main_program_state = 2;
jah128 9:085e090e1ec1 70 break;
jah128 9:085e090e1ec1 71 case 2:
jah128 9:085e090e1ec1 72 target_reached = 0;
jah128 9:085e090e1ec1 73 head_to_bearing_program(180);
jah128 9:085e090e1ec1 74 if(target_reached == 1) main_program_state = 1;
jah128 9:085e090e1ec1 75 break;
jah128 9:085e090e1ec1 76 }
jah128 9:085e090e1ec1 77 step_cycle=1-step_cycle;
jah128 9:085e090e1ec1 78 }
jah128 9:085e090e1ec1 79
jah128 9:085e090e1ec1 80
jah128 9:085e090e1ec1 81 //The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
jah128 9:085e090e1ec1 82 void head_to_bearing_program(int target_bearing)
jah128 9:085e090e1ec1 83 {
jah128 9:085e090e1ec1 84 if(step_cycle == 0 || was_turning == 0) {
jah128 9:085e090e1ec1 85 // Check if we are heading in the right bearing (+- 25 degrees)
jah128 9:085e090e1ec1 86 int current_bearing = (360 - beacon_heading) % 360;
jah128 9:085e090e1ec1 87 // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
jah128 9:085e090e1ec1 88 char bearing_ok = 0;
jah128 9:085e090e1ec1 89 int lower_bound = target_bearing - 25;
jah128 9:085e090e1ec1 90 int upper_bound = target_bearing + 25;
jah128 9:085e090e1ec1 91 if(lower_bound < 0) {
jah128 9:085e090e1ec1 92 if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
jah128 9:085e090e1ec1 93 } else if(upper_bound > 359) {
jah128 9:085e090e1ec1 94 if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
jah128 9:085e090e1ec1 95 } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
jah128 9:085e090e1ec1 96 // Check if there is an obstacle in front of us
jah128 9:085e090e1ec1 97 if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
jah128 9:085e090e1ec1 98 else {
jah128 9:085e090e1ec1 99 // Now move forward if we are facing correct bearing, otherwise turn
jah128 9:085e090e1ec1 100 if(bearing_ok == 1) {
jah128 9:085e090e1ec1 101 //Check if anything is in front of us to determine speed - if it is, move slowly
jah128 9:085e090e1ec1 102 int t_time = 6 * BEACON_PERIOD;
jah128 9:085e090e1ec1 103 float t_speed = 1.0;
jah128 9:085e090e1ec1 104 if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
jah128 9:085e090e1ec1 105 t_time = 4 * BEACON_PERIOD;
jah128 9:085e090e1ec1 106 t_speed = 0.6;
jah128 9:085e090e1ec1 107 }
jah128 9:085e090e1ec1 108 if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
jah128 9:085e090e1ec1 109 t_time = 3 * BEACON_PERIOD;
jah128 9:085e090e1ec1 110 t_speed = 0.4;
jah128 9:085e090e1ec1 111 }
jah128 9:085e090e1ec1 112 if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
jah128 9:085e090e1ec1 113 t_time = 2 * BEACON_PERIOD;
jah128 9:085e090e1ec1 114 t_speed = 0.2;
jah128 9:085e090e1ec1 115 }
jah128 9:085e090e1ec1 116 time_based_forward(t_speed,t_time,0);
jah128 9:085e090e1ec1 117 was_turning = 0;
jah128 9:085e090e1ec1 118 } else {
jah128 9:085e090e1ec1 119 turn_to_bearing(target_bearing);
jah128 9:085e090e1ec1 120 was_turning = 1;
jah128 9:085e090e1ec1 121 }
jah128 9:085e090e1ec1 122 }
jah128 9:085e090e1ec1 123 }
jah128 9:085e090e1ec1 124 }
jah128 0:8a5497a2e366 125
jah128 0:8a5497a2e366 126
jah128 1:f6356cf1cefc 127 ///Place user code here that should be run after initialisation but before the main loop
jah128 6:ff3c66f7372b 128 void user_code_setup()
jah128 6:ff3c66f7372b 129 {
jah128 9:085e090e1ec1 130 wait(0.8);
jah128 9:085e090e1ec1 131 display.clear_display();
jah128 9:085e090e1ec1 132 display.set_position(0,0);
jah128 9:085e090e1ec1 133 display.write_string("BEAUTIFUL MEME");
jah128 9:085e090e1ec1 134 display.set_position(1,0);
jah128 9:085e090e1ec1 135 display.write_string(" PROJECT");
jah128 9:085e090e1ec1 136 wait(0.2);
jah128 6:ff3c66f7372b 137 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 138 out("Beautiful Meme Project Demo Code \n");
jah128 6:ff3c66f7372b 139 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 140 locate_beacon();
jah128 6:ff3c66f7372b 141 while(beacon_found == 0) {
jah128 9:085e090e1ec1 142 wait(0.5);
jah128 6:ff3c66f7372b 143 locate_beacon();
jah128 6:ff3c66f7372b 144 }
jah128 6:ff3c66f7372b 145 start_infrared_timers();
jah128 9:085e090e1ec1 146 main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
jah128 9:085e090e1ec1 147 main_program_state = 1;
jah128 9:085e090e1ec1 148 set_leds(0x00,0x00);
jah128 9:085e090e1ec1 149 set_center_led(3,1);
jah128 9:085e090e1ec1 150 display.clear_display();
jah128 9:085e090e1ec1 151 display.set_position(0,0);
jah128 9:085e090e1ec1 152 display.write_string("BEACON FOUND AT");
jah128 9:085e090e1ec1 153 display.set_position(1,0);
jah128 9:085e090e1ec1 154 char degrees_string[16];
jah128 9:085e090e1ec1 155 sprintf(degrees_string,"%d DEGREES",beacon_heading);
jah128 9:085e090e1ec1 156 display.write_string(degrees_string);
jah128 0:8a5497a2e366 157 }
jah128 0:8a5497a2e366 158
jah128 1:f6356cf1cefc 159 /// Code goes here to handle what should happen when the user switch is pressed
jah128 0:8a5497a2e366 160 void handle_switch_event(char switch_state)
jah128 0:8a5497a2e366 161 {
jah128 6:ff3c66f7372b 162 /// 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 163 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 6:ff3c66f7372b 164
jah128 6:ff3c66f7372b 165 //pc.printf("User switch code block: %d\n",switch_state);
jah128 0:8a5497a2e366 166 }
jah128 0:8a5497a2e366 167
jah128 1:f6356cf1cefc 168 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
jah128 6:ff3c66f7372b 169 int main()
jah128 6:ff3c66f7372b 170 {
jah128 1:f6356cf1cefc 171 ///init() in psiswarm.cpp sets up the robot
jah128 0:8a5497a2e366 172 init();
jah128 0:8a5497a2e366 173 user_code_setup();
jah128 0:8a5497a2e366 174 user_code_running = 1;
jah128 0:8a5497a2e366 175 while(1) {
jah128 9:085e090e1ec1 176 wait(1);
jah128 6:ff3c66f7372b 177 }
jah128 6:ff3c66f7372b 178 }
jah128 6:ff3c66f7372b 179
jah128 9:085e090e1ec1 180
jah128 9:085e090e1ec1 181 void update_display()
jah128 9:085e090e1ec1 182 {
jah128 9:085e090e1ec1 183 if(main_program_state != old_program_state) {
jah128 9:085e090e1ec1 184 old_program_state = main_program_state;
jah128 9:085e090e1ec1 185 display.clear_display();
jah128 9:085e090e1ec1 186 display.set_position(0,0);
jah128 9:085e090e1ec1 187 switch(main_program_state) {
jah128 9:085e090e1ec1 188 case 0:
jah128 9:085e090e1ec1 189 display.write_string("P:FACE BEACON");
jah128 9:085e090e1ec1 190 break;
jah128 9:085e090e1ec1 191 case 1:
jah128 9:085e090e1ec1 192 display.write_string("P:HEAD TO BEACON");
jah128 9:085e090e1ec1 193 break;
jah128 9:085e090e1ec1 194 case 2:
jah128 9:085e090e1ec1 195 display.write_string("P:HEAD TO SOUTH");
jah128 9:085e090e1ec1 196 break;
jah128 9:085e090e1ec1 197 }
jah128 9:085e090e1ec1 198 }
jah128 9:085e090e1ec1 199 if(display_debug_inf==1) display_debug_info();
jah128 9:085e090e1ec1 200 }
jah128 9:085e090e1ec1 201
jah128 9:085e090e1ec1 202 void display_debug_info()
jah128 9:085e090e1ec1 203 {
jah128 9:085e090e1ec1 204 char disp_line[16] = "- - - - - - - -";
jah128 9:085e090e1ec1 205 if(beacon_found==1)disp_line[0]='B';
jah128 9:085e090e1ec1 206 for(int i=0; i<7; i++) {
jah128 9:085e090e1ec1 207 if(robots_found[i])disp_line[((i+1)*2)]=49+i;
jah128 9:085e090e1ec1 208 }
jah128 9:085e090e1ec1 209 display.set_position(1,0);
jah128 9:085e090e1ec1 210 display.write_string(disp_line);
jah128 9:085e090e1ec1 211 }
jah128 9:085e090e1ec1 212
jah128 6:ff3c66f7372b 213 /// Verbose output
jah128 6:ff3c66f7372b 214 void out(const char* format, ...)
jah128 6:ff3c66f7372b 215 {
jah128 6:ff3c66f7372b 216 char buffer[256];
jah128 6:ff3c66f7372b 217 if (debug_mode) {
jah128 6:ff3c66f7372b 218 va_list vl;
jah128 6:ff3c66f7372b 219 va_start(vl, format);
jah128 6:ff3c66f7372b 220 vsprintf(buffer,format,vl);
jah128 6:ff3c66f7372b 221 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 6:ff3c66f7372b 222 va_end(vl);
jah128 0:8a5497a2e366 223 }
jah128 0:8a5497a2e366 224 }