File content as of revision 24:d98c6e733dd6:

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

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

/// Include main.h - this includes psiswarm.h all the other necessary core files
#include "main.h"

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

//  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 toggle_program_stage = 0;
char default_normal_program = 5;      // The program to run on turn on (after 'face beacon' program)
char use_recharging_program = 0;         // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
char display_debug_inf = 0;              // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
char main_program_state;                 // Index of the currently running program
char previous_program;                   // Used to hold previous running program when it is paused for switch press etc
char program_changed = 0;                // Flag to update display when program is changed
char program_run_init = 0;               // Flag to tell program to run its initialisation on first loop, if neccessary
char success_count = 0;                  // Flag to indicate the success of a program
char step_cycle = 0;                     // Alternates between 0 and 1 in successive time-steps
char target_reached = 0;                 // Flag to indicate if a program target has been reached
char prog_name [17];                     // Stores the name of the running program [line 0 on the display]
char prog_info [17];                     // Stores information about the current state of the program [line 1 on the display]
char disable_ir_emitters = 0;            // Used to disable IR emission during charging etc [use with caution!]
char recharging_state = 0;               // Stores the state of the recharging program (0 is not currently running)
char switch_held = 0;                    // Used for detected when the cursor switch is held to override program choice
char choose_program_mode = 0;
char program_count = 8;
char program_selection;
Ticker toggle_program_ticker;

float battery_low_threshold = 3.60;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
float battery_high_threshold = 3.95;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0

Ticker main_loop_ticker;


///This is the main loop for the Beautiful Meme code.  The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected.
void main_loop()
{
    if(switch_held == 1)switch_held=2;
    if(switch_held == 3 && choose_program_mode == 0) {
        //The switch has been held right and then released:  stop the current program
        previous_program = main_program_state;
        program_selection = previous_program;
        choose_program_mode = 1;
        set_program(255);
        set_program_info(get_program_name(program_selection));
    }
    if(use_recharging_program == 1)recharging_program();
    update_display();
    if(recharging_state == 0) {
        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) set_program(default_normal_program);
                    } else success_count = 0;
                }
                break;
            case 1:
                target_reached = 0;
                head_to_bearing_program(0);
                if(target_reached == 1) set_program(2);
                break;
            case 2:
                target_reached = 0;
                head_to_bearing_program(180);
                if(target_reached == 1) set_program(1);
                break;
            case 3:
                obs_avoid();
                break;
            case 4:
                straight_random_walk_with_interaction_program();
                break;
            case 5:
                find_space_program(1);
                break;
            case 6:
                clustering_program(0,1);
                break;
            case 7:
                tag_game_program();
                break;
            case 8:
                mockFlocking();
                break;
            case 254:
                init();
                break;
            case 255:
                stop_program();
                break;
        }
    }
    step_cycle=1-step_cycle;
}

///Place user code here that should be run after initialisation but before the main loop
void user_code_setup()
{
    // Note for Alex:
    //
    // This is the code we have been working on for the beautiful meme project
    //
    // If you were to comment out (or delete...) all the code here, the robot will just
    // sit in an 'idle' loop when you turn it on - but should respond to any command codes
    // that are sent.
    
    wait(0.8);
    display.clear_display();
    display.set_position(0,0);
    display.write_string("SCI MUSEUM");
    display.set_position(1,0);
    display.write_string("   PROJECT");
    wait(0.2);
    out("------------------------------------------------------\n");
    out("Dank Meme Project Demo Code                      \n");
    out("------------------------------------------------------\n");
    locate_beacon();
    while(beacon_found == 0) {
        wait(0.5);
        locate_beacon();
    } 
    //toggle_program_ticker.attach(toggle_program,20);
    start_infrared_timers();
    main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
    set_program(4);//for debug ...was 0 ----------
    set_leds(0x00,0x00);
    set_center_led(3,0.5);
    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);
}

void toggle_program(){
    toggle_program_stage ++;
    if(toggle_program_stage == 3) toggle_program_stage = 0;
    switch(toggle_program_stage){
        case 0: set_program(5); break;
        case 1: set_program(6); break;
        case 2: set_program(7);break;
        case 3: set_program(7); break;
    }   
}

