File content as of revision 9:085e090e1ec1:

/***********************************************************************
**  ██████╗ ███████╗██╗███████╗██╗    ██╗ █████╗ ██████╗ ███╗   ███╗  **
**  ██╔══██╗██╔════╝██║██╔════╝██║    ██║██╔══██╗██╔══██╗████╗ ████║  **
**  ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║  **
**  ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║  **
**  ██║     ███████║██║███████║╚███╔███╔╝██║  ██║██║  ██║██║ ╚═╝ ██║  **
**  ╚═╝     ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝  ╚═╝╚═╝  ╚═╝╚═╝     ╚═╝  **
************************************************************************
**(C) Dr James Hilder - York Robotics Laboratory - University of York **
***********************************************************************/

/// PsiSwarm Beautiful Meme Project Source Code
/// Version 0.1
/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
/// University of York



/// Include psiswarm.h - this includes all the other necessary core files

#include "psiswarm.h"

//  IMPORTANT!!!
//  Do not call the IR functions at all as they will interfere with the correct operation of this program
//  Instead, use the values held in the variables below; they are updated every 500ms



char beacon_found = 0;                   // This will be a 1 when a beacon was detected during the previous 500ms window
int beacon_heading = 0;                  // This is the heading from the last time a beacon was detected
char robots_found[8];                    // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
int robots_heading[8];                   // These are the headings from the last time the respective robots were detected
unsigned short robots_distance[8];       // This is the maximum sensor value from the last time the respective robot was detected
unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on

char * program_name = "B-Meme";
char * author_name  = "YRL";
char * version_name = "1.0";

char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
char display_debug_inf = 1;            // Set to 1 to show debug info on display
char main_program_state;
char old_program_state = 255;
char step_cycle = 0;
char success_count = 0;
char target_reached = 0;
char was_turning = 0;
Ticker main_loop_ticker;


///This is the main loop for the Beautiful Meme code.  The code block is run once every 500mS* once all the IR samples have been collected.
void main_loop()
{
    update_display();
    switch(main_program_state) {
        case 0: //Case 0 is the initial program: turn to face beacon
            if(step_cycle == 0) {
                char turn_status = turn_to_bearing(0);
                if(turn_status == 0) {
                    success_count ++;
                    if(success_count > 1) main_program_state = 1;
                } else success_count = 0;
            }
            break;
        case 1:
            target_reached = 0;
            head_to_bearing_program(0);
            if(target_reached == 1) main_program_state = 2;
            break;
        case 2:
            target_reached = 0;
            head_to_bearing_program(180);
            if(target_reached == 1) main_program_state = 1;
            break;
    }
    step_cycle=1-step_cycle;
}


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


///Place user code here that should be run after initialisation but before the main loop
void user_code_setup()
{
    wait(0.8);
    display.clear_display();
    display.set_position(0,0);
    display.write_string("BEAUTIFUL MEME");
    display.set_position(1,0);
    display.write_string("   PROJECT");
    wait(0.2);
    out("------------------------------------------------------\n");
    out("Beautiful Meme Project Demo Code                      \n");
    out("------------------------------------------------------\n");
    locate_beacon();
    while(beacon_found == 0) {
        wait(0.5);
        locate_beacon();
    }
    start_infrared_timers();
    main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
    main_program_state = 1;
    set_leds(0x00,0x00);
    set_center_led(3,1);
    display.clear_display();
    display.set_position(0,0);
    display.write_string("BEACON FOUND AT");
    display.set_position(1,0);
    char degrees_string[16];
    sprintf(degrees_string,"%d DEGREES",beacon_heading);
    display.write_string(degrees_string);
}

/// Code goes here to handle what should happen when the user switch is pressed
void handle_switch_event(char switch_state)
{
    /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed
    /// NB For maximum compatability it is recommended to minimise reliance on center button press

    //pc.printf("User switch code block: %d\n",switch_state);
}

/// The main routine: it is recommended to leave this function alone and add user code to the above functions
int main()
{
    ///init() in psiswarm.cpp sets up the robot
    init();
    user_code_setup();
    user_code_running = 1;
    while(1) {
        wait(1);
    }
}


void update_display()
{
    if(main_program_state != old_program_state) {
        old_program_state = main_program_state;
        display.clear_display();
        display.set_position(0,0);
        switch(main_program_state) {
            case 0:
                display.write_string("P:FACE BEACON");
                break;
            case 1:
                display.write_string("P:HEAD TO BEACON");
                break;
            case 2:
                display.write_string("P:HEAD TO SOUTH");
                break;
        }
    }
    if(display_debug_inf==1) display_debug_info();
}

void display_debug_info()
{
    char disp_line[16] = "- - - - - - - -";
    if(beacon_found==1)disp_line[0]='B';
    for(int i=0; i<7; i++) {
        if(robots_found[i])disp_line[((i+1)*2)]=49+i;
    }
    display.set_position(1,0);
    display.write_string(disp_line);
}

/// Verbose output
void out(const char* format, ...)
{
    char buffer[256];
    if (debug_mode) {
        va_list vl;
        va_start(vl, format);
        vsprintf(buffer,format,vl);
        if(user_code_debug == 1) pc.printf("%s", buffer);
        va_end(vl);
    }
}