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