Example code for the UKESF Headstart robotics lab; code to make the PsiSwarm robot move forward and turn.
Dependencies: PsiSwarm-Headstart mbed
Fork of PsiSwarm_V41 by
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?
User | Revision | Line number | New 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 | } |