James Hilder
Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of
BeautifulMemeProjectBT
by 
Alan Millard
Diff: programs.cpp
- Revision:
- 30:513457c1ad12
- Parent:
- 29:9756004e8499
--- a/programs.cpp Thu Mar 03 23:22:01 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,869 +0,0 @@
-/// PsiSwarm Beautiful Meme Project Source Code
-/// Version 0.4
-/// 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;
- }
-}
-
-
-
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// 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