main.cpp

00001 /***********************************************************************
00002 **  ██████╗ ███████╗██╗███████╗██╗    ██╗ █████╗ ██████╗ ███╗   ███╗  **
00003 **  ██╔══██╗██╔════╝██║██╔════╝██║    ██║██╔══██╗██╔══██╗████╗ ████║  **
00004 **  ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║  **
00005 **  ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║  **
00006 **  ██║     ███████║██║███████║╚███╔███╔╝██║  ██║██║  ██║██║ ╚═╝ ██║  **
00007 **  ╚═╝     ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝  ╚═╝╚═╝  ╚═╝╚═╝     ╚═╝  **
00008 ************************************************************************
00009 **(C) Dr James Hilder - York Robotics Laboratory - University of York **
00010 ***********************************************************************/
00011 
00012 /// PsiSwarm Beautiful Meme Project Source Code
00013 /// Version 0.21
00014 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
00015 /// University of York
00016 
00017 /// Include main.h - this includes psiswarm.h all the other necessary core files
00018 #include "main.h"
00019 
00020 char * program_name = "B-Meme";
00021 char * author_name  = "YRL";
00022 char * version_name = "0.21";
00023 
00024 //  IMPORTANT!!!
00025 //  Do not call the IR functions at all as they will interfere with the correct operation of this program
00026 //  Instead, use the values held in the variables below; they are updated every 500ms
00027 
00028 char beacon_found = 0;                   // This will be a 1 when a beacon was detected during the previous 500ms window
00029 int beacon_heading = 0;                  // This is the heading from the last time a beacon was detected
00030 char robots_found[8];                    // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
00031 int robots_heading[8];                   // These are the headings from the last time the respective robots were detected
00032 unsigned short robots_distance[8];       // This is the maximum sensor value from the last time the respective robot was detected
00033 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
00034 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
00035 
00036 char default_normal_program = 8;         // The program to run on turn on (after 'face beacon' program)
00037 char use_recharging_program = 0;         // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
00038 char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
00039 char display_debug_inf = 0;              // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
00040 char main_program_state;                 // Index of the currently running program
00041 char previous_program;                   // Used to hold previous running program when it is paused for switch press etc
00042 char program_changed = 0;                // Flag to update display when program is changed
00043 char program_run_init = 0;               // Flag to tell program to run its initialisation on first loop, if neccessary
00044 char success_count = 0;                  // Flag to indicate the success of a program
00045 char step_cycle = 0;                     // Alternates between 0 and 1 in successive time-steps
00046 char target_reached = 0;                 // Flag to indicate if a program target has been reached
00047 char prog_name [17];                     // Stores the name of the running program [line 0 on the display]
00048 char prog_info [17];                     // Stores information about the current state of the program [line 1 on the display]
00049 char disable_ir_emitters = 0;            // Used to disable IR emission during charging etc [use with caution!]
00050 char recharging_state = 0;               // Stores the state of the recharging program (0 is not currently running)
00051 char switch_held = 0;                    // Used for detected when the cursor switch is held to override program choice
00052 char choose_program_mode = 0;
00053 char program_count = 8;
00054 char program_selection;
00055 int flocking_headings[8];                // Beacon heading of each robot
00056 
00057 float battery_low_threshold = 3.60;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
00058 float battery_high_threshold = 3.95;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
00059 
00060 Ticker main_loop_ticker;
00061 
00062 ///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.
00063 void main_loop()
00064 {
00065     if(switch_held == 1)switch_held=2;
00066     if(switch_held == 3 && choose_program_mode == 0) {
00067         //The switch has been held right and then released:  stop the current program
00068         previous_program = main_program_state;
00069         program_selection = previous_program;
00070         choose_program_mode = 1;
00071         set_program(255);
00072         set_program_info(get_program_name(program_selection));
00073     }
00074     if(use_recharging_program == 1)recharging_program();
00075     update_display();
00076     if(recharging_state == 0) {
00077         switch(main_program_state) {
00078             case 0: //Case 0 is the initial program: turn to face beacon
00079                 if(step_cycle == 0) {
00080                     char turn_status = turn_to_bearing(0);
00081                     if(turn_status == 0) {
00082                         success_count ++;
00083                         if(success_count > 1) set_program(default_normal_program);
00084                     } else success_count = 0;
00085                 }
00086                 break;
00087             case 1:
00088                 target_reached = 0;
00089                 head_to_bearing_program(0);
00090                 if(target_reached == 1) set_program(2);
00091                 break;
00092             case 2:
00093                 target_reached = 0;
00094                 head_to_bearing_program(180);
00095                 if(target_reached == 1) set_program(1);
00096                 break;
00097             case 3:
00098                 curved_random_walk_with_interaction_program();
00099                 break;
00100             case 4:
00101                 straight_random_walk_with_interaction_program();
00102                 break;
00103             case 5:
00104                 find_space_program(1);
00105                 break;
00106             case 6:
00107                 clustering_program(0,1);
00108                 break;
00109             case 7:
00110                 tag_game_program();
00111                 break;
00112             case 8:
00113                 flocking_program();
00114                 break;
00115             case 255:
00116                 stop_program();
00117                 break;
00118         }
00119     }
00120     step_cycle=1-step_cycle;
00121 }
00122 
00123 ///Place user code here that should be run after initialisation but before the main loop
00124 void user_code_setup()
00125 {
00126     wait(0.8);
00127     display.clear_display();
00128     display.set_position(0,0);
00129     display.write_string("BEAUTIFUL MEME");
00130     display.set_position(1,0);
00131     display.write_string("   PROJECT");
00132     wait(0.2);
00133     out("------------------------------------------------------\n");
00134     out("Beautiful Meme Project Demo Code                      \n");
00135     out("------------------------------------------------------\n");
00136     locate_beacon();
00137     while(beacon_found == 0) {
00138         wait(0.5);
00139         locate_beacon();
00140     }
00141     start_infrared_timers();
00142     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
00143     set_program(0);
00144     set_leds(0x00,0x00);
00145     set_center_led(3,0.5);
00146     display.clear_display();
00147     display.set_position(0,0);
00148     display.write_string("BEACON FOUND AT");
00149     display.set_position(1,0);
00150     char degrees_string[16];
00151     sprintf(degrees_string,"%d DEGREES",beacon_heading);
00152     display.write_string(degrees_string);
00153 }
00154 
00155 /// Code goes here to handle what should happen when the user switch is pressed
00156 void handle_switch_event(char switch_state)
00157 {
00158     /// 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
00159     /// NB For maximum compatability it is recommended to minimise reliance on center button press
00160     if(choose_program_mode == 0) {
00161         if(switch_state == 8) switch_held = 1;
00162         else if(switch_state == 0 && switch_held == 2) switch_held = 3;
00163         else switch_held = 0;
00164     } else {
00165         // We are in choose program mode
00166         if(switch_state == 8) {
00167             program_selection ++;
00168             if(program_selection > program_count) program_selection = 0;
00169             if(program_selection == program_count) set_program_info("RECHARGE");
00170             else set_program_info(get_program_name(program_selection));
00171         }
00172         if(switch_state == 4) {
00173             if(program_selection == 0) program_selection = program_count; 
00174             else program_selection --;  
00175             if(program_selection == program_count) set_program_info("RECHARGE");
00176             else set_program_info(get_program_name(program_selection));
00177         }
00178         if(switch_state == 1 || switch_state == 2){
00179             if(program_selection == program_count){
00180                   recharging_state = 1;
00181                   set_program(previous_program);
00182                   strcpy(prog_name,"CHARGING PROGRAM");
00183                   set_program_info("HEAD TO BEACON");  
00184                   
00185             }   
00186             else set_program(program_selection);
00187             choose_program_mode = 0; 
00188             switch_held = 0;
00189         }
00190     }
00191     //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);
00192 }
00193 
00194 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
00195 int main()
00196 {
00197     ///init() in psiswarm.cpp sets up the robot
00198     init();
00199     user_code_setup();
00200     user_code_running = 1;
00201     while(1) {
00202         wait(1);
00203     }
00204 }
00205 
00206 char * get_program_name(int index)
00207 {
00208     char * ret_name = new char[17];
00209     switch(index) {
00210         case 0:
00211             strcpy(ret_name,"FACE BEACON");
00212             break;
00213         case 1:
00214             strcpy(ret_name,"HEAD TO BEACON");
00215             break;
00216         case 2:
00217             strcpy(ret_name,"HEAD TO SOUTH");
00218             break;
00219         case 3:
00220             strcpy(ret_name,"RANDOM WALK 1");
00221             break;
00222         case 4:
00223             strcpy(ret_name,"RANDOM WALK 2");
00224             break;
00225         case 5:
00226             strcpy(ret_name,"FIND SPACE");
00227             break;
00228         case 6:
00229             strcpy(ret_name,"CLUSTERING");
00230             break;
00231         case 7:
00232             strcpy(ret_name,"TAG GAME");
00233             break;
00234         case 8:
00235             strcpy(ret_name,"FLOCKING");
00236             break;
00237         case 255:
00238             strcpy(ret_name,"PROGRAM:");
00239             break;
00240     }
00241     return ret_name;
00242 }
00243 
00244 void set_program(int index)
00245 {
00246     main_program_state = index;
00247     program_changed = 1;
00248     program_run_init = 1;
00249     strcpy(prog_info,"");
00250     strcpy(prog_name,get_program_name(index));
00251 }
00252 
00253 void set_program_info(char * info)
00254 {
00255     strcpy(prog_info,info);
00256     program_changed = 1;
00257 }
00258 
00259 void update_display()
00260 {
00261     if(program_changed == 1) {
00262         program_changed = 0;
00263         display.clear_display();
00264 
00265         if(display_debug_inf==1) display_debug_info();
00266         else {
00267             display.set_position(0,0);
00268             display.write_string(prog_name);
00269         }
00270         display.set_position(1,0);
00271         display.write_string(prog_info);
00272     }
00273 }
00274 
00275 void display_debug_info()
00276 {
00277     char disp_line[16] = "- - - - - - - -";
00278     if(beacon_found==1)disp_line[0]='B';
00279     for(int i=1; i<8; i++) {
00280         if(robots_found[i])disp_line[((i)*2)]=48+i;
00281     }
00282     display.set_position(0,0);
00283     display.write_string(disp_line);
00284 }
00285 
00286 /// Verbose output
00287 void out(const char* format, ...)
00288 {
00289     char buffer[256];
00290     if (debug_mode) {
00291         va_list vl;
00292         va_start(vl, format);
00293         vsprintf(buffer,format,vl);
00294         if(user_code_debug == 1) pc.printf("%s", buffer);
00295         va_end(vl);
00296     }
00297 }