Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of BeautifulMemeProjectBT by
Diff: BeautifulMeme/programs.cpp
- Revision:
- 30:513457c1ad12
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/programs.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,869 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project + +#include "bmeme.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; + } +} + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// tag_game_program + +enum tag_game_role {hunter, hunted}; +enum tag_game_role role; +int tag_distance = 500; +char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters +Timeout hunter_timeout; // Timeout before robots switch back to being hunted +char initial_hunter = 2; // Robot with this ID is permanently a hunter + +// Resets hunter robots to hunted after timeout +void role_reset() +{ + role = hunted; + set_program_info("HUNTED"); + // Clear memory of hunters + for(int i = 0; i < 8; i++) + hunters[i] = 0; +} + +void tag_game_program() +{ + // Initialisation + if(program_run_init == 1) { + + // Set robot roles + if(robot_id == initial_hunter){ + role = hunter; + set_program_info("FIRST HUNTER"); + } + else { + role = hunted; + set_program_info("HUNTED"); + } + // Clear memory of hunters + for(int i = 0; i < 8; i++) + hunters[i] = 0; + + program_run_init = 0; + } + + // Bias forward movement + float left_motor_speed = 0.5; + float right_motor_speed = 0.5; + + // 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; + + // Get heading of sensed obstacle + int heading = get_bearing_from_ir_array(reflected_sensor_data); + + // Turn in opposite direction + if(heading > 0 && heading < 90) + { + left_motor_speed -= 0.75; + right_motor_speed += 0.5; + } + else if(heading < 0 && heading > -90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.75; + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + // Return early - obstacle avoidance is the top priority + return; + } + + 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; + } + } + } + + // If the closest robot is within tag distance, this robot has been tagged + if(shortest_distance > tag_distance) + { + // Switch role to hunter + if(role == hunted) + { + role = hunter; + set_program_info("NEW HUNTER"); + // Reset to hunted after 10 seconds + hunter_timeout.attach(&role_reset, 10); + } + + // Keep a record of which robot tagged them, so hunters do not chase hunters + // Unless they are the initial hunter, who is aggresive towards everyone + if(robot_id != initial_hunter) + hunters[closest_robot] = 1; + } + + if(role == hunter) + { + // Set LEDS to red + set_leds(0x00, 0xFF); + set_center_led(1, 1); + + if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters + { + // Turn towards closest hunted robot (unless it is straight ahead, or behind) + if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.5; + } + else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90) + { + left_motor_speed -= 0.5; + right_motor_speed += 0.5; + } + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + } + else // role == hunted + { + // Set LEDs to green + set_leds(0xFF, 0x00); + set_center_led(2, 1); + + // Avoid everyone + if(closest_robot >= 0) + { + // Turn away from closest robot + if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90) + { + left_motor_speed -= 0.5; + right_motor_speed += 0.5; + } + else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.5; + } + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + } +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// flocking_program + +void flocking_program() +{ + char display_line[16] = " "; + int average_heading = 0; + int number_of_neighbours = 0; + int number_of_flocking_headings = 0; + int target_heading = 0; + int angles = 0; + int ir_sensor_threshold = 600; + int sensors_activated = 0; + + struct FloatVector obstacle_vector; + obstacle_vector.angle = 0; + obstacle_vector.distance = 0; + + struct FloatVector cohesion_vector; + cohesion_vector.angle = 0; + cohesion_vector.distance = 0; + + for(int i = 0; i < 8; i++) + { + if(reflected_sensor_data[i] > ir_sensor_threshold) + { + obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); + sensors_activated++; + } + + if(robots_found[i]) + { + // -180 degrees is reserved for "no byte received" + if(flocking_headings[i] != -180) + { + average_heading += flocking_headings[i]; + number_of_flocking_headings++; + } + + cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]); + number_of_neighbours++; + } + } + + cohesion_vector.distance /= number_of_neighbours; // Normalise + obstacle_vector.distance /= sensors_activated; // Normalise + + int obstacle_avoidance_angle; + + if(sensors_activated > 0) + { + obstacle_avoidance_angle = obstacle_vector.angle + 180; + + if(obstacle_avoidance_angle > 180) + obstacle_avoidance_angle -= 360; + if(obstacle_avoidance_angle < -180) + obstacle_avoidance_angle += 360; + + target_heading += obstacle_avoidance_angle; + angles++; + } + + int cohesion_angle; + + // Don't bother performing cohesion if robots are already close enough + if(number_of_neighbours > 0 && cohesion_vector.distance < 200) + { + cohesion_angle = cohesion_vector.angle; + + if(cohesion_angle > 180) + cohesion_angle -= 360; + if(cohesion_angle < -180) + cohesion_angle += 360; + + target_heading += cohesion_angle; + angles++; + } + + int relative_flocking_heading; + + if(number_of_flocking_headings > 0) + { + average_heading /= number_of_flocking_headings; + + relative_flocking_heading = beacon_heading - average_heading; + + if(relative_flocking_heading > 180) + relative_flocking_heading -= 360; + if(relative_flocking_heading < -180) + relative_flocking_heading += 360; + + target_heading += relative_flocking_heading; + angles++; + } + + if(angles > 0) + target_heading /= angles; // Calculate average + + float left_motor_speed = 0.25; + float right_motor_speed = 0.25; + + if(target_heading > 22.5) + right_motor_speed *= -1; + else if(target_heading < -22.5) + left_motor_speed *= -1; + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + // Only transmit beacon heading if the beacon is visible + if(beacon_found) + { + float degrees_per_value = 256.0f / 360.0f; + char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value + + bt.putc(beacon_heading_byte); + } + + sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading); + + set_program_info(display_line); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// 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!) + } + +} \ No newline at end of file