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