Alan Millard
/
BeautifulMemeProjectBT
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
BeautifulMemeProject
by 
James Hilder
programs.cpp@19:77f7089dca87, 2016-01-12 (annotated)
- Committer:
- alanmillard
- Date:
- Tue Jan 12 17:28:12 2016 +0000
- Revision:
- 19:77f7089dca87
- Parent:
- 18:5921c1853e8a
- Child:
- 20:e08376c0b4ea
Successfully sending beacon headings as a single byte.
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 | 19:77f7089dca87 | 730 | Timer timer; |
| alanmillard | 19:77f7089dca87 | 731 | |
| alanmillard | 17:da524989b637 | 732 | void flocking_program() |
| alanmillard | 19:77f7089dca87 | 733 | { |
| alanmillard | 19:77f7089dca87 | 734 | // Do something on the first run of a program |
| alanmillard | 19:77f7089dca87 | 735 | if(program_run_init == 1) |
| alanmillard | 19:77f7089dca87 | 736 | { |
| alanmillard | 19:77f7089dca87 | 737 | // Initialisation code goes here... |
| alanmillard | 19:77f7089dca87 | 738 | timer.start(); |
| alanmillard | 19:77f7089dca87 | 739 | program_run_init = 0; |
| alanmillard | 19:77f7089dca87 | 740 | } |
| alanmillard | 17:da524989b637 | 741 | |
| alanmillard | 19:77f7089dca87 | 742 | if(timer.read() > 1) |
| alanmillard | 19:77f7089dca87 | 743 | { |
| alanmillard | 19:77f7089dca87 | 744 | timer.reset(); |
| alanmillard | 19:77f7089dca87 | 745 | |
| alanmillard | 19:77f7089dca87 | 746 | int average_heading = 0; |
| alanmillard | 18:5921c1853e8a | 747 | |
| alanmillard | 19:77f7089dca87 | 748 | for(int i = 0; i < 8; i++) |
| alanmillard | 19:77f7089dca87 | 749 | average_heading += flocking_headings[i]; |
| alanmillard | 19:77f7089dca87 | 750 | |
| alanmillard | 19:77f7089dca87 | 751 | average_heading /= 8; |
| alanmillard | 19:77f7089dca87 | 752 | |
| alanmillard | 19:77f7089dca87 | 753 | // out("average heading: %d\n", average_heading); |
| alanmillard | 19:77f7089dca87 | 754 | |
| alanmillard | 19:77f7089dca87 | 755 | float degrees_per_value = 256.0f / 360.0f; |
| alanmillard | 19:77f7089dca87 | 756 | char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value |
| alanmillard | 19:77f7089dca87 | 757 | |
| alanmillard | 19:77f7089dca87 | 758 | out("sending beacon heading: %d\n", beacon_heading); |
| alanmillard | 19:77f7089dca87 | 759 | //out("beacon heading byte: %d\n", beacon_heading_byte); |
| alanmillard | 19:77f7089dca87 | 760 | // out("ground truth: %f\n", (beacon_heading + 180) * degrees_per_value); |
| alanmillard | 19:77f7089dca87 | 761 | // bt.putc(beacon_heading); |
| alanmillard | 19:77f7089dca87 | 762 | bt.putc(beacon_heading_byte); |
| alanmillard | 19:77f7089dca87 | 763 | // bt.printf("%d\n", beacon_heading); |
| alanmillard | 19:77f7089dca87 | 764 | } |
| alanmillard | 18:5921c1853e8a | 765 | } |
| alanmillard | 18:5921c1853e8a | 766 | |
| alanmillard | 18:5921c1853e8a | 767 | //Timer timer; |
| alanmillard | 18:5921c1853e8a | 768 | // |
| alanmillard | 18:5921c1853e8a | 769 | //void flocking_program() |
| alanmillard | 18:5921c1853e8a | 770 | //{ |
| alanmillard | 18:5921c1853e8a | 771 | // // Do something on the first run of a program |
| alanmillard | 18:5921c1853e8a | 772 | // if(program_run_init == 1) { |
| alanmillard | 18:5921c1853e8a | 773 | // // Initialisation code goes here... |
| alanmillard | 18:5921c1853e8a | 774 | // |
| alanmillard | 18:5921c1853e8a | 775 | // timer.start(); |
| alanmillard | 18:5921c1853e8a | 776 | // |
| alanmillard | 18:5921c1853e8a | 777 | // program_run_init = 0; |
| alanmillard | 18:5921c1853e8a | 778 | // } |
| alanmillard | 18:5921c1853e8a | 779 | // |
| alanmillard | 18:5921c1853e8a | 780 | // // 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 | 18:5921c1853e8a | 781 | // if(step_cycle == 0) { |
| alanmillard | 18:5921c1853e8a | 782 | // // Do something based on sensor data (eg turn) |
| alanmillard | 18:5921c1853e8a | 783 | // } else { |
| alanmillard | 18:5921c1853e8a | 784 | // // Do something ignoring sensor data (eg move, or nothing!) |
| alanmillard | 18:5921c1853e8a | 785 | // } |
| alanmillard | 18:5921c1853e8a | 786 | // |
| alanmillard | 18:5921c1853e8a | 787 | // // Separation |
| alanmillard | 18:5921c1853e8a | 788 | // // Alignment |
| alanmillard | 18:5921c1853e8a | 789 | // // Cohesion |
| alanmillard | 18:5921c1853e8a | 790 | // |
| alanmillard | 18:5921c1853e8a | 791 | // float left_motor_speed = 0.5; |
| alanmillard | 18:5921c1853e8a | 792 | // float right_motor_speed = 0.5; |
| alanmillard | 18:5921c1853e8a | 793 | // |
| alanmillard | 18:5921c1853e8a | 794 | // struct FloatVector obstacle_vector; |
| alanmillard | 18:5921c1853e8a | 795 | // obstacle_vector.angle = 0; |
| alanmillard | 18:5921c1853e8a | 796 | // obstacle_vector.distance = 0; |
| alanmillard | 18:5921c1853e8a | 797 | // |
| alanmillard | 18:5921c1853e8a | 798 | // for(int i = 1; i < 8; i++) |
| alanmillard | 18:5921c1853e8a | 799 | // obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); |
| alanmillard | 18:5921c1853e8a | 800 | // |
| alanmillard | 18:5921c1853e8a | 801 | // obstacle_vector.distance /= 8; // Normalise |
| alanmillard | 18:5921c1853e8a | 802 | // |
| alanmillard | 18:5921c1853e8a | 803 | // struct FloatVector cohesion_vector; |
| alanmillard | 18:5921c1853e8a | 804 | // cohesion_vector.angle = 0; |
| alanmillard | 18:5921c1853e8a | 805 | // cohesion_vector.distance = 0; |
| alanmillard | 18:5921c1853e8a | 806 | // |
| alanmillard | 18:5921c1853e8a | 807 | // int number_of_neighbours = 0; |
| alanmillard | 18:5921c1853e8a | 808 | // |
| alanmillard | 18:5921c1853e8a | 809 | // for(int i = 1; i < 8; i++) |
| alanmillard | 17:da524989b637 | 810 | // { |
| alanmillard | 18:5921c1853e8a | 811 | // if(robots_found[i]) |
| alanmillard | 18:5921c1853e8a | 812 | // { |
| alanmillard | 18:5921c1853e8a | 813 | // number_of_neighbours++; |
| alanmillard | 18:5921c1853e8a | 814 | // cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]); |
| alanmillard | 18:5921c1853e8a | 815 | // } |
| alanmillard | 17:da524989b637 | 816 | // } |
| alanmillard | 18:5921c1853e8a | 817 | // |
| alanmillard | 18:5921c1853e8a | 818 | // cohesion_vector.distance /= number_of_neighbours; // Normalise |
| alanmillard | 18:5921c1853e8a | 819 | // |
| alanmillard | 18:5921c1853e8a | 820 | //// out("distance: %f, angle: %f\n", cohesion_vector.distance, cohesion_vector.angle); |
| alanmillard | 18:5921c1853e8a | 821 | // //out("distance: %f, angle: %f\n", obstacle_vector.distance, obstacle_vector.angle); |
| alanmillard | 18:5921c1853e8a | 822 | // |
| alanmillard | 18:5921c1853e8a | 823 | // char robot_nearby = 0; |
| alanmillard | 18:5921c1853e8a | 824 | // |
| alanmillard | 18:5921c1853e8a | 825 | // for(int i = 0; i < 8; i++) |
| alanmillard | 18:5921c1853e8a | 826 | // { |
| alanmillard | 18:5921c1853e8a | 827 | // if(robots_found[i]) |
| alanmillard | 18:5921c1853e8a | 828 | // { |
| alanmillard | 18:5921c1853e8a | 829 | // if(robots_heading[i] > -90 && robots_heading[i] < 90) |
| alanmillard | 18:5921c1853e8a | 830 | // { |
| alanmillard | 18:5921c1853e8a | 831 | // robot_nearby = 1; |
| alanmillard | 18:5921c1853e8a | 832 | // break; |
| alanmillard | 18:5921c1853e8a | 833 | // } |
| alanmillard | 18:5921c1853e8a | 834 | // } |
| alanmillard | 18:5921c1853e8a | 835 | // } |
| alanmillard | 18:5921c1853e8a | 836 | // |
| alanmillard | 18:5921c1853e8a | 837 | // //if(obstacle_vector.distance > 150 && robot_nearby == 0) |
| alanmillard | 18:5921c1853e8a | 838 | // if(obstacle_vector.distance > 150) |
| alanmillard | 18:5921c1853e8a | 839 | // { |
| alanmillard | 18:5921c1853e8a | 840 | //// out("avoiding obstacle\n"); |
| alanmillard | 17:da524989b637 | 841 | // |
| alanmillard | 18:5921c1853e8a | 842 | // if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5) |
| alanmillard | 18:5921c1853e8a | 843 | // left_motor_speed *= -1; |
| alanmillard | 18:5921c1853e8a | 844 | // else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5) |
| alanmillard | 18:5921c1853e8a | 845 | // right_motor_speed *= -1; |
| alanmillard | 18:5921c1853e8a | 846 | // } |
| alanmillard | 18:5921c1853e8a | 847 | //// else if(cohesion_vector.distance > 400) |
| alanmillard | 18:5921c1853e8a | 848 | //// { |
| alanmillard | 18:5921c1853e8a | 849 | //// if(cohesion_vector.angle > 0) |
| alanmillard | 18:5921c1853e8a | 850 | //// left_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 851 | //// else if(cohesion_vector.angle < 0) |
| alanmillard | 18:5921c1853e8a | 852 | //// right_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 853 | //// } |
| alanmillard | 18:5921c1853e8a | 854 | // else if(cohesion_vector.distance > 0) |
| alanmillard | 18:5921c1853e8a | 855 | // { |
| alanmillard | 18:5921c1853e8a | 856 | //// int target_heading = (beacon_heading + flocking_heading) % 360; |
| alanmillard | 18:5921c1853e8a | 857 | //// |
| alanmillard | 18:5921c1853e8a | 858 | //// if(target_heading > 0) |
| alanmillard | 18:5921c1853e8a | 859 | //// right_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 860 | //// else |
| alanmillard | 18:5921c1853e8a | 861 | //// left_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 862 | // |
| alanmillard | 18:5921c1853e8a | 863 | //// out("beacon: %d, flocking: %d, target: %d, mod: %d\n", beacon_heading, flocking_heading, beacon_heading + flocking_heading, (beacon_heading + flocking_heading) % 360); |
| alanmillard | 18:5921c1853e8a | 864 | // |
| alanmillard | 18:5921c1853e8a | 865 | // if(cohesion_vector.angle < 0) |
| alanmillard | 17:da524989b637 | 866 | // left_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 867 | // else if(cohesion_vector.angle > 0) |
| alanmillard | 18:5921c1853e8a | 868 | // right_motor_speed = 0.25; |
| alanmillard | 18:5921c1853e8a | 869 | // |
| alanmillard | 18:5921c1853e8a | 870 | // // Set LEDs to green |
| alanmillard | 18:5921c1853e8a | 871 | // set_leds(0xFF, 0x00); |
| alanmillard | 18:5921c1853e8a | 872 | // set_center_led(2, 1); |
| alanmillard | 18:5921c1853e8a | 873 | // } |
| alanmillard | 18:5921c1853e8a | 874 | // else |
| alanmillard | 18:5921c1853e8a | 875 | // { |
| alanmillard | 18:5921c1853e8a | 876 | // // Set LEDS to red |
| alanmillard | 18:5921c1853e8a | 877 | // set_leds(0x00, 0xFF); |
| alanmillard | 18:5921c1853e8a | 878 | // set_center_led(1, 1); |
| alanmillard | 18:5921c1853e8a | 879 | // } |
| alanmillard | 18:5921c1853e8a | 880 | // |
| alanmillard | 18:5921c1853e8a | 881 | //// out("left_motor_speed: %f, right_motor_speed: %f\n", left_motor_speed, right_motor_speed); |
| alanmillard | 18:5921c1853e8a | 882 | // |
| alanmillard | 18:5921c1853e8a | 883 | // set_left_motor_speed(left_motor_speed); |
| alanmillard | 18:5921c1853e8a | 884 | // set_right_motor_speed(right_motor_speed); |
| alanmillard | 18:5921c1853e8a | 885 | //} |
| jah128 | 13:f5994956b1ba | 886 | |
| jah128 | 12:daa53285b6e4 | 887 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
| jah128 | 12:daa53285b6e4 | 888 | /// generic_program - Framework for building typical programs |
| jah128 | 12:daa53285b6e4 | 889 | |
| jah128 | 12:daa53285b6e4 | 890 | void generic_program() |
| jah128 | 12:daa53285b6e4 | 891 | { |
| jah128 | 12:daa53285b6e4 | 892 | // Do something on the first run of a program |
| jah128 | 12:daa53285b6e4 | 893 | if(program_run_init == 1) { |
| jah128 | 12:daa53285b6e4 | 894 | // Initialisation code goes here... |
| jah128 | 12:daa53285b6e4 | 895 | |
| jah128 | 12:daa53285b6e4 | 896 | program_run_init = 0; |
| jah128 | 12:daa53285b6e4 | 897 | } |
| jah128 | 12:daa53285b6e4 | 898 | |
| jah128 | 12:daa53285b6e4 | 899 | // 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 | 900 | if(step_cycle == 0) { |
| jah128 | 12:daa53285b6e4 | 901 | // Do something based on sensor data (eg turn) |
| jah128 | 12:daa53285b6e4 | 902 | } else { |
| jah128 | 12:daa53285b6e4 | 903 | // Do something ignoring sensor data (eg move, or nothing!) |
| jah128 | 12:daa53285b6e4 | 904 | } |
| jah128 | 12:daa53285b6e4 | 905 | |
| jah128 | 10:1b09d4bb847b | 906 | } |