Science Memeseum / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCodeNew by James O'Keeffe

programs.cpp@14:f623db1e6184, 2015-10-28 (annotated)

Committer:
jah128
Date:
Wed Oct 28 11:41:56 2015 +0000
Revision:
14:f623db1e6184
Parent:
13:f5994956b1ba
Child:
16:976a1d0ea897
Bug fix to program selection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 10:1b09d4bb847b 1 /// PsiSwarm Beautiful Meme Project Source Code
jah128 11:7b3ee540ba56 2 /// Version 0.2
jah128 10:1b09d4bb847b 3 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 10:1b09d4bb847b 4 /// University of York
jah128 10:1b09d4bb847b 5
jah128 11:7b3ee540ba56 6 // programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
jah128 10:1b09d4bb847b 7
jah128 10:1b09d4bb847b 8 #include "main.h"
jah128 10:1b09d4bb847b 9
jah128 12:daa53285b6e4 10 int obstacle_avoidance_threshold = 300;
jah128 12:daa53285b6e4 11 int robot_avoidance_threshold = 2000;
jah128 12:daa53285b6e4 12
jah128 11:7b3ee540ba56 13 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 14 /// head_to_bearing_program
jah128 10:1b09d4bb847b 15 char was_turning = 0;
jah128 10:1b09d4bb847b 16
jah128 10:1b09d4bb847b 17 ///The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
jah128 10:1b09d4bb847b 18 void head_to_bearing_program(int target_bearing)
jah128 10:1b09d4bb847b 19 {
jah128 10:1b09d4bb847b 20 if(step_cycle == 0 || was_turning == 0) {
jah128 10:1b09d4bb847b 21 // Check if we are heading in the right bearing (+- 25 degrees)
jah128 10:1b09d4bb847b 22 int current_bearing = (360 - beacon_heading) % 360;
jah128 10:1b09d4bb847b 23 // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
jah128 10:1b09d4bb847b 24 char bearing_ok = 0;
jah128 10:1b09d4bb847b 25 int lower_bound = target_bearing - 25;
jah128 10:1b09d4bb847b 26 int upper_bound = target_bearing + 25;
jah128 10:1b09d4bb847b 27 if(lower_bound < 0) {
jah128 10:1b09d4bb847b 28 if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
jah128 10:1b09d4bb847b 29 } else if(upper_bound > 359) {
jah128 10:1b09d4bb847b 30 if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
jah128 10:1b09d4bb847b 31 } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
jah128 10:1b09d4bb847b 32 // Check if there is an obstacle in front of us
jah128 10:1b09d4bb847b 33 if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
jah128 10:1b09d4bb847b 34 else {
jah128 10:1b09d4bb847b 35 // Now move forward if we are facing correct bearing, otherwise turn
jah128 10:1b09d4bb847b 36 if(bearing_ok == 1) {
jah128 10:1b09d4bb847b 37 //Check if anything is in front of us to determine speed - if it is, move slowly
jah128 10:1b09d4bb847b 38 int t_time = 6 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 39 float t_speed = 1.0;
jah128 10:1b09d4bb847b 40 if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
jah128 10:1b09d4bb847b 41 t_time = 4 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 42 t_speed = 0.6;
jah128 10:1b09d4bb847b 43 }
jah128 10:1b09d4bb847b 44 if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
jah128 10:1b09d4bb847b 45 t_time = 3 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 46 t_speed = 0.4;
jah128 10:1b09d4bb847b 47 }
jah128 10:1b09d4bb847b 48 if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
jah128 10:1b09d4bb847b 49 t_time = 2 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 50 t_speed = 0.2;
jah128 10:1b09d4bb847b 51 }
jah128 10:1b09d4bb847b 52 time_based_forward(t_speed,t_time,0);
jah128 10:1b09d4bb847b 53 was_turning = 0;
jah128 10:1b09d4bb847b 54 } else {
jah128 10:1b09d4bb847b 55 turn_to_bearing(target_bearing);
jah128 10:1b09d4bb847b 56 was_turning = 1;
jah128 10:1b09d4bb847b 57 }
jah128 10:1b09d4bb847b 58 }
jah128 10:1b09d4bb847b 59 }
jah128 10:1b09d4bb847b 60 }
jah128 10:1b09d4bb847b 61
jah128 11:7b3ee540ba56 62
jah128 11:7b3ee540ba56 63
jah128 11:7b3ee540ba56 64
jah128 11:7b3ee540ba56 65 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 66 /// recharging_program
jah128 11:7b3ee540ba56 67
jah128 11:7b3ee540ba56 68 char recharge_power_check_count = 0;
jah128 11:7b3ee540ba56 69 char battery_low_count = 0;
jah128 11:7b3ee540ba56 70
jah128 10:1b09d4bb847b 71 void recharging_program()
jah128 10:1b09d4bb847b 72 {
jah128 10:1b09d4bb847b 73 switch(recharging_state) {
jah128 10:1b09d4bb847b 74 case 0:
jah128 10:1b09d4bb847b 75 // We are not currently recharging, check the battery state
jah128 10:1b09d4bb847b 76 if(get_battery_voltage() < battery_low_threshold) {
jah128 10:1b09d4bb847b 77 // Battery is below low threshold
jah128 10:1b09d4bb847b 78 battery_low_count ++;
jah128 10:1b09d4bb847b 79 // We don't immediately start recharging routine in case as battery level can fluctuate a little due to load; if we get a low value for 4 continuous timesteps we start recharging routine
jah128 10:1b09d4bb847b 80 if(battery_low_count > 3) {
jah128 10:1b09d4bb847b 81 // Set recharging state to 'looking for charger'
jah128 10:1b09d4bb847b 82 recharging_state = 1;
jah128 10:1b09d4bb847b 83 strcpy(prog_name,"CHARGING PROGRAM");
jah128 10:1b09d4bb847b 84 set_program_info("HEAD TO BEACON");
jah128 10:1b09d4bb847b 85 }
jah128 10:1b09d4bb847b 86 } else battery_low_count = 0;
jah128 10:1b09d4bb847b 87 break;
jah128 10:1b09d4bb847b 88 // State 1: Head to beacon [as this is where battery charger is]
jah128 10:1b09d4bb847b 89 case 1:
jah128 10:1b09d4bb847b 90 target_reached = 0;
jah128 10:1b09d4bb847b 91 head_to_bearing_program(0);
jah128 10:1b09d4bb847b 92 if(target_reached == 1) {
jah128 10:1b09d4bb847b 93 recharging_state = 2;
jah128 10:1b09d4bb847b 94 set_program_info("TURN 90 DEGREES");
jah128 10:1b09d4bb847b 95 }
jah128 10:1b09d4bb847b 96 break;
jah128 10:1b09d4bb847b 97 // Stage 2: Turn 90 degrees to align with charging pads
jah128 10:1b09d4bb847b 98 case 2:
jah128 10:1b09d4bb847b 99 disable_ir_emitters = 1;
jah128 10:1b09d4bb847b 100 time_based_turn_degrees(0.8, 70.0, 1);
jah128 10:1b09d4bb847b 101 recharge_power_check_count = 0;
jah128 10:1b09d4bb847b 102 recharging_state = 3;
jah128 10:1b09d4bb847b 103 break;
jah128 10:1b09d4bb847b 104 // Stage 3: Wait for turn to complete
jah128 10:1b09d4bb847b 105 case 3:
jah128 10:1b09d4bb847b 106 if (time_based_motor_action != 1) {
jah128 10:1b09d4bb847b 107 recharging_state = 4;
jah128 10:1b09d4bb847b 108 set_program_info("CHECK FOR POWER");
jah128 10:1b09d4bb847b 109 }
jah128 10:1b09d4bb847b 110 break;
jah128 10:1b09d4bb847b 111 // Stage 4: Check if charging
jah128 10:1b09d4bb847b 112 case 4:
jah128 10:1b09d4bb847b 113 recharge_power_check_count++;
jah128 10:1b09d4bb847b 114 if(get_dc_status() == 1) {
jah128 10:1b09d4bb847b 115 recharging_state = 5;
jah128 10:1b09d4bb847b 116 } else {
jah128 10:1b09d4bb847b 117 if(recharge_power_check_count < 10)recharging_state = 6;
jah128 10:1b09d4bb847b 118 else {
jah128 10:1b09d4bb847b 119 recharging_state = 7;
jah128 10:1b09d4bb847b 120 set_program_info("NO POWER - RETRY");
jah128 10:1b09d4bb847b 121 }
jah128 10:1b09d4bb847b 122 }
jah128 10:1b09d4bb847b 123 break;
jah128 10:1b09d4bb847b 124 // Stage 5: Charging. Wait until threshold voltage exceeded
jah128 10:1b09d4bb847b 125 case 5:
jah128 10:1b09d4bb847b 126 if(get_battery_voltage() > battery_high_threshold) {
jah128 10:1b09d4bb847b 127 set_program_info("LEAVE CHARGER");
jah128 10:1b09d4bb847b 128 recharging_state = 7;
jah128 10:1b09d4bb847b 129 } else {
jah128 10:1b09d4bb847b 130 char b_voltage[16];
jah128 10:1b09d4bb847b 131 sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
jah128 10:1b09d4bb847b 132 set_program_info(b_voltage);
jah128 10:1b09d4bb847b 133 }
jah128 10:1b09d4bb847b 134 break;
jah128 10:1b09d4bb847b 135 // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck
jah128 10:1b09d4bb847b 136 case 6:
jah128 10:1b09d4bb847b 137 time_based_turn_degrees(0.5,4,1);
jah128 10:1b09d4bb847b 138 recharging_state = 4;
jah128 10:1b09d4bb847b 139 break;
jah128 10:1b09d4bb847b 140 // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program
jah128 10:1b09d4bb847b 141 case 7:
jah128 10:1b09d4bb847b 142 time_based_turn_degrees(0.8, 90.0, 1);
jah128 10:1b09d4bb847b 143 recharging_state = 8;
jah128 10:1b09d4bb847b 144 break;
jah128 10:1b09d4bb847b 145
jah128 10:1b09d4bb847b 146 // Stage 8: Wait for turn to complete
jah128 10:1b09d4bb847b 147 case 8:
jah128 10:1b09d4bb847b 148 if (time_based_motor_action != 1) recharging_state = 9;
jah128 10:1b09d4bb847b 149 break;
jah128 10:1b09d4bb847b 150 // Stage 9: Move away
jah128 10:1b09d4bb847b 151 case 9:
jah128 10:1b09d4bb847b 152 time_based_forward(0.5, 1000000, 1);
jah128 10:1b09d4bb847b 153 recharging_state = 10;
jah128 10:1b09d4bb847b 154 break;
jah128 10:1b09d4bb847b 155 // Stage 10: Wait for move to complete
jah128 10:1b09d4bb847b 156 case 10:
jah128 10:1b09d4bb847b 157 if (time_based_motor_action != 1) recharging_state = 11;
jah128 10:1b09d4bb847b 158 break;
jah128 10:1b09d4bb847b 159 // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle
jah128 10:1b09d4bb847b 160 case 11:
jah128 10:1b09d4bb847b 161 disable_ir_emitters = 0;
jah128 10:1b09d4bb847b 162 if (get_battery_voltage() < battery_low_threshold) {
jah128 10:1b09d4bb847b 163 recharging_state = 1;
jah128 10:1b09d4bb847b 164 } else {
jah128 10:1b09d4bb847b 165 recharging_state = 0;
jah128 10:1b09d4bb847b 166 //Restore name of old program on display
jah128 10:1b09d4bb847b 167 set_program(main_program_state);
jah128 10:1b09d4bb847b 168 }
jah128 10:1b09d4bb847b 169 break;
jah128 10:1b09d4bb847b 170 }
jah128 10:1b09d4bb847b 171 }
jah128 10:1b09d4bb847b 172
jah128 11:7b3ee540ba56 173
jah128 11:7b3ee540ba56 174
jah128 11:7b3ee540ba56 175
jah128 11:7b3ee540ba56 176 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 177 /// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program)
jah128 11:7b3ee540ba56 178
jah128 11:7b3ee540ba56 179 enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
jah128 11:7b3ee540ba56 180 enum random_walk_state internal_state = random_walk;
jah128 11:7b3ee540ba56 181 char action_timeout = 0;
jah128 11:7b3ee540ba56 182 char interaction_timeout = 0;
jah128 11:7b3ee540ba56 183 char random_walk_timeout = 0;
jah128 11:7b3ee540ba56 184 float previous_left_motor_speed = 0.5;
jah128 11:7b3ee540ba56 185 float previous_right_motor_speed = 0.5;
jah128 11:7b3ee540ba56 186
jah128 10:1b09d4bb847b 187 void curved_random_walk_with_interaction_program()
jah128 10:1b09d4bb847b 188 {
jah128 10:1b09d4bb847b 189 if(internal_state == random_walk) {
jah128 10:1b09d4bb847b 190 if(interaction_timeout < 4)
jah128 10:1b09d4bb847b 191 interaction_timeout++;
jah128 10:1b09d4bb847b 192
jah128 10:1b09d4bb847b 193 int closest_robot = -1;
jah128 10:1b09d4bb847b 194 unsigned short shortest_distance = 0;
jah128 10:1b09d4bb847b 195
jah128 10:1b09d4bb847b 196 // Check whether there are any other robots within range
jah128 10:1b09d4bb847b 197 for(int i = 0; i < 8; i++) {
jah128 10:1b09d4bb847b 198 if(robots_found[i]) {
jah128 10:1b09d4bb847b 199 if(robots_distance[i] > shortest_distance) {
jah128 10:1b09d4bb847b 200 shortest_distance = robots_distance[i];
jah128 10:1b09d4bb847b 201 closest_robot = i;
jah128 10:1b09d4bb847b 202 }
jah128 10:1b09d4bb847b 203 }
jah128 10:1b09d4bb847b 204 }
jah128 10:1b09d4bb847b 205
jah128 10:1b09d4bb847b 206 // Turn towards the closest robot
jah128 10:1b09d4bb847b 207 if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
jah128 10:1b09d4bb847b 208 time_based_turn_degrees(1, robots_heading[closest_robot], 1);
jah128 10:1b09d4bb847b 209
jah128 10:1b09d4bb847b 210 action_timeout = 0;
jah128 10:1b09d4bb847b 211 internal_state = turn_towards;
jah128 10:1b09d4bb847b 212 char temp_message[17];
jah128 10:1b09d4bb847b 213 sprintf(temp_message,"FACE ROBOT %d",closest_robot);
jah128 10:1b09d4bb847b 214 set_program_info(temp_message);
jah128 10:1b09d4bb847b 215 } else { // Otherwise, do a random walk
jah128 10:1b09d4bb847b 216 // Check the front sensors for obstacles
jah128 10:1b09d4bb847b 217 if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 218 reflected_sensor_data[1] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 219 reflected_sensor_data[6] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 220 reflected_sensor_data[7] > obstacle_avoidance_threshold) {
jah128 10:1b09d4bb847b 221 // Ignore the rear sensors when calculating the heading
jah128 10:1b09d4bb847b 222 reflected_sensor_data[2] = 0;
jah128 10:1b09d4bb847b 223 reflected_sensor_data[3] = 0;
jah128 10:1b09d4bb847b 224 reflected_sensor_data[4] = 0;
jah128 10:1b09d4bb847b 225 reflected_sensor_data[5] = 0;
jah128 10:1b09d4bb847b 226
jah128 10:1b09d4bb847b 227 // Turn 180 degrees away from the sensed obstacle
jah128 10:1b09d4bb847b 228 int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
jah128 10:1b09d4bb847b 229
jah128 10:1b09d4bb847b 230 // Normalise
jah128 10:1b09d4bb847b 231 heading %= 360;
jah128 10:1b09d4bb847b 232
jah128 10:1b09d4bb847b 233 if(heading < -180)
jah128 10:1b09d4bb847b 234 heading += 360;
jah128 10:1b09d4bb847b 235
jah128 10:1b09d4bb847b 236 if(heading > 180)
jah128 10:1b09d4bb847b 237 heading -= 360;
jah128 10:1b09d4bb847b 238 set_program_info("AVOID OBSTACLE");
jah128 10:1b09d4bb847b 239 time_based_turn_degrees(1, heading, 1);
jah128 10:1b09d4bb847b 240
jah128 10:1b09d4bb847b 241 action_timeout = 0;
jah128 10:1b09d4bb847b 242 internal_state = turn_away;
jah128 10:1b09d4bb847b 243 } else {
jah128 10:1b09d4bb847b 244 // Change motor speeds every 1s
jah128 10:1b09d4bb847b 245 if(random_walk_timeout >= 2) {
jah128 10:1b09d4bb847b 246 float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
jah128 10:1b09d4bb847b 247
jah128 10:1b09d4bb847b 248 float left_motor_speed = previous_left_motor_speed - random_offset;
jah128 10:1b09d4bb847b 249 float right_motor_speed = previous_right_motor_speed + random_offset;
jah128 10:1b09d4bb847b 250
jah128 10:1b09d4bb847b 251 float threshold = 0.25;
jah128 10:1b09d4bb847b 252
jah128 10:1b09d4bb847b 253 if(left_motor_speed < threshold)
jah128 10:1b09d4bb847b 254 left_motor_speed = threshold;
jah128 10:1b09d4bb847b 255
jah128 10:1b09d4bb847b 256 if(right_motor_speed < threshold)
jah128 10:1b09d4bb847b 257 right_motor_speed = threshold;
jah128 10:1b09d4bb847b 258
jah128 10:1b09d4bb847b 259 set_left_motor_speed(left_motor_speed);
jah128 10:1b09d4bb847b 260 set_right_motor_speed(right_motor_speed);
jah128 10:1b09d4bb847b 261
jah128 10:1b09d4bb847b 262 random_walk_timeout = 0;
jah128 10:1b09d4bb847b 263 }
jah128 10:1b09d4bb847b 264
jah128 10:1b09d4bb847b 265 random_walk_timeout++;
jah128 10:1b09d4bb847b 266 }
jah128 10:1b09d4bb847b 267 }
jah128 10:1b09d4bb847b 268 } else if(internal_state == turn_towards) {
jah128 10:1b09d4bb847b 269 if(action_timeout < 4)
jah128 10:1b09d4bb847b 270 action_timeout++;
jah128 10:1b09d4bb847b 271 else {
jah128 10:1b09d4bb847b 272 set_program_info("SAY HELLO");
jah128 10:1b09d4bb847b 273 vibrate();
jah128 10:1b09d4bb847b 274
jah128 10:1b09d4bb847b 275 action_timeout = 0;
jah128 10:1b09d4bb847b 276 internal_state = interact;
jah128 10:1b09d4bb847b 277 }
jah128 10:1b09d4bb847b 278 } else if(internal_state == interact) {
jah128 10:1b09d4bb847b 279 if(action_timeout < 4)
jah128 10:1b09d4bb847b 280 action_timeout++;
jah128 10:1b09d4bb847b 281 else {
jah128 10:1b09d4bb847b 282 set_program_info("TURN AROUND");
jah128 10:1b09d4bb847b 283 time_based_turn_degrees(1, 180, 1);
jah128 10:1b09d4bb847b 284
jah128 10:1b09d4bb847b 285 action_timeout = 0;
jah128 10:1b09d4bb847b 286 internal_state = turn_away;
jah128 10:1b09d4bb847b 287 }
jah128 10:1b09d4bb847b 288
jah128 10:1b09d4bb847b 289 } else if(internal_state == turn_away) {
jah128 10:1b09d4bb847b 290 if(action_timeout < 4)
jah128 10:1b09d4bb847b 291 action_timeout++;
jah128 10:1b09d4bb847b 292 else {
jah128 10:1b09d4bb847b 293 set_program_info("RANDOM WALK");
jah128 10:1b09d4bb847b 294 interaction_timeout = 0;
jah128 10:1b09d4bb847b 295 internal_state = random_walk;
jah128 10:1b09d4bb847b 296 }
jah128 10:1b09d4bb847b 297 } else if(internal_state == avoid_obstacle) {
jah128 10:1b09d4bb847b 298 if(action_timeout < 4)
jah128 10:1b09d4bb847b 299 action_timeout++;
jah128 10:1b09d4bb847b 300 else
jah128 10:1b09d4bb847b 301 set_program_info("RANDOM WALK");
jah128 12:daa53285b6e4 302 internal_state = random_walk;
jah128 10:1b09d4bb847b 303 }
jah128 10:1b09d4bb847b 304 }
jah128 10:1b09d4bb847b 305
jah128 10:1b09d4bb847b 306
jah128 11:7b3ee540ba56 307
jah128 11:7b3ee540ba56 308
jah128 11:7b3ee540ba56 309
jah128 11:7b3ee540ba56 310 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 12:daa53285b6e4 311 /// straight_random_walk_with_interaction_program
jah128 11:7b3ee540ba56 312
jah128 10:1b09d4bb847b 313 void straight_random_walk_with_interaction_program()
jah128 10:1b09d4bb847b 314 {
jah128 10:1b09d4bb847b 315
jah128 10:1b09d4bb847b 316 }
jah128 10:1b09d4bb847b 317
jah128 10:1b09d4bb847b 318
jah128 11:7b3ee540ba56 319
jah128 11:7b3ee540ba56 320
jah128 11:7b3ee540ba56 321
jah128 11:7b3ee540ba56 322 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 12:daa53285b6e4 323 /// find_space_program
jah128 11:7b3ee540ba56 324
jah128 14:f623db1e6184 325 char prog_debug = 1 ;
jah128 13:f5994956b1ba 326 float target_wheel_speed;
jah128 13:f5994956b1ba 327 int random_walk_bearing;
jah128 13:f5994956b1ba 328
jah128 13:f5994956b1ba 329 void find_space_program(char bidirectional)
jah128 10:1b09d4bb847b 330 {
jah128 13:f5994956b1ba 331 // The find_space_program is a continuous turn-move vector program
jah128 10:1b09d4bb847b 332
jah128 13:f5994956b1ba 333 if(program_run_init == 1) {
jah128 13:f5994956b1ba 334 // Setup the LEDs to red
jah128 13:f5994956b1ba 335 set_leds(0x00,0xFF);
jah128 13:f5994956b1ba 336 set_center_led(1,1);
jah128 13:f5994956b1ba 337 program_run_init = 0;
jah128 13:f5994956b1ba 338 random_walk_bearing = rand() % 360;
jah128 13:f5994956b1ba 339 }
jah128 13:f5994956b1ba 340
jah128 13:f5994956b1ba 341 // When step_cycle = 0 we calculate a vector to move to and a target distance
jah128 13:f5994956b1ba 342 if(step_cycle == 0) {
jah128 13:f5994956b1ba 343 struct FloatVector target_vector;
jah128 13:f5994956b1ba 344 target_vector.angle = 0;
jah128 13:f5994956b1ba 345 target_vector.distance = 0;
jah128 13:f5994956b1ba 346 // Check for near robots within range
jah128 13:f5994956b1ba 347 for(int i = 1; i < 8; i++) {
jah128 13:f5994956b1ba 348 if(robots_found[i]) {
jah128 13:f5994956b1ba 349 int res_bearing = robots_heading[i];
jah128 13:f5994956b1ba 350 res_bearing += 180;
jah128 13:f5994956b1ba 351 if(res_bearing > 180) res_bearing -= 360;
jah128 13:f5994956b1ba 352 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
jah128 13:f5994956b1ba 353 if(prog_debug) out("Repelled from robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 13:f5994956b1ba 354 }
jah128 13:f5994956b1ba 355 }
jah128 13:f5994956b1ba 356 if(target_vector.angle!=0){
jah128 13:f5994956b1ba 357 set_leds(0xFF,0xFF);
jah128 13:f5994956b1ba 358 set_center_led(3,1);
jah128 13:f5994956b1ba 359 }
jah128 13:f5994956b1ba 360 // Check for obstacles
jah128 13:f5994956b1ba 361 char obstacle = 0;
jah128 13:f5994956b1ba 362 int peak_strength = 0;
jah128 13:f5994956b1ba 363 for(int i=0; i<8; i++){
jah128 13:f5994956b1ba 364 if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
jah128 13:f5994956b1ba 365 if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
jah128 13:f5994956b1ba 366 }
jah128 13:f5994956b1ba 367 if(obstacle){
jah128 13:f5994956b1ba 368 //Choose new random walk bearing
jah128 13:f5994956b1ba 369 set_leds(0x00,0xFF);
jah128 13:f5994956b1ba 370 set_center_led(1,1);
jah128 13:f5994956b1ba 371 random_walk_bearing = rand() % 360;
jah128 13:f5994956b1ba 372 int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
jah128 13:f5994956b1ba 373 int obs_bearing = obstacle_bearing + 180;
jah128 13:f5994956b1ba 374 if(obs_bearing > 180) obs_bearing -= 360;
jah128 13:f5994956b1ba 375 target_vector = addVector(target_vector,obs_bearing,peak_strength);
jah128 13:f5994956b1ba 376 if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d, resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
jah128 13:f5994956b1ba 377 }
jah128 13:f5994956b1ba 378
jah128 13:f5994956b1ba 379 if(target_vector.angle == 0 && target_vector.distance == 0){
jah128 13:f5994956b1ba 380 set_leds(0xFF,0x00);
jah128 13:f5994956b1ba 381 set_center_led(2,1);
jah128 13:f5994956b1ba 382 random_walk_bearing += 180;
jah128 13:f5994956b1ba 383 if(random_walk_bearing > 360) random_walk_bearing -= 360;
jah128 13:f5994956b1ba 384 target_vector.distance = 100;
jah128 13:f5994956b1ba 385 int current_bearing = 360 - beacon_heading;
jah128 13:f5994956b1ba 386 //Now work out turn needed to face intended heading
jah128 13:f5994956b1ba 387 int target_turn = (random_walk_bearing - current_bearing) % 360;
jah128 13:f5994956b1ba 388 if(target_turn > 180) target_turn -= 360;
jah128 13:f5994956b1ba 389 if(target_turn < -180) target_turn += 360;
jah128 13:f5994956b1ba 390 target_vector.angle = target_turn;
jah128 13:f5994956b1ba 391 if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
jah128 13:f5994956b1ba 392 }
jah128 13:f5994956b1ba 393 char wheel_direction = 1;
jah128 13:f5994956b1ba 394 if(bidirectional){
jah128 13:f5994956b1ba 395 // Allow reverse wheel direction
jah128 13:f5994956b1ba 396 if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
jah128 13:f5994956b1ba 397 else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
jah128 13:f5994956b1ba 398 }
jah128 13:f5994956b1ba 399 //Now turn to angle
jah128 13:f5994956b1ba 400 float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
jah128 13:f5994956b1ba 401 if(target_vector.angle < 0){
jah128 13:f5994956b1ba 402 if(target_vector.angle < -maximum_turn_angle){
jah128 13:f5994956b1ba 403 target_vector.angle = -maximum_turn_angle;
jah128 13:f5994956b1ba 404 target_vector.distance = 100;
jah128 13:f5994956b1ba 405 }
jah128 13:f5994956b1ba 406 }else{
jah128 13:f5994956b1ba 407 if(target_vector.angle > maximum_turn_angle){
jah128 13:f5994956b1ba 408 target_vector.angle = maximum_turn_angle;
jah128 13:f5994956b1ba 409 target_vector.distance = 100;
jah128 13:f5994956b1ba 410 }
jah128 13:f5994956b1ba 411 }
jah128 13:f5994956b1ba 412 //Set the wheel speed for next action
jah128 13:f5994956b1ba 413 if(target_vector.distance < 120) target_wheel_speed = 0.25;
jah128 13:f5994956b1ba 414 else if(target_vector.distance < 240) target_wheel_speed = 0.35;
jah128 13:f5994956b1ba 415 else if(target_vector.distance < 480) target_wheel_speed = 0.45;
jah128 13:f5994956b1ba 416 else if(target_vector.distance < 960) target_wheel_speed = 0.55;
jah128 13:f5994956b1ba 417 else target_wheel_speed = 0.65;
jah128 13:f5994956b1ba 418 if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
jah128 13:f5994956b1ba 419
jah128 13:f5994956b1ba 420 //Now turn...
jah128 13:f5994956b1ba 421 time_based_turn_degrees(1, (int) target_vector.angle, 1);
jah128 13:f5994956b1ba 422 } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0);
jah128 12:daa53285b6e4 423 }
jah128 12:daa53285b6e4 424
jah128 12:daa53285b6e4 425
jah128 12:daa53285b6e4 426
jah128 12:daa53285b6e4 427
jah128 12:daa53285b6e4 428
jah128 12:daa53285b6e4 429 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 12:daa53285b6e4 430 /// clustering_program
jah128 12:daa53285b6e4 431
jah128 12:daa53285b6e4 432
jah128 13:f5994956b1ba 433 void clustering_program(char invert, char bidirectional)
jah128 12:daa53285b6e4 434 {
jah128 12:daa53285b6e4 435 // The clustering program is a continuous turn-move vector program
jah128 12:daa53285b6e4 436 // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots
jah128 12:daa53285b6e4 437 // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group
jah128 12:daa53285b6e4 438
jah128 12:daa53285b6e4 439 // Store the robot group: even robots (0) are green, odd robots (1) are red
jah128 12:daa53285b6e4 440 char group = robot_id % 2;
jah128 12:daa53285b6e4 441
jah128 12:daa53285b6e4 442 if(program_run_init == 1) {
jah128 12:daa53285b6e4 443 // Setup the LEDs based on robot_id
jah128 12:daa53285b6e4 444 if(group == 0) {
jah128 12:daa53285b6e4 445 set_leds(0xFF,0x00);
jah128 12:daa53285b6e4 446 set_center_led(2,1);
jah128 12:daa53285b6e4 447 } else {
jah128 12:daa53285b6e4 448 set_leds(0x00,0xFF);
jah128 12:daa53285b6e4 449 set_center_led(1,1);
jah128 12:daa53285b6e4 450 }
jah128 12:daa53285b6e4 451 program_run_init = 0;
jah128 13:f5994956b1ba 452 random_walk_bearing = rand() % 360;
jah128 12:daa53285b6e4 453 }
jah128 12:daa53285b6e4 454
jah128 12:daa53285b6e4 455 // When step_cycle = 0 we calculate a vector to move to and a target distance
jah128 12:daa53285b6e4 456 if(step_cycle == 0) {
jah128 13:f5994956b1ba 457 char avoiding_friend = 0;
jah128 12:daa53285b6e4 458 struct FloatVector target_vector;
jah128 12:daa53285b6e4 459 target_vector.angle = 0;
jah128 12:daa53285b6e4 460 target_vector.distance = 0;
jah128 12:daa53285b6e4 461 // Check for near robots within range
jah128 12:daa53285b6e4 462 for(int i = 1; i < 8; i++) {
jah128 12:daa53285b6e4 463 if(robots_found[i]) {
jah128 12:daa53285b6e4 464 // Determine if the robot is an attractor or a repellor
jah128 12:daa53285b6e4 465 char attract = 0;
jah128 12:daa53285b6e4 466 if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1;
jah128 12:daa53285b6e4 467 // Avoid very close attractors to stop collisions
jah128 13:f5994956b1ba 468 if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;}
jah128 12:daa53285b6e4 469 int res_bearing = robots_heading[i];
jah128 12:daa53285b6e4 470 if(attract==0){
jah128 12:daa53285b6e4 471 res_bearing += 180;
jah128 12:daa53285b6e4 472 if(res_bearing > 180) res_bearing -= 360;
jah128 12:daa53285b6e4 473 }
jah128 12:daa53285b6e4 474 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
jah128 12:daa53285b6e4 475 if(prog_debug) {
jah128 12:daa53285b6e4 476 if(attract) {
jah128 12:daa53285b6e4 477 out("Attracted to robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 12:daa53285b6e4 478 } else {
jah128 12:daa53285b6e4 479 out("Repelled from robot %d at bearing %d, strength %d, resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
jah128 12:daa53285b6e4 480 }
jah128 12:daa53285b6e4 481 }
jah128 12:daa53285b6e4 482 }
jah128 12:daa53285b6e4 483 }
jah128 12:daa53285b6e4 484
jah128 12:daa53285b6e4 485 // Check for obstacles
jah128 12:daa53285b6e4 486 char obstacle = 0;
jah128 12:daa53285b6e4 487 int peak_strength = 0;
jah128 12:daa53285b6e4 488 for(int i=0; i<8; i++){
jah128 12:daa53285b6e4 489 if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
jah128 12:daa53285b6e4 490 if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
jah128 12:daa53285b6e4 491 }
jah128 12:daa53285b6e4 492 if(obstacle){
jah128 13:f5994956b1ba 493 //Choose new random walk bearing
jah128 13:f5994956b1ba 494 random_walk_bearing = rand() % 360;
jah128 12:daa53285b6e4 495 int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
jah128 12:daa53285b6e4 496 int obs_bearing = obstacle_bearing + 180;
jah128 12:daa53285b6e4 497 if(obs_bearing > 180) obs_bearing -= 360;
jah128 12:daa53285b6e4 498 target_vector = addVector(target_vector,obs_bearing,peak_strength);
jah128 12:daa53285b6e4 499 if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d, resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
jah128 12:daa53285b6e4 500 }
jah128 13:f5994956b1ba 501 if(target_vector.angle == 0 && target_vector.distance == 0){
jah128 13:f5994956b1ba 502 //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing
jah128 13:f5994956b1ba 503 if(rand() % 100 > 97) random_walk_bearing = rand() % 360;
jah128 13:f5994956b1ba 504 target_vector.distance = 100;
jah128 13:f5994956b1ba 505 int current_bearing = 360 - beacon_heading;
jah128 13:f5994956b1ba 506 //Now work out turn needed to face intended heading
jah128 13:f5994956b1ba 507 int target_turn = (random_walk_bearing - current_bearing) % 360;
jah128 13:f5994956b1ba 508 if(target_turn > 180) target_turn -= 360;
jah128 13:f5994956b1ba 509 if(target_turn < -180) target_turn += 360;
jah128 13:f5994956b1ba 510 target_vector.angle = target_turn;
jah128 13:f5994956b1ba 511 if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
jah128 13:f5994956b1ba 512 }
jah128 13:f5994956b1ba 513 char wheel_direction = 1;
jah128 13:f5994956b1ba 514 if(bidirectional){
jah128 13:f5994956b1ba 515 // Allow reverse wheel direction
jah128 13:f5994956b1ba 516 if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
jah128 13:f5994956b1ba 517 else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
jah128 13:f5994956b1ba 518 }
jah128 13:f5994956b1ba 519 //Now turn to angle
jah128 13:f5994956b1ba 520 float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
jah128 13:f5994956b1ba 521 if(target_vector.angle < 0){
jah128 13:f5994956b1ba 522 if(target_vector.angle < -maximum_turn_angle){
jah128 13:f5994956b1ba 523 target_vector.angle = -maximum_turn_angle;
jah128 13:f5994956b1ba 524 target_vector.distance = 100;
jah128 13:f5994956b1ba 525 }
jah128 13:f5994956b1ba 526 }else{
jah128 13:f5994956b1ba 527 if(target_vector.angle > maximum_turn_angle){
jah128 13:f5994956b1ba 528 target_vector.angle = maximum_turn_angle;
jah128 13:f5994956b1ba 529 target_vector.distance = 100;
jah128 13:f5994956b1ba 530 }
jah128 13:f5994956b1ba 531 }
jah128 13:f5994956b1ba 532 if(avoiding_friend) target_vector.distance = 100;
jah128 13:f5994956b1ba 533 //Set the wheel speed for next action
jah128 13:f5994956b1ba 534 if(target_vector.distance < 120) target_wheel_speed = 0.25;
jah128 13:f5994956b1ba 535 else if(target_vector.distance < 240) target_wheel_speed = 0.35;
jah128 13:f5994956b1ba 536 else if(target_vector.distance < 480) target_wheel_speed = 0.5;
jah128 13:f5994956b1ba 537 else if(target_vector.distance < 960) target_wheel_speed = 0.65;
jah128 13:f5994956b1ba 538 else target_wheel_speed = 0.85;
jah128 13:f5994956b1ba 539 if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
jah128 13:f5994956b1ba 540
jah128 13:f5994956b1ba 541 //Now turn...
jah128 13:f5994956b1ba 542 time_based_turn_degrees(1, (int) target_vector.angle, 1);
jah128 13:f5994956b1ba 543 } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0);
jah128 13:f5994956b1ba 544 }
jah128 12:daa53285b6e4 545
jah128 13:f5994956b1ba 546
jah128 13:f5994956b1ba 547 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 13:f5994956b1ba 548 /// stop_program - Pauses robot
jah128 13:f5994956b1ba 549
jah128 13:f5994956b1ba 550 void stop_program()
jah128 13:f5994956b1ba 551 {
jah128 13:f5994956b1ba 552 if(program_run_init == 1) {
jah128 13:f5994956b1ba 553 save_led_states();
jah128 13:f5994956b1ba 554 set_leds(0,0);
jah128 13:f5994956b1ba 555 set_center_led(0,0);
jah128 13:f5994956b1ba 556 stop();
jah128 13:f5994956b1ba 557 program_run_init = 0;
jah128 13:f5994956b1ba 558 }
jah128 12:daa53285b6e4 559 }
jah128 12:daa53285b6e4 560
jah128 12:daa53285b6e4 561
jah128 13:f5994956b1ba 562
jah128 13:f5994956b1ba 563
jah128 12:daa53285b6e4 564 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 12:daa53285b6e4 565 /// generic_program - Framework for building typical programs
jah128 12:daa53285b6e4 566
jah128 12:daa53285b6e4 567 void generic_program()
jah128 12:daa53285b6e4 568 {
jah128 12:daa53285b6e4 569 // Do something on the first run of a program
jah128 12:daa53285b6e4 570 if(program_run_init == 1) {
jah128 12:daa53285b6e4 571 // Initialisation code goes here...
jah128 12:daa53285b6e4 572
jah128 12:daa53285b6e4 573 program_run_init = 0;
jah128 12:daa53285b6e4 574 }
jah128 12:daa53285b6e4 575
jah128 12:daa53285b6e4 576 // 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 577 if(step_cycle == 0) {
jah128 12:daa53285b6e4 578 // Do something based on sensor data (eg turn)
jah128 12:daa53285b6e4 579 } else {
jah128 12:daa53285b6e4 580 // Do something ignoring sensor data (eg move, or nothing!)
jah128 12:daa53285b6e4 581 }
jah128 12:daa53285b6e4 582
jah128 10:1b09d4bb847b 583 }