![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of BeautifulMemeProjectBT by
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?
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 | 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 | } |