James Hilder / Mbed 2 deprecated PsiSwarm_V41

Psi Swarm Code V0.41 [With Beautiful Meme program]

Dependencies:   PsiSwarmLibrary mbed

Fork of BeautifulMemeProjectBT by Alan Millard

BeautifulMeme/programs.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?

UserRevisionLine numberNew 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 // programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
jah128 30:513457c1ad12 7
jah128 30:513457c1ad12 8 #include "bmeme.h"
jah128 30:513457c1ad12 9
jah128 30:513457c1ad12 10 int obstacle_avoidance_threshold = 300;
jah128 30:513457c1ad12 11 int robot_avoidance_threshold = 2000;
jah128 30:513457c1ad12 12
jah128 30:513457c1ad12 13 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 14 /// head_to_bearing_program
jah128 30:513457c1ad12 15 char was_turning = 0;
jah128 30:513457c1ad12 16
jah128 30:513457c1ad12 17 ///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 30:513457c1ad12 18 void head_to_bearing_program(int target_bearing)
jah128 30:513457c1ad12 19 {
jah128 30:513457c1ad12 20 if(step_cycle == 0 || was_turning == 0) {
jah128 30:513457c1ad12 21 // Check if we are heading in the right bearing (+- 25 degrees)
jah128 30:513457c1ad12 22 int current_bearing = (360 - beacon_heading) % 360;
jah128 30:513457c1ad12 23 // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
jah128 30:513457c1ad12 24 char bearing_ok = 0;
jah128 30:513457c1ad12 25 int lower_bound = target_bearing - 25;
jah128 30:513457c1ad12 26 int upper_bound = target_bearing + 25;
jah128 30:513457c1ad12 27 if(lower_bound < 0) {
jah128 30:513457c1ad12 28 if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
jah128 30:513457c1ad12 29 } else if(upper_bound > 359) {
jah128 30:513457c1ad12 30 if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
jah128 30:513457c1ad12 31 } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
jah128 30:513457c1ad12 32 // Check if there is an obstacle in front of us
jah128 30:513457c1ad12 33 if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
jah128 30:513457c1ad12 34 else {
jah128 30:513457c1ad12 35 // Now move forward if we are facing correct bearing, otherwise turn
jah128 30:513457c1ad12 36 if(bearing_ok == 1) {
jah128 30:513457c1ad12 37 //Check if anything is in front of us to determine speed - if it is, move slowly
jah128 30:513457c1ad12 38 int t_time = 6 * BEACON_PERIOD;
jah128 30:513457c1ad12 39 float t_speed = 1.0;
jah128 30:513457c1ad12 40 if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
jah128 30:513457c1ad12 41 t_time = 4 * BEACON_PERIOD;
jah128 30:513457c1ad12 42 t_speed = 0.6;
jah128 30:513457c1ad12 43 }
jah128 30:513457c1ad12 44 if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
jah128 30:513457c1ad12 45 t_time = 3 * BEACON_PERIOD;
jah128 30:513457c1ad12 46 t_speed = 0.4;
jah128 30:513457c1ad12 47 }
jah128 30:513457c1ad12 48 if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
jah128 30:513457c1ad12 49 t_time = 2 * BEACON_PERIOD;
jah128 30:513457c1ad12 50 t_speed = 0.2;
jah128 30:513457c1ad12 51 }
jah128 30:513457c1ad12 52 time_based_forward(t_speed,t_time,0);
jah128 30:513457c1ad12 53 was_turning = 0;
jah128 30:513457c1ad12 54 } else {
jah128 30:513457c1ad12 55 turn_to_bearing(target_bearing);
jah128 30:513457c1ad12 56 was_turning = 1;
jah128 30:513457c1ad12 57 }
jah128 30:513457c1ad12 58 }
jah128 30:513457c1ad12 59 }
jah128 30:513457c1ad12 60 }
jah128 30:513457c1ad12 61
jah128 30:513457c1ad12 62
jah128 30:513457c1ad12 63
jah128 30:513457c1ad12 64
jah128 30:513457c1ad12 65 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 66 /// recharging_program
jah128 30:513457c1ad12 67
jah128 30:513457c1ad12 68 char recharge_power_check_count = 0;
jah128 30:513457c1ad12 69 char battery_low_count = 0;
jah128 30:513457c1ad12 70
jah128 30:513457c1ad12 71 void recharging_program()
jah128 30:513457c1ad12 72 {
jah128 30:513457c1ad12 73 switch(recharging_state) {
jah128 30:513457c1ad12 74 case 0:
jah128 30:513457c1ad12 75 // We are not currently recharging, check the battery state
jah128 30:513457c1ad12 76 if(get_battery_voltage() < battery_low_threshold) {
jah128 30:513457c1ad12 77 // Battery is below low threshold
jah128 30:513457c1ad12 78 battery_low_count ++;
jah128 30:513457c1ad12 79 // We don't immediately start recharging routine in case as battery level can fluctuate a little due to load; if we get a low value for 4 continuous timesteps we start recharging routine
jah128 30:513457c1ad12 80 if(battery_low_count > 3) {
jah128 30:513457c1ad12 81 // Set recharging state to 'looking for charger'
jah128 30:513457c1ad12 82 recharging_state = 1;
jah128 30:513457c1ad12 83 strcpy(prog_name,"CHARGING PROGRAM");
jah128 30:513457c1ad12 84 set_program_info("HEAD TO BEACON");
jah128 30:513457c1ad12 85 }
jah128 30:513457c1ad12 86 } else battery_low_count = 0;
jah128 30:513457c1ad12 87 break;
jah128 30:513457c1ad12 88 // State 1: Head to beacon [as this is where battery charger is]
jah128 30:513457c1ad12 89 case 1:
jah128 30:513457c1ad12 90 target_reached = 0;
jah128 30:513457c1ad12 91 head_to_bearing_program(0);
jah128 30:513457c1ad12 92 if(target_reached == 1) {
jah128 30:513457c1ad12 93 recharging_state = 2;
jah128 30:513457c1ad12 94 set_program_info("TURN 90 DEGREES");
jah128 30:513457c1ad12 95 }
jah128 30:513457c1ad12 96 break;
jah128 30:513457c1ad12 97 // Stage 2: Turn 90 degrees to align with charging pads
jah128 30:513457c1ad12 98 case 2:
jah128 30:513457c1ad12 99 disable_ir_emitters = 1;
jah128 30:513457c1ad12 100 time_based_turn_degrees(0.8, 70.0, 1);
jah128 30:513457c1ad12 101 recharge_power_check_count = 0;
jah128 30:513457c1ad12 102 recharging_state = 3;
jah128 30:513457c1ad12 103 break;
jah128 30:513457c1ad12 104 // Stage 3: Wait for turn to complete
jah128 30:513457c1ad12 105 case 3:
jah128 30:513457c1ad12 106 if (time_based_motor_action != 1) {
jah128 30:513457c1ad12 107 recharging_state = 4;
jah128 30:513457c1ad12 108 set_program_info("CHECK FOR POWER");
jah128 30:513457c1ad12 109 }
jah128 30:513457c1ad12 110 break;
jah128 30:513457c1ad12 111 // Stage 4: Check if charging
jah128 30:513457c1ad12 112 case 4:
jah128 30:513457c1ad12 113 recharge_power_check_count++;
jah128 30:513457c1ad12 114 if(get_dc_status() == 1) {
jah128 30:513457c1ad12 115 recharging_state = 5;
jah128 30:513457c1ad12 116 } else {
jah128 30:513457c1ad12 117 if(recharge_power_check_count < 10)recharging_state = 6;
jah128 30:513457c1ad12 118 else {
jah128 30:513457c1ad12 119 recharging_state = 7;
jah128 30:513457c1ad12 120 set_program_info("NO POWER - RETRY");
jah128 30:513457c1ad12 121 }
jah128 30:513457c1ad12 122 }
jah128 30:513457c1ad12 123 break;
jah128 30:513457c1ad12 124 // Stage 5: Charging. Wait until threshold voltage exceeded
jah128 30:513457c1ad12 125 case 5:
jah128 30:513457c1ad12 126 if(get_battery_voltage() > battery_high_threshold) {
jah128 30:513457c1ad12 127 set_program_info("LEAVE CHARGER");
jah128 30:513457c1ad12 128 recharging_state = 7;
jah128 30:513457c1ad12 129 } else {
jah128 30:513457c1ad12 130 char b_voltage[16];
jah128 30:513457c1ad12 131 sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
jah128 30:513457c1ad12 132 set_program_info(b_voltage);
jah128 30:513457c1ad12 133 }
jah128 30:513457c1ad12 134 break;
jah128 30:513457c1ad12 135 // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck
jah128 30:513457c1ad12 136 case 6:
jah128 30:513457c1ad12 137 time_based_turn_degrees(0.5,4,1);
jah128 30:513457c1ad12 138 recharging_state = 4;
jah128 30:513457c1ad12 139 break;
jah128 30:513457c1ad12 140 // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program
jah128 30:513457c1ad12 141 case 7:
jah128 30:513457c1ad12 142 time_based_turn_degrees(0.8, 90.0, 1);
jah128 30:513457c1ad12 143 recharging_state = 8;
jah128 30:513457c1ad12 144 break;
jah128 30:513457c1ad12 145
jah128 30:513457c1ad12 146 // Stage 8: Wait for turn to complete
jah128 30:513457c1ad12 147 case 8:
jah128 30:513457c1ad12 148 if (time_based_motor_action != 1) recharging_state = 9;
jah128 30:513457c1ad12 149 break;
jah128 30:513457c1ad12 150 // Stage 9: Move away
jah128 30:513457c1ad12 151 case 9:
jah128 30:513457c1ad12 152 time_based_forward(0.5, 1000000, 1);
jah128 30:513457c1ad12 153 recharging_state = 10;
jah128 30:513457c1ad12 154 break;
jah128 30:513457c1ad12 155 // Stage 10: Wait for move to complete
jah128 30:513457c1ad12 156 case 10:
jah128 30:513457c1ad12 157 if (time_based_motor_action != 1) recharging_state = 11;
jah128 30:513457c1ad12 158 break;
jah128 30:513457c1ad12 159 // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle
jah128 30:513457c1ad12 160 case 11:
jah128 30:513457c1ad12 161 disable_ir_emitters = 0;
jah128 30:513457c1ad12 162 if (get_battery_voltage() < battery_low_threshold) {
jah128 30:513457c1ad12 163 recharging_state = 1;
jah128 30:513457c1ad12 164 } else {
jah128 30:513457c1ad12 165 recharging_state = 0;
jah128 30:513457c1ad12 166 //Restore name of old program on display
jah128 30:513457c1ad12 167 set_program(main_program_state);
jah128 30:513457c1ad12 168 }
jah128 30:513457c1ad12 169 break;
jah128 30:513457c1ad12 170 }
jah128 30:513457c1ad12 171 }
jah128 30:513457c1ad12 172
jah128 30:513457c1ad12 173
jah128 30:513457c1ad12 174
jah128 30:513457c1ad12 175
jah128 30:513457c1ad12 176 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 177 /// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program)
jah128 30:513457c1ad12 178
jah128 30:513457c1ad12 179 enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
jah128 30:513457c1ad12 180 enum random_walk_state internal_state = random_walk;
jah128 30:513457c1ad12 181 char action_timeout = 0;
jah128 30:513457c1ad12 182 char interaction_timeout = 0;
jah128 30:513457c1ad12 183 char random_walk_timeout = 0;
jah128 30:513457c1ad12 184 float previous_left_motor_speed = 0.5;
jah128 30:513457c1ad12 185 float previous_right_motor_speed = 0.5;
jah128 30:513457c1ad12 186
jah128 30:513457c1ad12 187 void curved_random_walk_with_interaction_program()
jah128 30:513457c1ad12 188 {
jah128 30:513457c1ad12 189 if(internal_state == random_walk) {
jah128 30:513457c1ad12 190 if(interaction_timeout < 4)
jah128 30:513457c1ad12 191 interaction_timeout++;
jah128 30:513457c1ad12 192
jah128 30:513457c1ad12 193 int closest_robot = -1;
jah128 30:513457c1ad12 194 unsigned short shortest_distance = 0;
jah128 30:513457c1ad12 195
jah128 30:513457c1ad12 196 // Check whether there are any other robots within range
jah128 30:513457c1ad12 197 for(int i = 0; i < 8; i++) {
jah128 30:513457c1ad12 198 if(robots_found[i]) {
jah128 30:513457c1ad12 199 if(robots_distance[i] > shortest_distance) {
jah128 30:513457c1ad12 200 shortest_distance = robots_distance[i];
jah128 30:513457c1ad12 201 closest_robot = i;
jah128 30:513457c1ad12 202 }
jah128 30:513457c1ad12 203 }
jah128 30:513457c1ad12 204 }
jah128 30:513457c1ad12 205
jah128 30:513457c1ad12 206 // Turn towards the closest robot
jah128 30:513457c1ad12 207 if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
jah128 30:513457c1ad12 208 time_based_turn_degrees(1, robots_heading[closest_robot], 1);
jah128 30:513457c1ad12 209
jah128 30:513457c1ad12 210 action_timeout = 0;
jah128 30:513457c1ad12 211 internal_state = turn_towards;
jah128 30:513457c1ad12 212 char temp_message[17];
jah128 30:513457c1ad12 213 sprintf(temp_message,"FACE ROBOT %d",closest_robot);
jah128 30:513457c1ad12 214 set_program_info(temp_message);
jah128 30:513457c1ad12 215 } else { // Otherwise, do a random walk
jah128 30:513457c1ad12 216 // Check the front sensors for obstacles
jah128 30:513457c1ad12 217 if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 218 reflected_sensor_data[1] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 219 reflected_sensor_data[6] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 220 reflected_sensor_data[7] > obstacle_avoidance_threshold) {
jah128 30:513457c1ad12 221 // Ignore the rear sensors when calculating the heading
jah128 30:513457c1ad12 222 reflected_sensor_data[2] = 0;
jah128 30:513457c1ad12 223 reflected_sensor_data[3] = 0;
jah128 30:513457c1ad12 224 reflected_sensor_data[4] = 0;
jah128 30:513457c1ad12 225 reflected_sensor_data[5] = 0;
jah128 30:513457c1ad12 226
jah128 30:513457c1ad12 227 // Turn 180 degrees away from the sensed obstacle
jah128 30:513457c1ad12 228 int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
jah128 30:513457c1ad12 229
jah128 30:513457c1ad12 230 // Normalise
jah128 30:513457c1ad12 231 heading %= 360;
jah128 30:513457c1ad12 232
jah128 30:513457c1ad12 233 if(heading < -180)
jah128 30:513457c1ad12 234 heading += 360;
jah128 30:513457c1ad12 235
jah128 30:513457c1ad12 236 if(heading > 180)
jah128 30:513457c1ad12 237 heading -= 360;
jah128 30:513457c1ad12 238 set_program_info("AVOID OBSTACLE");
jah128 30:513457c1ad12 239 time_based_turn_degrees(1, heading, 1);
jah128 30:513457c1ad12 240
jah128 30:513457c1ad12 241 action_timeout = 0;
jah128 30:513457c1ad12 242 internal_state = turn_away;
jah128 30:513457c1ad12 243 } else {
jah128 30:513457c1ad12 244 // Change motor speeds every 1s
jah128 30:513457c1ad12 245 if(random_walk_timeout >= 2) {
jah128 30:513457c1ad12 246 float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
jah128 30:513457c1ad12 247
jah128 30:513457c1ad12 248 float left_motor_speed = previous_left_motor_speed - random_offset;
jah128 30:513457c1ad12 249 float right_motor_speed = previous_right_motor_speed + random_offset;
jah128 30:513457c1ad12 250
jah128 30:513457c1ad12 251 float threshold = 0.25;
jah128 30:513457c1ad12 252
jah128 30:513457c1ad12 253 if(left_motor_speed < threshold)
jah128 30:513457c1ad12 254 left_motor_speed = threshold;
jah128 30:513457c1ad12 255
jah128 30:513457c1ad12 256 if(right_motor_speed < threshold)
jah128 30:513457c1ad12 257 right_motor_speed = threshold;
jah128 30:513457c1ad12 258
jah128 30:513457c1ad12 259 set_left_motor_speed(left_motor_speed);
jah128 30:513457c1ad12 260 set_right_motor_speed(right_motor_speed);
jah128 30:513457c1ad12 261
jah128 30:513457c1ad12 262 random_walk_timeout = 0;
jah128 30:513457c1ad12 263 }
jah128 30:513457c1ad12 264
jah128 30:513457c1ad12 265 random_walk_timeout++;
jah128 30:513457c1ad12 266 }
jah128 30:513457c1ad12 267 }
jah128 30:513457c1ad12 268 } else if(internal_state == turn_towards) {
jah128 30:513457c1ad12 269 if(action_timeout < 4)
jah128 30:513457c1ad12 270 action_timeout++;
jah128 30:513457c1ad12 271 else {
jah128 30:513457c1ad12 272 set_program_info("SAY HELLO");
jah128 30:513457c1ad12 273 vibrate();
jah128 30:513457c1ad12 274
jah128 30:513457c1ad12 275 action_timeout = 0;
jah128 30:513457c1ad12 276 internal_state = interact;
jah128 30:513457c1ad12 277 }
jah128 30:513457c1ad12 278 } else if(internal_state == interact) {
jah128 30:513457c1ad12 279 if(action_timeout < 4)
jah128 30:513457c1ad12 280 action_timeout++;
jah128 30:513457c1ad12 281 else {
jah128 30:513457c1ad12 282 set_program_info("TURN AROUND");
jah128 30:513457c1ad12 283 time_based_turn_degrees(1, 180, 1);
jah128 30:513457c1ad12 284
jah128 30:513457c1ad12 285 action_timeout = 0;
jah128 30:513457c1ad12 286 internal_state = turn_away;
jah128 30:513457c1ad12 287 }
jah128 30:513457c1ad12 288
jah128 30:513457c1ad12 289 } else if(internal_state == turn_away) {
jah128 30:513457c1ad12 290 if(action_timeout < 4)
jah128 30:513457c1ad12 291 action_timeout++;
jah128 30:513457c1ad12 292 else {
jah128 30:513457c1ad12 293 set_program_info("RANDOM WALK");
jah128 30:513457c1ad12 294 interaction_timeout = 0;
jah128 30:513457c1ad12 295 internal_state = random_walk;
jah128 30:513457c1ad12 296 }
jah128 30:513457c1ad12 297 } else if(internal_state == avoid_obstacle) {
jah128 30:513457c1ad12 298 if(action_timeout < 4)
jah128 30:513457c1ad12 299 action_timeout++;
jah128 30:513457c1ad12 300 else
jah128 30:513457c1ad12 301 set_program_info("RANDOM WALK");
jah128 30:513457c1ad12 302 internal_state = random_walk;
jah128 30:513457c1ad12 303 }
jah128 30:513457c1ad12 304 }
jah128 30:513457c1ad12 305
jah128 30:513457c1ad12 306
jah128 30:513457c1ad12 307
jah128 30:513457c1ad12 308
jah128 30:513457c1ad12 309
jah128 30:513457c1ad12 310 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 311 /// straight_random_walk_with_interaction_program
jah128 30:513457c1ad12 312
jah128 30:513457c1ad12 313 void straight_random_walk_with_interaction_program()
jah128 30:513457c1ad12 314 {
jah128 30:513457c1ad12 315
jah128 30:513457c1ad12 316 }
jah128 30:513457c1ad12 317
jah128 30:513457c1ad12 318
jah128 30:513457c1ad12 319
jah128 30:513457c1ad12 320
jah128 30:513457c1ad12 321
jah128 30:513457c1ad12 322 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 323 /// find_space_program
jah128 30:513457c1ad12 324
jah128 30:513457c1ad12 325 char prog_debug = 1 ;
jah128 30:513457c1ad12 326 float target_wheel_speed;
jah128 30:513457c1ad12 327 int random_walk_bearing;
jah128 30:513457c1ad12 328
jah128 30:513457c1ad12 329 void find_space_program(char bidirectional)
jah128 30:513457c1ad12 330 {
jah128 30:513457c1ad12 331 // The find_space_program is a continuous turn-move vector program
jah128 30:513457c1ad12 332
jah128 30:513457c1ad12 333 if(program_run_init == 1) {
jah128 30:513457c1ad12 334 // Setup the LEDs to red
jah128 30:513457c1ad12 335 set_leds(0x00,0xFF);
jah128 30:513457c1ad12 336 set_center_led(1,1);
jah128 30:513457c1ad12 337 program_run_init = 0;
jah128 30:513457c1ad12 338 random_walk_bearing = rand() % 360;
jah128 30:513457c1ad12 339 }
jah128 30:513457c1ad12 340
jah128 30:513457c1ad12 341 // When step_cycle = 0 we calculate a vector to move to and a target distance
jah128 30:513457c1ad12 342 if(step_cycle == 0) {
jah128 30:513457c1ad12 343 struct FloatVector target_vector;
jah128 30:513457c1ad12 344 target_vector.angle = 0;
jah128 30:513457c1ad12 345 target_vector.distance = 0;
jah128 30:513457c1ad12 346 // Check for near robots within range
jah128 30:513457c1ad12 347 for(int i = 1; i < 8; i++) {
jah128 30:513457c1ad12 348 if(robots_found[i]) {
jah128 30:513457c1ad12 349 int res_bearing = robots_heading[i];
jah128 30:513457c1ad12 350 res_bearing += 180;
jah128 30:513457c1ad12 351 if(res_bearing > 180) res_bearing -= 360;
jah128 30:513457c1ad12 352 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
jah128 30:513457c1ad12 353 if(prog_debug) out("Repelled from robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 30:513457c1ad12 354 }
jah128 30:513457c1ad12 355 }
jah128 30:513457c1ad12 356 if(target_vector.angle!=0){
jah128 30:513457c1ad12 357 set_leds(0xFF,0xFF);
jah128 30:513457c1ad12 358 set_center_led(3,1);
jah128 30:513457c1ad12 359 }
jah128 30:513457c1ad12 360 // Check for obstacles
jah128 30:513457c1ad12 361 char obstacle = 0;
jah128 30:513457c1ad12 362 int peak_strength = 0;
jah128 30:513457c1ad12 363 for(int i=0; i<8; i++){
jah128 30:513457c1ad12 364 if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
jah128 30:513457c1ad12 365 if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
jah128 30:513457c1ad12 366 }
jah128 30:513457c1ad12 367 if(obstacle){
jah128 30:513457c1ad12 368 //Choose new random walk bearing
jah128 30:513457c1ad12 369 set_leds(0x00,0xFF);
jah128 30:513457c1ad12 370 set_center_led(1,1);
jah128 30:513457c1ad12 371 random_walk_bearing = rand() % 360;
jah128 30:513457c1ad12 372 int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
jah128 30:513457c1ad12 373 int obs_bearing = obstacle_bearing + 180;
jah128 30:513457c1ad12 374 if(obs_bearing > 180) obs_bearing -= 360;
jah128 30:513457c1ad12 375 target_vector = addVector(target_vector,obs_bearing,peak_strength);
jah128 30:513457c1ad12 376 if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d, resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
jah128 30:513457c1ad12 377 }
jah128 30:513457c1ad12 378
jah128 30:513457c1ad12 379 if(target_vector.angle == 0 && target_vector.distance == 0){
jah128 30:513457c1ad12 380 set_leds(0xFF,0x00);
jah128 30:513457c1ad12 381 set_center_led(2,1);
jah128 30:513457c1ad12 382 random_walk_bearing += 180;
jah128 30:513457c1ad12 383 if(random_walk_bearing > 360) random_walk_bearing -= 360;
jah128 30:513457c1ad12 384 target_vector.distance = 100;
jah128 30:513457c1ad12 385 int current_bearing = 360 - beacon_heading;
jah128 30:513457c1ad12 386 //Now work out turn needed to face intended heading
jah128 30:513457c1ad12 387 int target_turn = (random_walk_bearing - current_bearing) % 360;
jah128 30:513457c1ad12 388 if(target_turn > 180) target_turn -= 360;
jah128 30:513457c1ad12 389 if(target_turn < -180) target_turn += 360;
jah128 30:513457c1ad12 390 target_vector.angle = target_turn;
jah128 30:513457c1ad12 391 if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
jah128 30:513457c1ad12 392 }
jah128 30:513457c1ad12 393 char wheel_direction = 1;
jah128 30:513457c1ad12 394 if(bidirectional){
jah128 30:513457c1ad12 395 // Allow reverse wheel direction
jah128 30:513457c1ad12 396 if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
jah128 30:513457c1ad12 397 else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
jah128 30:513457c1ad12 398 }
jah128 30:513457c1ad12 399 //Now turn to angle
jah128 30:513457c1ad12 400 float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
jah128 30:513457c1ad12 401 if(target_vector.angle < 0){
jah128 30:513457c1ad12 402 if(target_vector.angle < -maximum_turn_angle){
jah128 30:513457c1ad12 403 target_vector.angle = -maximum_turn_angle;
jah128 30:513457c1ad12 404 target_vector.distance = 100;
jah128 30:513457c1ad12 405 }
jah128 30:513457c1ad12 406 }else{
jah128 30:513457c1ad12 407 if(target_vector.angle > maximum_turn_angle){
jah128 30:513457c1ad12 408 target_vector.angle = maximum_turn_angle;
jah128 30:513457c1ad12 409 target_vector.distance = 100;
jah128 30:513457c1ad12 410 }
jah128 30:513457c1ad12 411 }
jah128 30:513457c1ad12 412 //Set the wheel speed for next action
jah128 30:513457c1ad12 413 if(target_vector.distance < 120) target_wheel_speed = 0.25;
jah128 30:513457c1ad12 414 else if(target_vector.distance < 240) target_wheel_speed = 0.35;
jah128 30:513457c1ad12 415 else if(target_vector.distance < 480) target_wheel_speed = 0.45;
jah128 30:513457c1ad12 416 else if(target_vector.distance < 960) target_wheel_speed = 0.55;
jah128 30:513457c1ad12 417 else target_wheel_speed = 0.65;
jah128 30:513457c1ad12 418 if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
jah128 30:513457c1ad12 419
jah128 30:513457c1ad12 420 //Now turn...
jah128 30:513457c1ad12 421 time_based_turn_degrees(1, (int) target_vector.angle, 1);
jah128 30:513457c1ad12 422 } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0);
jah128 30:513457c1ad12 423 }
jah128 30:513457c1ad12 424
jah128 30:513457c1ad12 425
jah128 30:513457c1ad12 426
jah128 30:513457c1ad12 427
jah128 30:513457c1ad12 428
jah128 30:513457c1ad12 429 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 430 /// clustering_program
jah128 30:513457c1ad12 431
jah128 30:513457c1ad12 432
jah128 30:513457c1ad12 433 void clustering_program(char invert, char bidirectional)
jah128 30:513457c1ad12 434 {
jah128 30:513457c1ad12 435 // The clustering program is a continuous turn-move vector program
jah128 30:513457c1ad12 436 // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots
jah128 30:513457c1ad12 437 // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group
jah128 30:513457c1ad12 438
jah128 30:513457c1ad12 439 // Store the robot group: even robots (0) are green, odd robots (1) are red
jah128 30:513457c1ad12 440 char group = robot_id % 2;
jah128 30:513457c1ad12 441
jah128 30:513457c1ad12 442 if(program_run_init == 1) {
jah128 30:513457c1ad12 443 // Setup the LEDs based on robot_id
jah128 30:513457c1ad12 444 if(group == 0) {
jah128 30:513457c1ad12 445 set_leds(0xFF,0x00);
jah128 30:513457c1ad12 446 set_center_led(2,1);
jah128 30:513457c1ad12 447 } else {
jah128 30:513457c1ad12 448 set_leds(0x00,0xFF);
jah128 30:513457c1ad12 449 set_center_led(1,1);
jah128 30:513457c1ad12 450 }
jah128 30:513457c1ad12 451 program_run_init = 0;
jah128 30:513457c1ad12 452 random_walk_bearing = rand() % 360;
jah128 30:513457c1ad12 453 }
jah128 30:513457c1ad12 454
jah128 30:513457c1ad12 455 // When step_cycle = 0 we calculate a vector to move to and a target distance
jah128 30:513457c1ad12 456 if(step_cycle == 0) {
jah128 30:513457c1ad12 457 char avoiding_friend = 0;
jah128 30:513457c1ad12 458 struct FloatVector target_vector;
jah128 30:513457c1ad12 459 target_vector.angle = 0;
jah128 30:513457c1ad12 460 target_vector.distance = 0;
jah128 30:513457c1ad12 461 // Check for near robots within range
jah128 30:513457c1ad12 462 for(int i = 1; i < 8; i++) {
jah128 30:513457c1ad12 463 if(robots_found[i]) {
jah128 30:513457c1ad12 464 // Determine if the robot is an attractor or a repellor
jah128 30:513457c1ad12 465 char attract = 0;
jah128 30:513457c1ad12 466 if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1;
jah128 30:513457c1ad12 467 // Avoid very close attractors to stop collisions
jah128 30:513457c1ad12 468 if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;}
jah128 30:513457c1ad12 469 int res_bearing = robots_heading[i];
jah128 30:513457c1ad12 470 if(attract==0){
jah128 30:513457c1ad12 471 res_bearing += 180;
jah128 30:513457c1ad12 472 if(res_bearing > 180) res_bearing -= 360;
jah128 30:513457c1ad12 473 }
jah128 30:513457c1ad12 474 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
jah128 30:513457c1ad12 475 if(prog_debug) {
jah128 30:513457c1ad12 476 if(attract) {
jah128 30:513457c1ad12 477 out("Attracted to robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 30:513457c1ad12 478 } else {
jah128 30:513457c1ad12 479 out("Repelled from robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 30:513457c1ad12 480 }
jah128 30:513457c1ad12 481 }
jah128 30:513457c1ad12 482 }
jah128 30:513457c1ad12 483 }
jah128 30:513457c1ad12 484
jah128 30:513457c1ad12 485 // Check for obstacles
jah128 30:513457c1ad12 486 char obstacle = 0;
jah128 30:513457c1ad12 487 int peak_strength = 0;
jah128 30:513457c1ad12 488 for(int i=0; i<8; i++){
jah128 30:513457c1ad12 489 if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
jah128 30:513457c1ad12 490 if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
jah128 30:513457c1ad12 491 }
jah128 30:513457c1ad12 492 if(obstacle){
jah128 30:513457c1ad12 493 //Choose new random walk bearing
jah128 30:513457c1ad12 494 random_walk_bearing = rand() % 360;
jah128 30:513457c1ad12 495 int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
jah128 30:513457c1ad12 496 int obs_bearing = obstacle_bearing + 180;
jah128 30:513457c1ad12 497 if(obs_bearing > 180) obs_bearing -= 360;
jah128 30:513457c1ad12 498 target_vector = addVector(target_vector,obs_bearing,peak_strength);
jah128 30:513457c1ad12 499 if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d, resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
jah128 30:513457c1ad12 500 }
jah128 30:513457c1ad12 501 if(target_vector.angle == 0 && target_vector.distance == 0){
jah128 30:513457c1ad12 502 //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing
jah128 30:513457c1ad12 503 if(rand() % 100 > 97) random_walk_bearing = rand() % 360;
jah128 30:513457c1ad12 504 target_vector.distance = 100;
jah128 30:513457c1ad12 505 int current_bearing = 360 - beacon_heading;
jah128 30:513457c1ad12 506 //Now work out turn needed to face intended heading
jah128 30:513457c1ad12 507 int target_turn = (random_walk_bearing - current_bearing) % 360;
jah128 30:513457c1ad12 508 if(target_turn > 180) target_turn -= 360;
jah128 30:513457c1ad12 509 if(target_turn < -180) target_turn += 360;
jah128 30:513457c1ad12 510 target_vector.angle = target_turn;
jah128 30:513457c1ad12 511 if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
jah128 30:513457c1ad12 512 }
jah128 30:513457c1ad12 513 char wheel_direction = 1;
jah128 30:513457c1ad12 514 if(bidirectional){
jah128 30:513457c1ad12 515 // Allow reverse wheel direction
jah128 30:513457c1ad12 516 if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
jah128 30:513457c1ad12 517 else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
jah128 30:513457c1ad12 518 }
jah128 30:513457c1ad12 519 //Now turn to angle
jah128 30:513457c1ad12 520 float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
jah128 30:513457c1ad12 521 if(target_vector.angle < 0){
jah128 30:513457c1ad12 522 if(target_vector.angle < -maximum_turn_angle){
jah128 30:513457c1ad12 523 target_vector.angle = -maximum_turn_angle;
jah128 30:513457c1ad12 524 target_vector.distance = 100;
jah128 30:513457c1ad12 525 }
jah128 30:513457c1ad12 526 }else{
jah128 30:513457c1ad12 527 if(target_vector.angle > maximum_turn_angle){
jah128 30:513457c1ad12 528 target_vector.angle = maximum_turn_angle;
jah128 30:513457c1ad12 529 target_vector.distance = 100;
jah128 30:513457c1ad12 530 }
jah128 30:513457c1ad12 531 }
jah128 30:513457c1ad12 532 if(avoiding_friend) target_vector.distance = 100;
jah128 30:513457c1ad12 533 //Set the wheel speed for next action
jah128 30:513457c1ad12 534 if(target_vector.distance < 120) target_wheel_speed = 0.25;
jah128 30:513457c1ad12 535 else if(target_vector.distance < 240) target_wheel_speed = 0.35;
jah128 30:513457c1ad12 536 else if(target_vector.distance < 480) target_wheel_speed = 0.5;
jah128 30:513457c1ad12 537 else if(target_vector.distance < 960) target_wheel_speed = 0.65;
jah128 30:513457c1ad12 538 else target_wheel_speed = 0.85;
jah128 30:513457c1ad12 539 if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
jah128 30:513457c1ad12 540
jah128 30:513457c1ad12 541 //Now turn...
jah128 30:513457c1ad12 542 time_based_turn_degrees(1, (int) target_vector.angle, 1);
jah128 30:513457c1ad12 543 } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0);
jah128 30:513457c1ad12 544 }
jah128 30:513457c1ad12 545
jah128 30:513457c1ad12 546
jah128 30:513457c1ad12 547 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 548 /// stop_program - Pauses robot
jah128 30:513457c1ad12 549
jah128 30:513457c1ad12 550 void stop_program()
jah128 30:513457c1ad12 551 {
jah128 30:513457c1ad12 552 if(program_run_init == 1) {
jah128 30:513457c1ad12 553 save_led_states();
jah128 30:513457c1ad12 554 set_leds(0,0);
jah128 30:513457c1ad12 555 set_center_led(0,0);
jah128 30:513457c1ad12 556 stop();
jah128 30:513457c1ad12 557 program_run_init = 0;
jah128 30:513457c1ad12 558 }
jah128 30:513457c1ad12 559 }
jah128 30:513457c1ad12 560
jah128 30:513457c1ad12 561
jah128 30:513457c1ad12 562
jah128 30:513457c1ad12 563 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 564 /// tag_game_program
jah128 30:513457c1ad12 565
jah128 30:513457c1ad12 566 enum tag_game_role {hunter, hunted};
jah128 30:513457c1ad12 567 enum tag_game_role role;
jah128 30:513457c1ad12 568 int tag_distance = 500;
jah128 30:513457c1ad12 569 char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters
jah128 30:513457c1ad12 570 Timeout hunter_timeout; // Timeout before robots switch back to being hunted
jah128 30:513457c1ad12 571 char initial_hunter = 2; // Robot with this ID is permanently a hunter
jah128 30:513457c1ad12 572
jah128 30:513457c1ad12 573 // Resets hunter robots to hunted after timeout
jah128 30:513457c1ad12 574 void role_reset()
jah128 30:513457c1ad12 575 {
jah128 30:513457c1ad12 576 role = hunted;
jah128 30:513457c1ad12 577 set_program_info("HUNTED");
jah128 30:513457c1ad12 578 // Clear memory of hunters
jah128 30:513457c1ad12 579 for(int i = 0; i < 8; i++)
jah128 30:513457c1ad12 580 hunters[i] = 0;
jah128 30:513457c1ad12 581 }
jah128 30:513457c1ad12 582
jah128 30:513457c1ad12 583 void tag_game_program()
jah128 30:513457c1ad12 584 {
jah128 30:513457c1ad12 585 // Initialisation
jah128 30:513457c1ad12 586 if(program_run_init == 1) {
jah128 30:513457c1ad12 587
jah128 30:513457c1ad12 588 // Set robot roles
jah128 30:513457c1ad12 589 if(robot_id == initial_hunter){
jah128 30:513457c1ad12 590 role = hunter;
jah128 30:513457c1ad12 591 set_program_info("FIRST HUNTER");
jah128 30:513457c1ad12 592 }
jah128 30:513457c1ad12 593 else {
jah128 30:513457c1ad12 594 role = hunted;
jah128 30:513457c1ad12 595 set_program_info("HUNTED");
jah128 30:513457c1ad12 596 }
jah128 30:513457c1ad12 597 // Clear memory of hunters
jah128 30:513457c1ad12 598 for(int i = 0; i < 8; i++)
jah128 30:513457c1ad12 599 hunters[i] = 0;
jah128 30:513457c1ad12 600
jah128 30:513457c1ad12 601 program_run_init = 0;
jah128 30:513457c1ad12 602 }
jah128 30:513457c1ad12 603
jah128 30:513457c1ad12 604 // Bias forward movement
jah128 30:513457c1ad12 605 float left_motor_speed = 0.5;
jah128 30:513457c1ad12 606 float right_motor_speed = 0.5;
jah128 30:513457c1ad12 607
jah128 30:513457c1ad12 608 // Check the front sensors for obstacles
jah128 30:513457c1ad12 609 if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 610 reflected_sensor_data[1] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 611 reflected_sensor_data[6] > obstacle_avoidance_threshold ||
jah128 30:513457c1ad12 612 reflected_sensor_data[7] > obstacle_avoidance_threshold)
jah128 30:513457c1ad12 613 {
jah128 30:513457c1ad12 614 // Ignore the rear sensors when calculating the heading
jah128 30:513457c1ad12 615 reflected_sensor_data[2] = 0;
jah128 30:513457c1ad12 616 reflected_sensor_data[3] = 0;
jah128 30:513457c1ad12 617 reflected_sensor_data[4] = 0;
jah128 30:513457c1ad12 618 reflected_sensor_data[5] = 0;
jah128 30:513457c1ad12 619
jah128 30:513457c1ad12 620 // Get heading of sensed obstacle
jah128 30:513457c1ad12 621 int heading = get_bearing_from_ir_array(reflected_sensor_data);
jah128 30:513457c1ad12 622
jah128 30:513457c1ad12 623 // Turn in opposite direction
jah128 30:513457c1ad12 624 if(heading > 0 && heading < 90)
jah128 30:513457c1ad12 625 {
jah128 30:513457c1ad12 626 left_motor_speed -= 0.75;
jah128 30:513457c1ad12 627 right_motor_speed += 0.5;
jah128 30:513457c1ad12 628 }
jah128 30:513457c1ad12 629 else if(heading < 0 && heading > -90)
jah128 30:513457c1ad12 630 {
jah128 30:513457c1ad12 631 left_motor_speed += 0.5;
jah128 30:513457c1ad12 632 right_motor_speed -= 0.75;
jah128 30:513457c1ad12 633 }
jah128 30:513457c1ad12 634
jah128 30:513457c1ad12 635 set_left_motor_speed(left_motor_speed);
jah128 30:513457c1ad12 636 set_right_motor_speed(right_motor_speed);
jah128 30:513457c1ad12 637
jah128 30:513457c1ad12 638 // Return early - obstacle avoidance is the top priority
jah128 30:513457c1ad12 639 return;
jah128 30:513457c1ad12 640 }
jah128 30:513457c1ad12 641
jah128 30:513457c1ad12 642 int closest_robot = -1;
jah128 30:513457c1ad12 643 unsigned short shortest_distance = 0;
jah128 30:513457c1ad12 644
jah128 30:513457c1ad12 645 // Check whether there are any other robots within range
jah128 30:513457c1ad12 646 for(int i = 0; i < 8; i++)
jah128 30:513457c1ad12 647 {
jah128 30:513457c1ad12 648 if(robots_found[i])
jah128 30:513457c1ad12 649 {
jah128 30:513457c1ad12 650 if(robots_distance[i] > shortest_distance)
jah128 30:513457c1ad12 651 {
jah128 30:513457c1ad12 652 shortest_distance = robots_distance[i];
jah128 30:513457c1ad12 653 closest_robot = i;
jah128 30:513457c1ad12 654 }
jah128 30:513457c1ad12 655 }
jah128 30:513457c1ad12 656 }
jah128 30:513457c1ad12 657
jah128 30:513457c1ad12 658 // If the closest robot is within tag distance, this robot has been tagged
jah128 30:513457c1ad12 659 if(shortest_distance > tag_distance)
jah128 30:513457c1ad12 660 {
jah128 30:513457c1ad12 661 // Switch role to hunter
jah128 30:513457c1ad12 662 if(role == hunted)
jah128 30:513457c1ad12 663 {
jah128 30:513457c1ad12 664 role = hunter;
jah128 30:513457c1ad12 665 set_program_info("NEW HUNTER");
jah128 30:513457c1ad12 666 // Reset to hunted after 10 seconds
jah128 30:513457c1ad12 667 hunter_timeout.attach(&role_reset, 10);
jah128 30:513457c1ad12 668 }
jah128 30:513457c1ad12 669
jah128 30:513457c1ad12 670 // Keep a record of which robot tagged them, so hunters do not chase hunters
jah128 30:513457c1ad12 671 // Unless they are the initial hunter, who is aggresive towards everyone
jah128 30:513457c1ad12 672 if(robot_id != initial_hunter)
jah128 30:513457c1ad12 673 hunters[closest_robot] = 1;
jah128 30:513457c1ad12 674 }
jah128 30:513457c1ad12 675
jah128 30:513457c1ad12 676 if(role == hunter)
jah128 30:513457c1ad12 677 {
jah128 30:513457c1ad12 678 // Set LEDS to red
jah128 30:513457c1ad12 679 set_leds(0x00, 0xFF);
jah128 30:513457c1ad12 680 set_center_led(1, 1);
jah128 30:513457c1ad12 681
jah128 30:513457c1ad12 682 if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters
jah128 30:513457c1ad12 683 {
jah128 30:513457c1ad12 684 // Turn towards closest hunted robot (unless it is straight ahead, or behind)
jah128 30:513457c1ad12 685 if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90)
jah128 30:513457c1ad12 686 {
jah128 30:513457c1ad12 687 left_motor_speed += 0.5;
jah128 30:513457c1ad12 688 right_motor_speed -= 0.5;
jah128 30:513457c1ad12 689 }
jah128 30:513457c1ad12 690 else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90)
jah128 30:513457c1ad12 691 {
jah128 30:513457c1ad12 692 left_motor_speed -= 0.5;
jah128 30:513457c1ad12 693 right_motor_speed += 0.5;
jah128 30:513457c1ad12 694 }
jah128 30:513457c1ad12 695 }
jah128 30:513457c1ad12 696
jah128 30:513457c1ad12 697 set_left_motor_speed(left_motor_speed);
jah128 30:513457c1ad12 698 set_right_motor_speed(right_motor_speed);
jah128 30:513457c1ad12 699 }
jah128 30:513457c1ad12 700 else // role == hunted
jah128 30:513457c1ad12 701 {
jah128 30:513457c1ad12 702 // Set LEDs to green
jah128 30:513457c1ad12 703 set_leds(0xFF, 0x00);
jah128 30:513457c1ad12 704 set_center_led(2, 1);
jah128 30:513457c1ad12 705
jah128 30:513457c1ad12 706 // Avoid everyone
jah128 30:513457c1ad12 707 if(closest_robot >= 0)
jah128 30:513457c1ad12 708 {
jah128 30:513457c1ad12 709 // Turn away from closest robot
jah128 30:513457c1ad12 710 if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90)
jah128 30:513457c1ad12 711 {
jah128 30:513457c1ad12 712 left_motor_speed -= 0.5;
jah128 30:513457c1ad12 713 right_motor_speed += 0.5;
jah128 30:513457c1ad12 714 }
jah128 30:513457c1ad12 715 else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90)
jah128 30:513457c1ad12 716 {
jah128 30:513457c1ad12 717 left_motor_speed += 0.5;
jah128 30:513457c1ad12 718 right_motor_speed -= 0.5;
jah128 30:513457c1ad12 719 }
jah128 30:513457c1ad12 720 }
jah128 30:513457c1ad12 721
jah128 30:513457c1ad12 722 set_left_motor_speed(left_motor_speed);
jah128 30:513457c1ad12 723 set_right_motor_speed(right_motor_speed);
jah128 30:513457c1ad12 724 }
jah128 30:513457c1ad12 725 }
jah128 30:513457c1ad12 726
jah128 30:513457c1ad12 727 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 728 /// flocking_program
jah128 30:513457c1ad12 729
jah128 30:513457c1ad12 730 void flocking_program()
jah128 30:513457c1ad12 731 {
jah128 30:513457c1ad12 732 char display_line[16] = " ";
jah128 30:513457c1ad12 733 int average_heading = 0;
jah128 30:513457c1ad12 734 int number_of_neighbours = 0;
jah128 30:513457c1ad12 735 int number_of_flocking_headings = 0;
jah128 30:513457c1ad12 736 int target_heading = 0;
jah128 30:513457c1ad12 737 int angles = 0;
jah128 30:513457c1ad12 738 int ir_sensor_threshold = 600;
jah128 30:513457c1ad12 739 int sensors_activated = 0;
jah128 30:513457c1ad12 740
jah128 30:513457c1ad12 741 struct FloatVector obstacle_vector;
jah128 30:513457c1ad12 742 obstacle_vector.angle = 0;
jah128 30:513457c1ad12 743 obstacle_vector.distance = 0;
jah128 30:513457c1ad12 744
jah128 30:513457c1ad12 745 struct FloatVector cohesion_vector;
jah128 30:513457c1ad12 746 cohesion_vector.angle = 0;
jah128 30:513457c1ad12 747 cohesion_vector.distance = 0;
jah128 30:513457c1ad12 748
jah128 30:513457c1ad12 749 for(int i = 0; i < 8; i++)
jah128 30:513457c1ad12 750 {
jah128 30:513457c1ad12 751 if(reflected_sensor_data[i] > ir_sensor_threshold)
jah128 30:513457c1ad12 752 {
jah128 30:513457c1ad12 753 obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
jah128 30:513457c1ad12 754 sensors_activated++;
jah128 30:513457c1ad12 755 }
jah128 30:513457c1ad12 756
jah128 30:513457c1ad12 757 if(robots_found[i])
jah128 30:513457c1ad12 758 {
jah128 30:513457c1ad12 759 // -180 degrees is reserved for "no byte received"
jah128 30:513457c1ad12 760 if(flocking_headings[i] != -180)
jah128 30:513457c1ad12 761 {
jah128 30:513457c1ad12 762 average_heading += flocking_headings[i];
jah128 30:513457c1ad12 763 number_of_flocking_headings++;
jah128 30:513457c1ad12 764 }
jah128 30:513457c1ad12 765
jah128 30:513457c1ad12 766 cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
jah128 30:513457c1ad12 767 number_of_neighbours++;
jah128 30:513457c1ad12 768 }
jah128 30:513457c1ad12 769 }
jah128 30:513457c1ad12 770
jah128 30:513457c1ad12 771 cohesion_vector.distance /= number_of_neighbours; // Normalise
jah128 30:513457c1ad12 772 obstacle_vector.distance /= sensors_activated; // Normalise
jah128 30:513457c1ad12 773
jah128 30:513457c1ad12 774 int obstacle_avoidance_angle;
jah128 30:513457c1ad12 775
jah128 30:513457c1ad12 776 if(sensors_activated > 0)
jah128 30:513457c1ad12 777 {
jah128 30:513457c1ad12 778 obstacle_avoidance_angle = obstacle_vector.angle + 180;
jah128 30:513457c1ad12 779
jah128 30:513457c1ad12 780 if(obstacle_avoidance_angle > 180)
jah128 30:513457c1ad12 781 obstacle_avoidance_angle -= 360;
jah128 30:513457c1ad12 782 if(obstacle_avoidance_angle < -180)
jah128 30:513457c1ad12 783 obstacle_avoidance_angle += 360;
jah128 30:513457c1ad12 784
jah128 30:513457c1ad12 785 target_heading += obstacle_avoidance_angle;
jah128 30:513457c1ad12 786 angles++;
jah128 30:513457c1ad12 787 }
jah128 30:513457c1ad12 788
jah128 30:513457c1ad12 789 int cohesion_angle;
jah128 30:513457c1ad12 790
jah128 30:513457c1ad12 791 // Don't bother performing cohesion if robots are already close enough
jah128 30:513457c1ad12 792 if(number_of_neighbours > 0 && cohesion_vector.distance < 200)
jah128 30:513457c1ad12 793 {
jah128 30:513457c1ad12 794 cohesion_angle = cohesion_vector.angle;
jah128 30:513457c1ad12 795
jah128 30:513457c1ad12 796 if(cohesion_angle > 180)
jah128 30:513457c1ad12 797 cohesion_angle -= 360;
jah128 30:513457c1ad12 798 if(cohesion_angle < -180)
jah128 30:513457c1ad12 799 cohesion_angle += 360;
jah128 30:513457c1ad12 800
jah128 30:513457c1ad12 801 target_heading += cohesion_angle;
jah128 30:513457c1ad12 802 angles++;
jah128 30:513457c1ad12 803 }
jah128 30:513457c1ad12 804
jah128 30:513457c1ad12 805 int relative_flocking_heading;
jah128 30:513457c1ad12 806
jah128 30:513457c1ad12 807 if(number_of_flocking_headings > 0)
jah128 30:513457c1ad12 808 {
jah128 30:513457c1ad12 809 average_heading /= number_of_flocking_headings;
jah128 30:513457c1ad12 810
jah128 30:513457c1ad12 811 relative_flocking_heading = beacon_heading - average_heading;
jah128 30:513457c1ad12 812
jah128 30:513457c1ad12 813 if(relative_flocking_heading > 180)
jah128 30:513457c1ad12 814 relative_flocking_heading -= 360;
jah128 30:513457c1ad12 815 if(relative_flocking_heading < -180)
jah128 30:513457c1ad12 816 relative_flocking_heading += 360;
jah128 30:513457c1ad12 817
jah128 30:513457c1ad12 818 target_heading += relative_flocking_heading;
jah128 30:513457c1ad12 819 angles++;
jah128 30:513457c1ad12 820 }
jah128 30:513457c1ad12 821
jah128 30:513457c1ad12 822 if(angles > 0)
jah128 30:513457c1ad12 823 target_heading /= angles; // Calculate average
jah128 30:513457c1ad12 824
jah128 30:513457c1ad12 825 float left_motor_speed = 0.25;
jah128 30:513457c1ad12 826 float right_motor_speed = 0.25;
jah128 30:513457c1ad12 827
jah128 30:513457c1ad12 828 if(target_heading > 22.5)
jah128 30:513457c1ad12 829 right_motor_speed *= -1;
jah128 30:513457c1ad12 830 else if(target_heading < -22.5)
jah128 30:513457c1ad12 831 left_motor_speed *= -1;
jah128 30:513457c1ad12 832
jah128 30:513457c1ad12 833 set_left_motor_speed(left_motor_speed);
jah128 30:513457c1ad12 834 set_right_motor_speed(right_motor_speed);
jah128 30:513457c1ad12 835
jah128 30:513457c1ad12 836 // Only transmit beacon heading if the beacon is visible
jah128 30:513457c1ad12 837 if(beacon_found)
jah128 30:513457c1ad12 838 {
jah128 30:513457c1ad12 839 float degrees_per_value = 256.0f / 360.0f;
jah128 30:513457c1ad12 840 char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value
jah128 30:513457c1ad12 841
jah128 30:513457c1ad12 842 bt.putc(beacon_heading_byte);
jah128 30:513457c1ad12 843 }
jah128 30:513457c1ad12 844
jah128 30:513457c1ad12 845 sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading);
jah128 30:513457c1ad12 846
jah128 30:513457c1ad12 847 set_program_info(display_line);
jah128 30:513457c1ad12 848 }
jah128 30:513457c1ad12 849
jah128 30:513457c1ad12 850 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 30:513457c1ad12 851 /// generic_program - Framework for building typical programs
jah128 30:513457c1ad12 852
jah128 30:513457c1ad12 853 void generic_program()
jah128 30:513457c1ad12 854 {
jah128 30:513457c1ad12 855 // Do something on the first run of a program
jah128 30:513457c1ad12 856 if(program_run_init == 1) {
jah128 30:513457c1ad12 857 // Initialisation code goes here...
jah128 30:513457c1ad12 858
jah128 30:513457c1ad12 859 program_run_init = 0;
jah128 30:513457c1ad12 860 }
jah128 30:513457c1ad12 861
jah128 30:513457c1ad12 862 // step_cycle is either zero or one; use this avoid estimating bearings on the cycle after a turn (as the results will be skewed by the turn)
jah128 30:513457c1ad12 863 if(step_cycle == 0) {
jah128 30:513457c1ad12 864 // Do something based on sensor data (eg turn)
jah128 30:513457c1ad12 865 } else {
jah128 30:513457c1ad12 866 // Do something ignoring sensor data (eg move, or nothing!)
jah128 30:513457c1ad12 867 }
jah128 30:513457c1ad12 868
jah128 30:513457c1ad12 869 }