![](/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-28
- Revision:
- 14:f623db1e6184
- Parent:
- 13:f5994956b1ba
- Child:
- 16:976a1d0ea897
File content as of revision 14:f623db1e6184:
/// PsiSwarm Beautiful Meme Project Source Code /// Version 0.2 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis /// University of York // programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project #include "main.h" int obstacle_avoidance_threshold = 300; int robot_avoidance_threshold = 2000; ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// head_to_bearing_program char was_turning = 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 char recharge_power_check_count = 0; char battery_low_count = 0; 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; } } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program) 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; 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; } } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// straight_random_walk_with_interaction_program void straight_random_walk_with_interaction_program() { } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// find_space_program char prog_debug = 1 ; float target_wheel_speed; int random_walk_bearing; void find_space_program(char bidirectional) { // The find_space_program is a continuous turn-move vector program if(program_run_init == 1) { // Setup the LEDs to red set_leds(0x00,0xFF); set_center_led(1,1); program_run_init = 0; random_walk_bearing = rand() % 360; } // When step_cycle = 0 we calculate a vector to move to and a target distance if(step_cycle == 0) { struct FloatVector target_vector; target_vector.angle = 0; target_vector.distance = 0; // Check for near robots within range for(int i = 1; i < 8; i++) { if(robots_found[i]) { int res_bearing = robots_heading[i]; res_bearing += 180; if(res_bearing > 180) res_bearing -= 360; target_vector = addVector(target_vector,res_bearing,robots_distance[i]); 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); } } if(target_vector.angle!=0){ set_leds(0xFF,0xFF); set_center_led(3,1); } // Check for obstacles char obstacle = 0; int peak_strength = 0; for(int i=0; i<8; i++){ if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; } if(obstacle){ //Choose new random walk bearing set_leds(0x00,0xFF); set_center_led(1,1); random_walk_bearing = rand() % 360; int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); int obs_bearing = obstacle_bearing + 180; if(obs_bearing > 180) obs_bearing -= 360; target_vector = addVector(target_vector,obs_bearing,peak_strength); 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); } if(target_vector.angle == 0 && target_vector.distance == 0){ set_leds(0xFF,0x00); set_center_led(2,1); random_walk_bearing += 180; if(random_walk_bearing > 360) random_walk_bearing -= 360; target_vector.distance = 100; int current_bearing = 360 - beacon_heading; //Now work out turn needed to face intended heading int target_turn = (random_walk_bearing - current_bearing) % 360; if(target_turn > 180) target_turn -= 360; if(target_turn < -180) target_turn += 360; target_vector.angle = target_turn; if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); } char wheel_direction = 1; if(bidirectional){ // Allow reverse wheel direction if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} } //Now turn to angle float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); if(target_vector.angle < 0){ if(target_vector.angle < -maximum_turn_angle){ target_vector.angle = -maximum_turn_angle; target_vector.distance = 100; } }else{ if(target_vector.angle > maximum_turn_angle){ target_vector.angle = maximum_turn_angle; target_vector.distance = 100; } } //Set the wheel speed for next action if(target_vector.distance < 120) target_wheel_speed = 0.25; else if(target_vector.distance < 240) target_wheel_speed = 0.35; else if(target_vector.distance < 480) target_wheel_speed = 0.45; else if(target_vector.distance < 960) target_wheel_speed = 0.55; else target_wheel_speed = 0.65; if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; //Now turn... time_based_turn_degrees(1, (int) target_vector.angle, 1); } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0); } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// clustering_program void clustering_program(char invert, char bidirectional) { // The clustering program is a continuous turn-move vector program // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group // Store the robot group: even robots (0) are green, odd robots (1) are red char group = robot_id % 2; if(program_run_init == 1) { // Setup the LEDs based on robot_id if(group == 0) { set_leds(0xFF,0x00); set_center_led(2,1); } else { set_leds(0x00,0xFF); set_center_led(1,1); } program_run_init = 0; random_walk_bearing = rand() % 360; } // When step_cycle = 0 we calculate a vector to move to and a target distance if(step_cycle == 0) { char avoiding_friend = 0; struct FloatVector target_vector; target_vector.angle = 0; target_vector.distance = 0; // Check for near robots within range for(int i = 1; i < 8; i++) { if(robots_found[i]) { // Determine if the robot is an attractor or a repellor char attract = 0; if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1; // Avoid very close attractors to stop collisions if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;} int res_bearing = robots_heading[i]; if(attract==0){ res_bearing += 180; if(res_bearing > 180) res_bearing -= 360; } target_vector = addVector(target_vector,res_bearing,robots_distance[i]); if(prog_debug) { if(attract) { 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); } else { 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); } } } } // Check for obstacles char obstacle = 0; int peak_strength = 0; for(int i=0; i<8; i++){ if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; } if(obstacle){ //Choose new random walk bearing random_walk_bearing = rand() % 360; int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); int obs_bearing = obstacle_bearing + 180; if(obs_bearing > 180) obs_bearing -= 360; target_vector = addVector(target_vector,obs_bearing,peak_strength); 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); } if(target_vector.angle == 0 && target_vector.distance == 0){ //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing if(rand() % 100 > 97) random_walk_bearing = rand() % 360; target_vector.distance = 100; int current_bearing = 360 - beacon_heading; //Now work out turn needed to face intended heading int target_turn = (random_walk_bearing - current_bearing) % 360; if(target_turn > 180) target_turn -= 360; if(target_turn < -180) target_turn += 360; target_vector.angle = target_turn; if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); } char wheel_direction = 1; if(bidirectional){ // Allow reverse wheel direction if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} } //Now turn to angle float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); if(target_vector.angle < 0){ if(target_vector.angle < -maximum_turn_angle){ target_vector.angle = -maximum_turn_angle; target_vector.distance = 100; } }else{ if(target_vector.angle > maximum_turn_angle){ target_vector.angle = maximum_turn_angle; target_vector.distance = 100; } } if(avoiding_friend) target_vector.distance = 100; //Set the wheel speed for next action if(target_vector.distance < 120) target_wheel_speed = 0.25; else if(target_vector.distance < 240) target_wheel_speed = 0.35; else if(target_vector.distance < 480) target_wheel_speed = 0.5; else if(target_vector.distance < 960) target_wheel_speed = 0.65; else target_wheel_speed = 0.85; if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; //Now turn... time_based_turn_degrees(1, (int) target_vector.angle, 1); } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0); } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// stop_program - Pauses robot void stop_program() { if(program_run_init == 1) { save_led_states(); set_leds(0,0); set_center_led(0,0); stop(); program_run_init = 0; } } ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// generic_program - Framework for building typical programs void generic_program() { // Do something on the first run of a program if(program_run_init == 1) { // Initialisation code goes here... program_run_init = 0; } // 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) if(step_cycle == 0) { // Do something based on sensor data (eg turn) } else { // Do something ignoring sensor data (eg move, or nothing!) } }