/// 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
    if(choose_program_mode == 0) {
        if(switch_state == 8) switch_held = 1;
        else if(switch_state == 0 && switch_held == 2) switch_held = 3;
        else switch_held = 0;
    } else {
        // We are in choose program mode
        if(switch_state == 8) {
            program_selection ++;
            if(program_selection > program_count) program_selection = 0;
            if(program_selection == program_count) set_program_info("RECHARGE");
            else set_program_info(get_program_name(program_selection));
        }
        if(switch_state == 4) {
            if(program_selection == 0) program_selection = program_count; 
            else program_selection --;  
            if(program_selection == program_count) set_program_info("RECHARGE");
            else set_program_info(get_program_name(program_selection));
        }
        if(switch_state == 1 || switch_state == 2){
            if(program_selection == program_count){
                  recharging_state = 1;
                  set_program(previous_program);
                  strcpy(prog_name,"CHARGING PROGRAM");
                  set_program_info("HEAD TO BEACON");  
                  
            }   
            else set_program(program_selection);
            choose_program_mode = 0; 
            switch_held = 0;
        }
    }
    //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info);
}
void handle_user_serial_message(char * message, char length, char interface)
{
    // This is where user code for handling a (non-system) serial message should go
    //
    // message = pointer to message char array
    // length = length of message
    // interface = 0 for PC serial connection, 1 for Bluetooth
    
    char buffer[255];
    sprintf(buffer,message,length);
    for(int i=0; i<length; i++) {
        buffer[i]=message[i];
    }
    buffer[length]=0;
    if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
    else debug("Received USB message:%s [%d chars]\n",buffer,length);
    
    display.clear_display();
    display.set_position(0,0);
    //display.write_string(buffer);
    
    //"A" = 0x41
    if(buffer[0] == 'A'){

        main_program_state = 255;
        display.write_string("MALLARD QUACK QUACK");
    }
    //"B" = 0x42
    else if(buffer[0] == 'B'){
        //set_program(1); 
        main_program_state = 1;
        display.write_string("LAPS (BEACON)");
        }
    //"C" = 0x43
    else if(buffer[0] == 'C'){
        main_program_state = 2;
        display.write_string("LAPS (SOUTH)");
        }
    //"D" = 0x44
    else if(buffer[0] == 'D'){
        main_program_state = 8;
        display.write_string("FLOCKING");
        }
    else if(buffer[0] == 'E'){
        main_program_state = 3;
        display.write_string("RANDOM WALK");
        }
    else if(buffer[0] == 'F'){
        main_program_state = 6;
        display.write_string("AGGREGATION");
        }
    else if(buffer[0] == 'G'){
        main_program_state = 5;
        display.write_string("FIND SPACE");
        }
    else if(buffer[0] == 'H'){
        main_program_state = 6;
        display.write_string("CLUSTERING");
        }
    else if(buffer[0] == 'I'){
        main_program_state = 7;
        display.write_string("TAG");
        }
    else if(buffer[0] == 'R'){
        set_left_motor_speed(0);
        set_right_motor_speed(0);
        init();
        }
    else
    {

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

char * get_program_name(int index)
{
    char * ret_name = new char[17];
    switch(index) {
        case 0:
            strcpy(ret_name,"FACE BEACON");
            break;
        case 1:
            strcpy(ret_name,"HEAD TO BEACON");
            break;
        case 2:
            strcpy(ret_name,"HEAD TO SOUTH");
            break;
        case 3:
            strcpy(ret_name,"RANDOM WALK 1");
            break;
        case 4:
            strcpy(ret_name,"WAITING COMMAND");///gus, was random walk2
            break;
        case 5:
            strcpy(ret_name,"FIND SPACE");
            break;
        case 6:
            strcpy(ret_name,"CLUSTERING");
            break;
        case 7:
            strcpy(ret_name,"TAG GAME");
            break;
        case 255:
            strcpy(ret_name,"PROGRAM:");
            break;
    }
    return ret_name;
}

void set_program(int index)
{
    main_program_state = index;
    program_changed = 1;
    program_run_init = 1;
    strcpy(prog_info,"");
    strcpy(prog_name,get_program_name(index));
}

void set_program_info(char * info)
{
    strcpy(prog_info,info);
    program_changed = 1;
}

void update_display()
{
    if(program_changed == 1) {
        program_changed = 0;
        display.clear_display();

        if(display_debug_inf==1) display_debug_info();
        else {
            display.set_position(0,0);
            display.write_string(prog_name);
        }
        display.set_position(1,0);
        display.write_string(prog_info);
    }
}

void display_debug_info()
{
    char disp_line[16] = "- - - - - - - -";
    if(beacon_found==1)disp_line[0]='B';
    for(int i=1; i<8; i++) {
        if(robots_found[i])disp_line[((i)*2)]=48+i;
    }
    display.set_position(0,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);
    }
}