File content as of revision 11:7b3ee540ba56:

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

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// 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;
int obstacle_avoidance_threshold = 300;

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 

void find_space_program()
{

}