Alan Millard / Mbed 2 deprecated BeautifulMemeProjectBT

Bluetooth communication for flocking.

Dependencies:   mbed

Fork of BeautifulMemeProject by James Hilder

programs.cpp@11:7b3ee540ba56, 2015-10-27 (annotated)

Committer:
jah128
Date:
Tue Oct 27 00:13:49 2015 +0000
Revision:
11:7b3ee540ba56
Parent:
10:1b09d4bb847b
Child:
12:daa53285b6e4
Cleaned+reordered

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 11:7b3ee540ba56 10 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 11 /// head_to_bearing_program
jah128 10:1b09d4bb847b 12 char was_turning = 0;
jah128 10:1b09d4bb847b 13
jah128 10:1b09d4bb847b 14 ///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 15 void head_to_bearing_program(int target_bearing)
jah128 10:1b09d4bb847b 16 {
jah128 10:1b09d4bb847b 17 if(step_cycle == 0 || was_turning == 0) {
jah128 10:1b09d4bb847b 18 // Check if we are heading in the right bearing (+- 25 degrees)
jah128 10:1b09d4bb847b 19 int current_bearing = (360 - beacon_heading) % 360;
jah128 10:1b09d4bb847b 20 // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
jah128 10:1b09d4bb847b 21 char bearing_ok = 0;
jah128 10:1b09d4bb847b 22 int lower_bound = target_bearing - 25;
jah128 10:1b09d4bb847b 23 int upper_bound = target_bearing + 25;
jah128 10:1b09d4bb847b 24 if(lower_bound < 0) {
jah128 10:1b09d4bb847b 25 if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
jah128 10:1b09d4bb847b 26 } else if(upper_bound > 359) {
jah128 10:1b09d4bb847b 27 if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
jah128 10:1b09d4bb847b 28 } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
jah128 10:1b09d4bb847b 29 // Check if there is an obstacle in front of us
jah128 10:1b09d4bb847b 30 if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
jah128 10:1b09d4bb847b 31 else {
jah128 10:1b09d4bb847b 32 // Now move forward if we are facing correct bearing, otherwise turn
jah128 10:1b09d4bb847b 33 if(bearing_ok == 1) {
jah128 10:1b09d4bb847b 34 //Check if anything is in front of us to determine speed - if it is, move slowly
jah128 10:1b09d4bb847b 35 int t_time = 6 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 36 float t_speed = 1.0;
jah128 10:1b09d4bb847b 37 if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
jah128 10:1b09d4bb847b 38 t_time = 4 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 39 t_speed = 0.6;
jah128 10:1b09d4bb847b 40 }
jah128 10:1b09d4bb847b 41 if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
jah128 10:1b09d4bb847b 42 t_time = 3 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 43 t_speed = 0.4;
jah128 10:1b09d4bb847b 44 }
jah128 10:1b09d4bb847b 45 if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
jah128 10:1b09d4bb847b 46 t_time = 2 * BEACON_PERIOD;
jah128 10:1b09d4bb847b 47 t_speed = 0.2;
jah128 10:1b09d4bb847b 48 }
jah128 10:1b09d4bb847b 49 time_based_forward(t_speed,t_time,0);
jah128 10:1b09d4bb847b 50 was_turning = 0;
jah128 10:1b09d4bb847b 51 } else {
jah128 10:1b09d4bb847b 52 turn_to_bearing(target_bearing);
jah128 10:1b09d4bb847b 53 was_turning = 1;
jah128 10:1b09d4bb847b 54 }
jah128 10:1b09d4bb847b 55 }
jah128 10:1b09d4bb847b 56 }
jah128 10:1b09d4bb847b 57 }
jah128 10:1b09d4bb847b 58
jah128 11:7b3ee540ba56 59
jah128 11:7b3ee540ba56 60
jah128 11:7b3ee540ba56 61
jah128 11:7b3ee540ba56 62 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 63 /// recharging_program
jah128 11:7b3ee540ba56 64
jah128 11:7b3ee540ba56 65 char recharge_power_check_count = 0;
jah128 11:7b3ee540ba56 66 char battery_low_count = 0;
jah128 11:7b3ee540ba56 67
jah128 10:1b09d4bb847b 68 void recharging_program()
jah128 10:1b09d4bb847b 69 {
jah128 10:1b09d4bb847b 70 switch(recharging_state) {
jah128 10:1b09d4bb847b 71 case 0:
jah128 10:1b09d4bb847b 72 // We are not currently recharging, check the battery state
jah128 10:1b09d4bb847b 73 if(get_battery_voltage() < battery_low_threshold) {
jah128 10:1b09d4bb847b 74 // Battery is below low threshold
jah128 10:1b09d4bb847b 75 battery_low_count ++;
jah128 10:1b09d4bb847b 76 // 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 77 if(battery_low_count > 3) {
jah128 10:1b09d4bb847b 78 // Set recharging state to 'looking for charger'
jah128 10:1b09d4bb847b 79 recharging_state = 1;
jah128 10:1b09d4bb847b 80 strcpy(prog_name,"CHARGING PROGRAM");
jah128 10:1b09d4bb847b 81 set_program_info("HEAD TO BEACON");
jah128 10:1b09d4bb847b 82 }
jah128 10:1b09d4bb847b 83 } else battery_low_count = 0;
jah128 10:1b09d4bb847b 84 break;
jah128 10:1b09d4bb847b 85 // State 1: Head to beacon [as this is where battery charger is]
jah128 10:1b09d4bb847b 86 case 1:
jah128 10:1b09d4bb847b 87 target_reached = 0;
jah128 10:1b09d4bb847b 88 head_to_bearing_program(0);
jah128 10:1b09d4bb847b 89 if(target_reached == 1) {
jah128 10:1b09d4bb847b 90 recharging_state = 2;
jah128 10:1b09d4bb847b 91 set_program_info("TURN 90 DEGREES");
jah128 10:1b09d4bb847b 92 }
jah128 10:1b09d4bb847b 93 break;
jah128 10:1b09d4bb847b 94 // Stage 2: Turn 90 degrees to align with charging pads
jah128 10:1b09d4bb847b 95 case 2:
jah128 10:1b09d4bb847b 96 disable_ir_emitters = 1;
jah128 10:1b09d4bb847b 97 time_based_turn_degrees(0.8, 70.0, 1);
jah128 10:1b09d4bb847b 98 recharge_power_check_count = 0;
jah128 10:1b09d4bb847b 99 recharging_state = 3;
jah128 10:1b09d4bb847b 100 break;
jah128 10:1b09d4bb847b 101 // Stage 3: Wait for turn to complete
jah128 10:1b09d4bb847b 102 case 3:
jah128 10:1b09d4bb847b 103 if (time_based_motor_action != 1) {
jah128 10:1b09d4bb847b 104 recharging_state = 4;
jah128 10:1b09d4bb847b 105 set_program_info("CHECK FOR POWER");
jah128 10:1b09d4bb847b 106 }
jah128 10:1b09d4bb847b 107 break;
jah128 10:1b09d4bb847b 108 // Stage 4: Check if charging
jah128 10:1b09d4bb847b 109 case 4:
jah128 10:1b09d4bb847b 110 recharge_power_check_count++;
jah128 10:1b09d4bb847b 111 if(get_dc_status() == 1) {
jah128 10:1b09d4bb847b 112 recharging_state = 5;
jah128 10:1b09d4bb847b 113 } else {
jah128 10:1b09d4bb847b 114 if(recharge_power_check_count < 10)recharging_state = 6;
jah128 10:1b09d4bb847b 115 else {
jah128 10:1b09d4bb847b 116 recharging_state = 7;
jah128 10:1b09d4bb847b 117 set_program_info("NO POWER - RETRY");
jah128 10:1b09d4bb847b 118 }
jah128 10:1b09d4bb847b 119 }
jah128 10:1b09d4bb847b 120 break;
jah128 10:1b09d4bb847b 121 // Stage 5: Charging. Wait until threshold voltage exceeded
jah128 10:1b09d4bb847b 122 case 5:
jah128 10:1b09d4bb847b 123 if(get_battery_voltage() > battery_high_threshold) {
jah128 10:1b09d4bb847b 124 set_program_info("LEAVE CHARGER");
jah128 10:1b09d4bb847b 125 recharging_state = 7;
jah128 10:1b09d4bb847b 126 } else {
jah128 10:1b09d4bb847b 127 char b_voltage[16];
jah128 10:1b09d4bb847b 128 sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
jah128 10:1b09d4bb847b 129 set_program_info(b_voltage);
jah128 10:1b09d4bb847b 130 }
jah128 10:1b09d4bb847b 131 break;
jah128 10:1b09d4bb847b 132 // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck
jah128 10:1b09d4bb847b 133 case 6:
jah128 10:1b09d4bb847b 134 time_based_turn_degrees(0.5,4,1);
jah128 10:1b09d4bb847b 135 recharging_state = 4;
jah128 10:1b09d4bb847b 136 break;
jah128 10:1b09d4bb847b 137 // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program
jah128 10:1b09d4bb847b 138 case 7:
jah128 10:1b09d4bb847b 139 time_based_turn_degrees(0.8, 90.0, 1);
jah128 10:1b09d4bb847b 140 recharging_state = 8;
jah128 10:1b09d4bb847b 141 break;
jah128 10:1b09d4bb847b 142
jah128 10:1b09d4bb847b 143 // Stage 8: Wait for turn to complete
jah128 10:1b09d4bb847b 144 case 8:
jah128 10:1b09d4bb847b 145 if (time_based_motor_action != 1) recharging_state = 9;
jah128 10:1b09d4bb847b 146 break;
jah128 10:1b09d4bb847b 147 // Stage 9: Move away
jah128 10:1b09d4bb847b 148 case 9:
jah128 10:1b09d4bb847b 149 time_based_forward(0.5, 1000000, 1);
jah128 10:1b09d4bb847b 150 recharging_state = 10;
jah128 10:1b09d4bb847b 151 break;
jah128 10:1b09d4bb847b 152 // Stage 10: Wait for move to complete
jah128 10:1b09d4bb847b 153 case 10:
jah128 10:1b09d4bb847b 154 if (time_based_motor_action != 1) recharging_state = 11;
jah128 10:1b09d4bb847b 155 break;
jah128 10:1b09d4bb847b 156 // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle
jah128 10:1b09d4bb847b 157 case 11:
jah128 10:1b09d4bb847b 158 disable_ir_emitters = 0;
jah128 10:1b09d4bb847b 159 if (get_battery_voltage() < battery_low_threshold) {
jah128 10:1b09d4bb847b 160 recharging_state = 1;
jah128 10:1b09d4bb847b 161 } else {
jah128 10:1b09d4bb847b 162 recharging_state = 0;
jah128 10:1b09d4bb847b 163 //Restore name of old program on display
jah128 10:1b09d4bb847b 164 set_program(main_program_state);
jah128 10:1b09d4bb847b 165 }
jah128 10:1b09d4bb847b 166 break;
jah128 10:1b09d4bb847b 167 }
jah128 10:1b09d4bb847b 168 }
jah128 10:1b09d4bb847b 169
jah128 11:7b3ee540ba56 170
jah128 11:7b3ee540ba56 171
jah128 11:7b3ee540ba56 172
jah128 11:7b3ee540ba56 173 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 174 /// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program)
jah128 11:7b3ee540ba56 175
jah128 11:7b3ee540ba56 176 enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
jah128 11:7b3ee540ba56 177 enum random_walk_state internal_state = random_walk;
jah128 11:7b3ee540ba56 178 char action_timeout = 0;
jah128 11:7b3ee540ba56 179 char interaction_timeout = 0;
jah128 11:7b3ee540ba56 180 char random_walk_timeout = 0;
jah128 11:7b3ee540ba56 181 float previous_left_motor_speed = 0.5;
jah128 11:7b3ee540ba56 182 float previous_right_motor_speed = 0.5;
jah128 11:7b3ee540ba56 183 int obstacle_avoidance_threshold = 300;
jah128 11:7b3ee540ba56 184
jah128 10:1b09d4bb847b 185 void curved_random_walk_with_interaction_program()
jah128 10:1b09d4bb847b 186 {
jah128 10:1b09d4bb847b 187 if(internal_state == random_walk) {
jah128 10:1b09d4bb847b 188 if(interaction_timeout < 4)
jah128 10:1b09d4bb847b 189 interaction_timeout++;
jah128 10:1b09d4bb847b 190
jah128 10:1b09d4bb847b 191 int closest_robot = -1;
jah128 10:1b09d4bb847b 192 unsigned short shortest_distance = 0;
jah128 10:1b09d4bb847b 193
jah128 10:1b09d4bb847b 194 // Check whether there are any other robots within range
jah128 10:1b09d4bb847b 195 for(int i = 0; i < 8; i++) {
jah128 10:1b09d4bb847b 196 if(robots_found[i]) {
jah128 10:1b09d4bb847b 197 if(robots_distance[i] > shortest_distance) {
jah128 10:1b09d4bb847b 198 shortest_distance = robots_distance[i];
jah128 10:1b09d4bb847b 199 closest_robot = i;
jah128 10:1b09d4bb847b 200 }
jah128 10:1b09d4bb847b 201 }
jah128 10:1b09d4bb847b 202 }
jah128 10:1b09d4bb847b 203
jah128 10:1b09d4bb847b 204 // Turn towards the closest robot
jah128 10:1b09d4bb847b 205 if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
jah128 10:1b09d4bb847b 206 time_based_turn_degrees(1, robots_heading[closest_robot], 1);
jah128 10:1b09d4bb847b 207
jah128 10:1b09d4bb847b 208 action_timeout = 0;
jah128 10:1b09d4bb847b 209 internal_state = turn_towards;
jah128 10:1b09d4bb847b 210 char temp_message[17];
jah128 10:1b09d4bb847b 211 sprintf(temp_message,"FACE ROBOT %d",closest_robot);
jah128 10:1b09d4bb847b 212 set_program_info(temp_message);
jah128 10:1b09d4bb847b 213 } else { // Otherwise, do a random walk
jah128 10:1b09d4bb847b 214 // Check the front sensors for obstacles
jah128 10:1b09d4bb847b 215 if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 216 reflected_sensor_data[1] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 217 reflected_sensor_data[6] > obstacle_avoidance_threshold ||
jah128 10:1b09d4bb847b 218 reflected_sensor_data[7] > obstacle_avoidance_threshold) {
jah128 10:1b09d4bb847b 219 // Ignore the rear sensors when calculating the heading
jah128 10:1b09d4bb847b 220 reflected_sensor_data[2] = 0;
jah128 10:1b09d4bb847b 221 reflected_sensor_data[3] = 0;
jah128 10:1b09d4bb847b 222 reflected_sensor_data[4] = 0;
jah128 10:1b09d4bb847b 223 reflected_sensor_data[5] = 0;
jah128 10:1b09d4bb847b 224
jah128 10:1b09d4bb847b 225 // Turn 180 degrees away from the sensed obstacle
jah128 10:1b09d4bb847b 226 int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
jah128 10:1b09d4bb847b 227
jah128 10:1b09d4bb847b 228 // Normalise
jah128 10:1b09d4bb847b 229 heading %= 360;
jah128 10:1b09d4bb847b 230
jah128 10:1b09d4bb847b 231 if(heading < -180)
jah128 10:1b09d4bb847b 232 heading += 360;
jah128 10:1b09d4bb847b 233
jah128 10:1b09d4bb847b 234 if(heading > 180)
jah128 10:1b09d4bb847b 235 heading -= 360;
jah128 10:1b09d4bb847b 236 set_program_info("AVOID OBSTACLE");
jah128 10:1b09d4bb847b 237 time_based_turn_degrees(1, heading, 1);
jah128 10:1b09d4bb847b 238
jah128 10:1b09d4bb847b 239 action_timeout = 0;
jah128 10:1b09d4bb847b 240 internal_state = turn_away;
jah128 10:1b09d4bb847b 241 } else {
jah128 10:1b09d4bb847b 242 // Change motor speeds every 1s
jah128 10:1b09d4bb847b 243 if(random_walk_timeout >= 2) {
jah128 10:1b09d4bb847b 244 float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
jah128 10:1b09d4bb847b 245
jah128 10:1b09d4bb847b 246 float left_motor_speed = previous_left_motor_speed - random_offset;
jah128 10:1b09d4bb847b 247 float right_motor_speed = previous_right_motor_speed + random_offset;
jah128 10:1b09d4bb847b 248
jah128 10:1b09d4bb847b 249 float threshold = 0.25;
jah128 10:1b09d4bb847b 250
jah128 10:1b09d4bb847b 251 if(left_motor_speed < threshold)
jah128 10:1b09d4bb847b 252 left_motor_speed = threshold;
jah128 10:1b09d4bb847b 253
jah128 10:1b09d4bb847b 254 if(right_motor_speed < threshold)
jah128 10:1b09d4bb847b 255 right_motor_speed = threshold;
jah128 10:1b09d4bb847b 256
jah128 10:1b09d4bb847b 257 set_left_motor_speed(left_motor_speed);
jah128 10:1b09d4bb847b 258 set_right_motor_speed(right_motor_speed);
jah128 10:1b09d4bb847b 259
jah128 10:1b09d4bb847b 260 random_walk_timeout = 0;
jah128 10:1b09d4bb847b 261 }
jah128 10:1b09d4bb847b 262
jah128 10:1b09d4bb847b 263 random_walk_timeout++;
jah128 10:1b09d4bb847b 264 }
jah128 10:1b09d4bb847b 265 }
jah128 10:1b09d4bb847b 266 } else if(internal_state == turn_towards) {
jah128 10:1b09d4bb847b 267 if(action_timeout < 4)
jah128 10:1b09d4bb847b 268 action_timeout++;
jah128 10:1b09d4bb847b 269 else {
jah128 10:1b09d4bb847b 270 set_program_info("SAY HELLO");
jah128 10:1b09d4bb847b 271 vibrate();
jah128 10:1b09d4bb847b 272
jah128 10:1b09d4bb847b 273 action_timeout = 0;
jah128 10:1b09d4bb847b 274 internal_state = interact;
jah128 10:1b09d4bb847b 275 }
jah128 10:1b09d4bb847b 276 } else if(internal_state == interact) {
jah128 10:1b09d4bb847b 277 if(action_timeout < 4)
jah128 10:1b09d4bb847b 278 action_timeout++;
jah128 10:1b09d4bb847b 279 else {
jah128 10:1b09d4bb847b 280 set_program_info("TURN AROUND");
jah128 10:1b09d4bb847b 281 time_based_turn_degrees(1, 180, 1);
jah128 10:1b09d4bb847b 282
jah128 10:1b09d4bb847b 283 action_timeout = 0;
jah128 10:1b09d4bb847b 284 internal_state = turn_away;
jah128 10:1b09d4bb847b 285 }
jah128 10:1b09d4bb847b 286
jah128 10:1b09d4bb847b 287 } else if(internal_state == turn_away) {
jah128 10:1b09d4bb847b 288 if(action_timeout < 4)
jah128 10:1b09d4bb847b 289 action_timeout++;
jah128 10:1b09d4bb847b 290 else {
jah128 10:1b09d4bb847b 291 set_program_info("RANDOM WALK");
jah128 10:1b09d4bb847b 292 interaction_timeout = 0;
jah128 10:1b09d4bb847b 293 internal_state = random_walk;
jah128 10:1b09d4bb847b 294 }
jah128 10:1b09d4bb847b 295 } else if(internal_state == avoid_obstacle) {
jah128 10:1b09d4bb847b 296 if(action_timeout < 4)
jah128 10:1b09d4bb847b 297 action_timeout++;
jah128 10:1b09d4bb847b 298 else
jah128 10:1b09d4bb847b 299 set_program_info("RANDOM WALK");
jah128 10:1b09d4bb847b 300 internal_state = random_walk;
jah128 10:1b09d4bb847b 301 }
jah128 10:1b09d4bb847b 302 }
jah128 10:1b09d4bb847b 303
jah128 10:1b09d4bb847b 304
jah128 11:7b3ee540ba56 305
jah128 11:7b3ee540ba56 306
jah128 11:7b3ee540ba56 307
jah128 11:7b3ee540ba56 308 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 309 /// straight_random_walk_with_interaction_program
jah128 11:7b3ee540ba56 310
jah128 10:1b09d4bb847b 311 void straight_random_walk_with_interaction_program()
jah128 10:1b09d4bb847b 312 {
jah128 10:1b09d4bb847b 313
jah128 10:1b09d4bb847b 314 }
jah128 10:1b09d4bb847b 315
jah128 10:1b09d4bb847b 316
jah128 11:7b3ee540ba56 317
jah128 11:7b3ee540ba56 318
jah128 11:7b3ee540ba56 319
jah128 11:7b3ee540ba56 320 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128 11:7b3ee540ba56 321 /// find_space_program
jah128 11:7b3ee540ba56 322
jah128 10:1b09d4bb847b 323 void find_space_program()
jah128 10:1b09d4bb847b 324 {
jah128 10:1b09d4bb847b 325
jah128 10:1b09d4bb847b 326 }