File content as of revision 21:e5ab8c56a769:

/// 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;
    }
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// 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

Timer timer;

void flocking_program()
{    
    // Do something on the first run of a program
    if(program_run_init == 1)
    {
        // Initialisation code goes here...        
        timer.start();
        program_run_init = 0;
    }
    
    if(timer.read() > 1)
    {
        timer.reset();
             
        int average_heading = 0;
        int number_of_neighbours = 0;
        
        for(int i = 0; i < 8; i++)
        {            
            if(robots_found[i])
            {
                average_heading += flocking_headings[i];
                number_of_neighbours++;
            }
        }
        
        if(number_of_neighbours != 0)
        {
            average_heading /= number_of_neighbours;
        
            out("average heading: %d\n", average_heading);
            
            turn_to_bearing(average_heading + 180);
        }
        
        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
        
//        out("sending beacon heading: %d\n", beacon_heading);
        bt.putc(beacon_heading_byte);
        
//        out("robot_id: %d\n", robot_id);
    }
}

//Timer timer;
//
//void flocking_program()
//{    
//    // Do something on the first run of a program
//    if(program_run_init == 1) {
//        // Initialisation code goes here...
//        
//        timer.start();
//
//        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!)
//    }
//    
//    // Separation
//    // Alignment
//    // Cohesion
//    
//    float left_motor_speed = 0.5;
//    float right_motor_speed = 0.5;
//    
//    struct FloatVector obstacle_vector;
//    obstacle_vector.angle = 0;
//    obstacle_vector.distance = 0;
//    
//    for(int i = 1; i < 8; i++)
//        obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
//        
//    obstacle_vector.distance /= 8; // Normalise
//    
//    struct FloatVector cohesion_vector;
//    cohesion_vector.angle = 0;
//    cohesion_vector.distance = 0;
//    
//    int number_of_neighbours = 0;
//
//    for(int i = 1; i < 8; i++)
//    {
//        if(robots_found[i])
//        {
//            number_of_neighbours++;
//            cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
//        }
//    }
//    
//    cohesion_vector.distance /= number_of_neighbours; // Normalise
//    
////    out("distance: %f, angle: %f\n", cohesion_vector.distance, cohesion_vector.angle);    
//    //out("distance: %f, angle: %f\n", obstacle_vector.distance, obstacle_vector.angle);
//    
//    char robot_nearby = 0;
//    
//    for(int i = 0; i < 8; i++)
//    {
//        if(robots_found[i])
//        {
//            if(robots_heading[i] > -90 && robots_heading[i] < 90)
//            {
//                robot_nearby = 1;
//                break;
//            }
//        }
//    }
//    
//    //if(obstacle_vector.distance > 150 && robot_nearby == 0)
//    if(obstacle_vector.distance > 150)
//    {
////        out("avoiding obstacle\n");
//        
//        if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5)
//            left_motor_speed *= -1;
//        else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5)
//            right_motor_speed *= -1;
//    }
////    else if(cohesion_vector.distance > 400)
////    {
////        if(cohesion_vector.angle > 0)
////            left_motor_speed = 0.25;
////        else if(cohesion_vector.angle < 0)
////            right_motor_speed = 0.25;
////    }
//    else if(cohesion_vector.distance > 0)
//    {     
////        int target_heading = (beacon_heading + flocking_heading) % 360;
////        
////        if(target_heading > 0)
////            right_motor_speed = 0.25;
////        else
////            left_motor_speed = 0.25;
//
////    out("beacon: %d, flocking: %d, target: %d, mod: %d\n", beacon_heading, flocking_heading, beacon_heading + flocking_heading, (beacon_heading + flocking_heading) % 360);
//       
//        if(cohesion_vector.angle < 0)
//            left_motor_speed = 0.25;
//        else if(cohesion_vector.angle > 0)
//            right_motor_speed = 0.25;
//            
//        // Set LEDs to green        
//        set_leds(0xFF, 0x00);
//        set_center_led(2, 1);
//    }
//    else
//    {
//        // Set LEDS to red
//        set_leds(0x00, 0xFF);
//        set_center_led(1, 1);
//    }
//    
////    out("left_motor_speed: %f, right_motor_speed: %f\n", left_motor_speed, right_motor_speed);
//    
//    set_left_motor_speed(left_motor_speed);
//    set_right_motor_speed(right_motor_speed);
//}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// 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!)
    }

}