![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: programs.cpp
- Revision:
- 10:1b09d4bb847b
- Child:
- 11:7b3ee540ba56
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/programs.cpp Mon Oct 26 23:58:08 2015 +0000 @@ -0,0 +1,303 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.1 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// Programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project + +#include "main.h" + +float battery_low_threshold = 3.63; // Threshold at which to interrupt program and start recharging routine: suggest 3.55 +float battery_high_threshold = 3.97; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0 + +char was_turning = 0; +enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle}; +enum random_walk_state internal_state = random_walk; +char action_timeout = 0; +char interaction_timeout = 0; +char random_walk_timeout = 0; +float previous_left_motor_speed = 0.5; +float previous_right_motor_speed = 0.5; +int obstacle_avoidance_threshold = 300; +char recharge_power_check_count = 0; +char battery_low_count = 0; + +///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 +void head_to_bearing_program(int target_bearing) +{ + if(step_cycle == 0 || was_turning == 0) { + // Check if we are heading in the right bearing (+- 25 degrees) + int current_bearing = (360 - beacon_heading) % 360; + // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other + char bearing_ok = 0; + int lower_bound = target_bearing - 25; + int upper_bound = target_bearing + 25; + if(lower_bound < 0) { + if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1; + } else if(upper_bound > 359) { + if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1; + } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1; + // Check if there is an obstacle in front of us + if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1; + else { + // Now move forward if we are facing correct bearing, otherwise turn + if(bearing_ok == 1) { + //Check if anything is in front of us to determine speed - if it is, move slowly + int t_time = 6 * BEACON_PERIOD; + float t_speed = 1.0; + if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) { + t_time = 4 * BEACON_PERIOD; + t_speed = 0.6; + } + if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) { + t_time = 3 * BEACON_PERIOD; + t_speed = 0.4; + } + if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) { + t_time = 2 * BEACON_PERIOD; + t_speed = 0.2; + } + time_based_forward(t_speed,t_time,0); + was_turning = 0; + } else { + turn_to_bearing(target_bearing); + was_turning = 1; + } + } + } +} + +/// Recharging program +void recharging_program() +{ + switch(recharging_state) { + case 0: + // We are not currently recharging, check the battery state + if(get_battery_voltage() < battery_low_threshold) { + // Battery is below low threshold + battery_low_count ++; + // 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 + if(battery_low_count > 3) { + // Set recharging state to 'looking for charger' + recharging_state = 1; + strcpy(prog_name,"CHARGING PROGRAM"); + set_program_info("HEAD TO BEACON"); + } + } else battery_low_count = 0; + break; + // State 1: Head to beacon [as this is where battery charger is] + case 1: + target_reached = 0; + head_to_bearing_program(0); + if(target_reached == 1) { + recharging_state = 2; + set_program_info("TURN 90 DEGREES"); + } + break; + // Stage 2: Turn 90 degrees to align with charging pads + case 2: + disable_ir_emitters = 1; + time_based_turn_degrees(0.8, 70.0, 1); + recharge_power_check_count = 0; + recharging_state = 3; + break; + // Stage 3: Wait for turn to complete + case 3: + if (time_based_motor_action != 1) { + recharging_state = 4; + set_program_info("CHECK FOR POWER"); + } + break; + // Stage 4: Check if charging + case 4: + recharge_power_check_count++; + if(get_dc_status() == 1) { + recharging_state = 5; + } else { + if(recharge_power_check_count < 10)recharging_state = 6; + else { + recharging_state = 7; + set_program_info("NO POWER - RETRY"); + } + } + break; + // Stage 5: Charging. Wait until threshold voltage exceeded + case 5: + if(get_battery_voltage() > battery_high_threshold) { + set_program_info("LEAVE CHARGER"); + recharging_state = 7; + } else { + char b_voltage[16]; + sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage()); + set_program_info(b_voltage); + } + break; + // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck + case 6: + time_based_turn_degrees(0.5,4,1); + recharging_state = 4; + break; + // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program + case 7: + time_based_turn_degrees(0.8, 90.0, 1); + recharging_state = 8; + break; + + // Stage 8: Wait for turn to complete + case 8: + if (time_based_motor_action != 1) recharging_state = 9; + break; + // Stage 9: Move away + case 9: + time_based_forward(0.5, 1000000, 1); + recharging_state = 10; + break; + // Stage 10: Wait for move to complete + case 10: + if (time_based_motor_action != 1) recharging_state = 11; + break; + // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle + case 11: + disable_ir_emitters = 0; + if (get_battery_voltage() < battery_low_threshold) { + recharging_state = 1; + } else { + recharging_state = 0; + //Restore name of old program on display + set_program(main_program_state); + } + break; + } +} + +///Alan's Random Walk\Obstacle Avoid and Robot Interaction Program +void curved_random_walk_with_interaction_program() +{ + if(internal_state == random_walk) { + if(interaction_timeout < 4) + interaction_timeout++; + + int closest_robot = -1; + unsigned short shortest_distance = 0; + + // Check whether there are any other robots within range + for(int i = 0; i < 8; i++) { + if(robots_found[i]) { + if(robots_distance[i] > shortest_distance) { + shortest_distance = robots_distance[i]; + closest_robot = i; + } + } + } + + // Turn towards the closest robot + if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) { + time_based_turn_degrees(1, robots_heading[closest_robot], 1); + + action_timeout = 0; + internal_state = turn_towards; + char temp_message[17]; + sprintf(temp_message,"FACE ROBOT %d",closest_robot); + set_program_info(temp_message); + } else { // Otherwise, do a random walk + // Check the front sensors for obstacles + if(reflected_sensor_data[0] > obstacle_avoidance_threshold || + reflected_sensor_data[1] > obstacle_avoidance_threshold || + reflected_sensor_data[6] > obstacle_avoidance_threshold || + reflected_sensor_data[7] > obstacle_avoidance_threshold) { + // Ignore the rear sensors when calculating the heading + reflected_sensor_data[2] = 0; + reflected_sensor_data[3] = 0; + reflected_sensor_data[4] = 0; + reflected_sensor_data[5] = 0; + + // Turn 180 degrees away from the sensed obstacle + int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180; + + // Normalise + heading %= 360; + + if(heading < -180) + heading += 360; + + if(heading > 180) + heading -= 360; + set_program_info("AVOID OBSTACLE"); + time_based_turn_degrees(1, heading, 1); + + action_timeout = 0; + internal_state = turn_away; + } else { + // Change motor speeds every 1s + if(random_walk_timeout >= 2) { + float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5; + + float left_motor_speed = previous_left_motor_speed - random_offset; + float right_motor_speed = previous_right_motor_speed + random_offset; + + float threshold = 0.25; + + if(left_motor_speed < threshold) + left_motor_speed = threshold; + + if(right_motor_speed < threshold) + right_motor_speed = threshold; + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + random_walk_timeout = 0; + } + + random_walk_timeout++; + } + } + } else if(internal_state == turn_towards) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("SAY HELLO"); + vibrate(); + + action_timeout = 0; + internal_state = interact; + } + } else if(internal_state == interact) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("TURN AROUND"); + time_based_turn_degrees(1, 180, 1); + + action_timeout = 0; + internal_state = turn_away; + } + + } else if(internal_state == turn_away) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("RANDOM WALK"); + interaction_timeout = 0; + internal_state = random_walk; + } + } else if(internal_state == avoid_obstacle) { + if(action_timeout < 4) + action_timeout++; + else + set_program_info("RANDOM WALK"); + internal_state = random_walk; + } +} + + +void straight_random_walk_with_interaction_program() +{ + +} + + +void find_space_program() +{ + +} \ No newline at end of file