Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

programs.cpp@17:da524989b637, 2016-01-07 (annotated)

Committer:
alanmillard
Date:
Thu Jan 07 17:58:07 2016 +0000
Revision:
17:da524989b637
Parent:
16:976a1d0ea897
Child:
18:5921c1853e8a
Now stitching two successive partial user messages together, if they are received within 0.1s of each other.

Who changed what in which revision?

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