James Hilder
Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of
BeautifulMemeProjectBT
by 
Alan Millard
programs.cpp@25:b5742873e02b, 2016-01-25 (annotated)
- Committer:
- alanmillard
- Date:
- Mon Jan 25 15:58:58 2016 +0000
- Revision:
- 25:b5742873e02b
- Parent:
- 24:31707fec20a5
- Child:
- 26:3869ae5eddd7
Continuous motion version of flocking. Obstacle avoidance needs tweaking.
Who changed what in which revision?
| User | Revision | Line number | New 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 | void flocking_program() |
| alanmillard | 24:31707fec20a5 | 731 | { |
| alanmillard | 22:fd75afdadfb2 | 732 | int average_heading = 0; |
| alanmillard | 22:fd75afdadfb2 | 733 | int number_of_neighbours = 0; |
| alanmillard | 22:fd75afdadfb2 | 734 | int number_of_flocking_headings = 0; |
| alanmillard | 17:da524989b637 | 735 | |
| alanmillard | 22:fd75afdadfb2 | 736 | struct FloatVector cohesion_vector; |
| alanmillard | 22:fd75afdadfb2 | 737 | cohesion_vector.angle = 0; |
| alanmillard | 22:fd75afdadfb2 | 738 | cohesion_vector.distance = 0; |
| alanmillard | 22:fd75afdadfb2 | 739 | |
| alanmillard | 25:b5742873e02b | 740 | // out("flocking_headings: "); |
| alanmillard | 25:b5742873e02b | 741 | // |
| alanmillard | 25:b5742873e02b | 742 | // for(int i = 0; i < 8; i++) |
| alanmillard | 25:b5742873e02b | 743 | // { |
| alanmillard | 25:b5742873e02b | 744 | // out("%d, ", flocking_headings[i]); |
| alanmillard | 25:b5742873e02b | 745 | // } |
| alanmillard | 25:b5742873e02b | 746 | // |
| alanmillard | 25:b5742873e02b | 747 | // out("\n"); |
| alanmillard | 25:b5742873e02b | 748 | |
| alanmillard | 22:fd75afdadfb2 | 749 | for(int i = 0; i < 8; i++) |
| alanmillard | 22:fd75afdadfb2 | 750 | { |
| alanmillard | 22:fd75afdadfb2 | 751 | if(robots_found[i]) |
| alanmillard | 22:fd75afdadfb2 | 752 | { |
| alanmillard | 22:fd75afdadfb2 | 753 | // -180 degrees is reserved for "no byte received" |
| alanmillard | 22:fd75afdadfb2 | 754 | if(flocking_headings[i] != -180) |
| alanmillard | 21:e5ab8c56a769 | 755 | { |
| alanmillard | 21:e5ab8c56a769 | 756 | average_heading += flocking_headings[i]; |
| alanmillard | 22:fd75afdadfb2 | 757 | number_of_flocking_headings++; |
| alanmillard | 22:fd75afdadfb2 | 758 | } |
| alanmillard | 22:fd75afdadfb2 | 759 | |
| alanmillard | 22:fd75afdadfb2 | 760 | cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]); |
| alanmillard | 22:fd75afdadfb2 | 761 | number_of_neighbours++; |
| alanmillard | 22:fd75afdadfb2 | 762 | } |
| alanmillard | 22:fd75afdadfb2 | 763 | } |
| alanmillard | 22:fd75afdadfb2 | 764 | |
| alanmillard | 25:b5742873e02b | 765 | // out("number_of_flocking_headings: %d\n", number_of_flocking_headings); |
| alanmillard | 25:b5742873e02b | 766 | |
| alanmillard | 22:fd75afdadfb2 | 767 | cohesion_vector.distance /= number_of_neighbours; // Normalise |
| alanmillard | 22:fd75afdadfb2 | 768 | |
| alanmillard | 23:e59040ac05c4 | 769 | int target_heading = 0; |
| alanmillard | 23:e59040ac05c4 | 770 | int angles = 0; |
| alanmillard | 22:fd75afdadfb2 | 771 | |
| alanmillard | 25:b5742873e02b | 772 | int ir_sensor_threshold = 400; |
| alanmillard | 25:b5742873e02b | 773 | |
| alanmillard | 22:fd75afdadfb2 | 774 | struct FloatVector obstacle_vector; |
| alanmillard | 22:fd75afdadfb2 | 775 | obstacle_vector.angle = 0; |
| alanmillard | 22:fd75afdadfb2 | 776 | obstacle_vector.distance = 0; |
| alanmillard | 22:fd75afdadfb2 | 777 | |
| alanmillard | 23:e59040ac05c4 | 778 | int sensors_activated = 0; |
| alanmillard | 23:e59040ac05c4 | 779 | |
| alanmillard | 25:b5742873e02b | 780 | // out("reflected_sensor_data: "); |
| alanmillard | 25:b5742873e02b | 781 | |
| alanmillard | 25:b5742873e02b | 782 | for(int i = 0; i < 8; i++) |
| alanmillard | 23:e59040ac05c4 | 783 | { |
| alanmillard | 25:b5742873e02b | 784 | // out("%d, ", reflected_sensor_data[i]); |
| alanmillard | 25:b5742873e02b | 785 | |
| alanmillard | 25:b5742873e02b | 786 | if(reflected_sensor_data[i] > ir_sensor_threshold) |
| alanmillard | 25:b5742873e02b | 787 | { |
| alanmillard | 23:e59040ac05c4 | 788 | obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); |
| alanmillard | 23:e59040ac05c4 | 789 | sensors_activated++; |
| alanmillard | 23:e59040ac05c4 | 790 | } |
| alanmillard | 23:e59040ac05c4 | 791 | } |
| alanmillard | 25:b5742873e02b | 792 | |
| alanmillard | 25:b5742873e02b | 793 | // out("\n"); |
| alanmillard | 22:fd75afdadfb2 | 794 | |
| alanmillard | 23:e59040ac05c4 | 795 | obstacle_vector.distance /= sensors_activated; // Normalise |
| alanmillard | 22:fd75afdadfb2 | 796 | |
| alanmillard | 25:b5742873e02b | 797 | // out("obstacle_vector.distance: %f, sensors_activated: %d\n", obstacle_vector.distance, sensors_activated); |
| alanmillard | 25:b5742873e02b | 798 | |
| alanmillard | 25:b5742873e02b | 799 | //if(obstacle_vector.distance > 150) |
| alanmillard | 25:b5742873e02b | 800 | if(sensors_activated > 0) |
| alanmillard | 22:fd75afdadfb2 | 801 | { |
| alanmillard | 25:b5742873e02b | 802 | int obstacle_avoidance_angle = obstacle_vector.angle + 180; |
| alanmillard | 25:b5742873e02b | 803 | |
| alanmillard | 25:b5742873e02b | 804 | if(obstacle_avoidance_angle > 180) |
| alanmillard | 25:b5742873e02b | 805 | obstacle_avoidance_angle -= 360; |
| alanmillard | 25:b5742873e02b | 806 | if(obstacle_avoidance_angle < -180) |
| alanmillard | 25:b5742873e02b | 807 | obstacle_avoidance_angle += 360; |
| alanmillard | 25:b5742873e02b | 808 | |
| alanmillard | 25:b5742873e02b | 809 | target_heading += obstacle_avoidance_angle; |
| alanmillard | 23:e59040ac05c4 | 810 | angles++; |
| alanmillard | 25:b5742873e02b | 811 | |
| alanmillard | 25:b5742873e02b | 812 | out("obstacle_vector.angle: %f, target_heading: %d\n", obstacle_vector.angle, target_heading); |
| alanmillard | 23:e59040ac05c4 | 813 | } |
| alanmillard | 23:e59040ac05c4 | 814 | |
| alanmillard | 25:b5742873e02b | 815 | // if(cohesion_vector.distance > 0) |
| alanmillard | 25:b5742873e02b | 816 | if(number_of_neighbours > 0) |
| alanmillard | 23:e59040ac05c4 | 817 | { |
| alanmillard | 25:b5742873e02b | 818 | int cohesion_angle = cohesion_vector.angle; |
| alanmillard | 25:b5742873e02b | 819 | |
| alanmillard | 25:b5742873e02b | 820 | if(cohesion_angle > 180) |
| alanmillard | 25:b5742873e02b | 821 | cohesion_angle -= 360; |
| alanmillard | 25:b5742873e02b | 822 | if(cohesion_angle < -180) |
| alanmillard | 25:b5742873e02b | 823 | cohesion_angle += 360; |
| alanmillard | 25:b5742873e02b | 824 | |
| alanmillard | 25:b5742873e02b | 825 | target_heading += cohesion_angle; |
| alanmillard | 23:e59040ac05c4 | 826 | angles++; |
| alanmillard | 22:fd75afdadfb2 | 827 | } |
| alanmillard | 22:fd75afdadfb2 | 828 | |
| alanmillard | 24:31707fec20a5 | 829 | if(number_of_flocking_headings > 0) |
| alanmillard | 24:31707fec20a5 | 830 | { |
| alanmillard | 25:b5742873e02b | 831 | // // Include own heading in flocking heading |
| alanmillard | 25:b5742873e02b | 832 | // average_heading += beacon_heading; |
| alanmillard | 25:b5742873e02b | 833 | // number_of_flocking_headings++; |
| alanmillard | 25:b5742873e02b | 834 | |
| alanmillard | 24:31707fec20a5 | 835 | average_heading /= number_of_flocking_headings; |
| alanmillard | 23:e59040ac05c4 | 836 | |
| alanmillard | 24:31707fec20a5 | 837 | int relative_flocking_heading = beacon_heading - average_heading; |
| alanmillard | 23:e59040ac05c4 | 838 | |
| alanmillard | 25:b5742873e02b | 839 | if(relative_flocking_heading > 180) |
| alanmillard | 25:b5742873e02b | 840 | relative_flocking_heading -= 360; |
| alanmillard | 25:b5742873e02b | 841 | if(relative_flocking_heading < -180) |
| alanmillard | 25:b5742873e02b | 842 | relative_flocking_heading += 360; |
| alanmillard | 25:b5742873e02b | 843 | |
| alanmillard | 24:31707fec20a5 | 844 | target_heading += relative_flocking_heading; |
| alanmillard | 24:31707fec20a5 | 845 | angles++; |
| alanmillard | 25:b5742873e02b | 846 | |
| alanmillard | 25:b5742873e02b | 847 | out("average_heading: %d, relative_flocking_heading: %d, target_heading: %d\n", average_heading, relative_flocking_heading, target_heading); |
| alanmillard | 24:31707fec20a5 | 848 | } |
| alanmillard | 23:e59040ac05c4 | 849 | |
| alanmillard | 24:31707fec20a5 | 850 | if(angles > 0) |
| alanmillard | 24:31707fec20a5 | 851 | target_heading /= angles; // Calculate average |
| alanmillard | 24:31707fec20a5 | 852 | |
| alanmillard | 25:b5742873e02b | 853 | // if(target_heading > 180) |
| alanmillard | 25:b5742873e02b | 854 | // target_heading -= 360; |
| alanmillard | 25:b5742873e02b | 855 | // if(target_heading < -180) |
| alanmillard | 25:b5742873e02b | 856 | // target_heading += 360; |
| alanmillard | 25:b5742873e02b | 857 | |
| alanmillard | 25:b5742873e02b | 858 | out("normalised target_heading: %d\n", target_heading); |
| alanmillard | 25:b5742873e02b | 859 | |
| alanmillard | 25:b5742873e02b | 860 | // if(step_cycle == 0) |
| alanmillard | 25:b5742873e02b | 861 | // time_based_turn_degrees(1, target_heading, 1); |
| alanmillard | 25:b5742873e02b | 862 | // else |
| alanmillard | 25:b5742873e02b | 863 | // forward(0.25); |
| alanmillard | 25:b5742873e02b | 864 | |
| alanmillard | 25:b5742873e02b | 865 | float left_motor_speed = 0.25; |
| alanmillard | 25:b5742873e02b | 866 | float right_motor_speed = 0.25; |
| alanmillard | 22:fd75afdadfb2 | 867 | |
| alanmillard | 25:b5742873e02b | 868 | // // Only avoid obstacles if very close to a wall |
| alanmillard | 25:b5742873e02b | 869 | // if(sensors_activated > 0 && obstacle_vector.distance > 1000) |
| alanmillard | 25:b5742873e02b | 870 | // { |
| alanmillard | 25:b5742873e02b | 871 | // int obstacle_avoidance_angle = obstacle_vector.angle + 180; |
| alanmillard | 25:b5742873e02b | 872 | // |
| alanmillard | 25:b5742873e02b | 873 | // if(obstacle_avoidance_angle > 180) |
| alanmillard | 25:b5742873e02b | 874 | // obstacle_avoidance_angle -= 360; |
| alanmillard | 25:b5742873e02b | 875 | // if(obstacle_avoidance_angle < -180) |
| alanmillard | 25:b5742873e02b | 876 | // obstacle_avoidance_angle += 360; |
| alanmillard | 25:b5742873e02b | 877 | // |
| alanmillard | 25:b5742873e02b | 878 | // target_heading = obstacle_avoidance_angle; |
| alanmillard | 25:b5742873e02b | 879 | // } |
| alanmillard | 25:b5742873e02b | 880 | |
| alanmillard | 25:b5742873e02b | 881 | if(target_heading > 22.5) |
| alanmillard | 25:b5742873e02b | 882 | right_motor_speed *= -1; |
| alanmillard | 25:b5742873e02b | 883 | else if(target_heading < -22.5) |
| alanmillard | 25:b5742873e02b | 884 | left_motor_speed *= -1; |
| alanmillard | 25:b5742873e02b | 885 | |
| alanmillard | 25:b5742873e02b | 886 | set_left_motor_speed(left_motor_speed); |
| alanmillard | 25:b5742873e02b | 887 | set_right_motor_speed(right_motor_speed); |
| alanmillard | 24:31707fec20a5 | 888 | |
| alanmillard | 24:31707fec20a5 | 889 | // Only transmit beacon heading if the beacon is visible |
| alanmillard | 24:31707fec20a5 | 890 | if(beacon_found) |
| alanmillard | 24:31707fec20a5 | 891 | { |
| alanmillard | 24:31707fec20a5 | 892 | float degrees_per_value = 256.0f / 360.0f; |
| alanmillard | 24:31707fec20a5 | 893 | char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value |
| alanmillard | 24:31707fec20a5 | 894 | |
| alanmillard | 24:31707fec20a5 | 895 | bt.putc(beacon_heading_byte); |
| alanmillard | 24:31707fec20a5 | 896 | } |
| alanmillard | 18:5921c1853e8a | 897 | } |
| alanmillard | 18:5921c1853e8a | 898 | |
| jah128 | 12:daa53285b6e4 | 899 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
| jah128 | 12:daa53285b6e4 | 900 | /// generic_program - Framework for building typical programs |
| jah128 | 12:daa53285b6e4 | 901 | |
| jah128 | 12:daa53285b6e4 | 902 | void generic_program() |
| jah128 | 12:daa53285b6e4 | 903 | { |
| jah128 | 12:daa53285b6e4 | 904 | // Do something on the first run of a program |
| jah128 | 12:daa53285b6e4 | 905 | if(program_run_init == 1) { |
| jah128 | 12:daa53285b6e4 | 906 | // Initialisation code goes here... |
| jah128 | 12:daa53285b6e4 | 907 | |
| jah128 | 12:daa53285b6e4 | 908 | program_run_init = 0; |
| jah128 | 12:daa53285b6e4 | 909 | } |
| jah128 | 12:daa53285b6e4 | 910 | |
| jah128 | 12:daa53285b6e4 | 911 | // 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 | 912 | if(step_cycle == 0) { |
| jah128 | 12:daa53285b6e4 | 913 | // Do something based on sensor data (eg turn) |
| jah128 | 12:daa53285b6e4 | 914 | } else { |
| jah128 | 12:daa53285b6e4 | 915 | // Do something ignoring sensor data (eg move, or nothing!) |
| jah128 | 12:daa53285b6e4 | 916 | } |
| jah128 | 12:daa53285b6e4 | 917 | |
| jah128 | 10:1b09d4bb847b | 918 | } |