![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
programs.cpp
- Committer:
- jah128
- Date:
- 2015-10-26
- Revision:
- 10:1b09d4bb847b
- Child:
- 11:7b3ee540ba56
File content as of revision 10:1b09d4bb847b:
/// 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() { }