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 
00037 char toggle_program_stage = 0;
00038 char default_normal_program = 5;      // The program to run on turn on (after 'face beacon' program)
00039 char use_recharging_program = 0;         // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold
00040 char user_code_debug = 1;                // Set to 1 to show terminal messages from "out" function [specific to this code]
00041 char display_debug_inf = 0;              // Set to 1 to show debug info about beacon\robots on display [instead of running program info]
00042 char main_program_state;                 // Index of the currently running program
00043 char previous_program;                   // Used to hold previous running program when it is paused for switch press etc
00044 char program_changed = 0;                // Flag to update display when program is changed
00045 char program_run_init = 0;               // Flag to tell program to run its initialisation on first loop, if neccessary
00046 char success_count = 0;                  // Flag to indicate the success of a program
00047 char step_cycle = 0;                     // Alternates between 0 and 1 in successive time-steps
00048 char target_reached = 0;                 // Flag to indicate if a program target has been reached
00049 char prog_name [17];                     // Stores the name of the running program [line 0 on the display]
00050 char prog_info [17];                     // Stores information about the current state of the program [line 1 on the display]
00051 char disable_ir_emitters = 0;            // Used to disable IR emission during charging etc [use with caution!]
00052 char recharging_state = 0;               // Stores the state of the recharging program (0 is not currently running)
00053 char switch_held = 0;                    // Used for detected when the cursor switch is held to override program choice
00054 char choose_program_mode = 0;
00055 char program_count = 8;
00056 char program_selection;
00057 Ticker toggle_program_ticker;
00058 
00059 float battery_low_threshold = 3.60;      // Threshold at which to interrupt program and start recharging routine: suggest 3.55
00060 float battery_high_threshold = 3.95;     // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0
00061 
00062 Ticker main_loop_ticker;
00063 
00064 
00065 ///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.
00066 void main_loop()
00067 {
00068     if(switch_held == 1)switch_held=2;
00069     if(switch_held == 3 && choose_program_mode == 0) {
00070         //The switch has been held right and then released:  stop the current program
00071         previous_program = main_program_state;
00072         program_selection = previous_program;
00073         choose_program_mode = 1;
00074         set_program(255);
00075         set_program_info(get_program_name(program_selection));
00076     }
00077     if(use_recharging_program == 1)recharging_program();
00078     update_display();
00079     if(recharging_state == 0) {
00080         // remove after testing
00081         main_program_state = 8;
00082         switch(main_program_state) {
00083             case 0: //Case 0 is the initial program: turn to face beacon
00084                 if(step_cycle == 0) {
00085                     char turn_status = turn_to_bearing(0);
00086                     if(turn_status == 0) {
00087                         success_count ++;
00088                         if(success_count > 1) set_program(default_normal_program);
00089                     } else success_count = 0;
00090                 }
00091                 break;
00092             case 1:
00093                 target_reached = 0;
00094                 head_to_bearing_program(0);
00095                 if(target_reached == 1) set_program(2);
00096                 break;
00097             case 2:
00098                 target_reached = 0;
00099                 head_to_bearing_program(180);
00100                 if(target_reached == 1) set_program(1);
00101                 break;
00102             case 3:
00103                 curved_random_walk_with_interaction_program();
00104                 break;
00105             case 4:
00106                 straight_random_walk_with_interaction_program();
00107                 break;
00108             case 5:
00109                 find_space_program(1);
00110                 break;
00111             case 6:
00112                 clustering_program(0,1);
00113                 break;
00114             case 7:
00115                 tag_game_program();
00116                 break;
00117             case 8:
00118                 mockFlocking();
00119                 break;
00120             case 254:
00121                 init();
00122                 break;
00123             case 255:
00124                 stop_program();
00125                 break;
00126         }
00127     }
00128     step_cycle=1-step_cycle;
00129 }
00130 
00131 ///Place user code here that should be run after initialisation but before the main loop
00132 void user_code_setup()
00133 {
00134     // Note for Alex:
00135     //
00136     // This is the code we have been working on for the beautiful meme project
00137     //
00138     // If you were to comment out (or delete...) all the code here, the robot will just
00139     // sit in an 'idle' loop when you turn it on - but should respond to any command codes
00140     // that are sent.
00141     
00142     wait(0.8);
00143     display.clear_display();
00144     display.set_position(0,0);
00145     display.write_string("BEAUTIFUL MEME");
00146     display.set_position(1,0);
00147     display.write_string("   PROJECT");
00148     wait(0.2);
00149     out("------------------------------------------------------\n");
00150     out("Beautiful Meme Project Demo Code                      \n");
00151     out("------------------------------------------------------\n");
00152     locate_beacon();
00153     while(beacon_found == 0) {
00154         wait(0.5);
00155         locate_beacon();
00156     } 
00157     //toggle_program_ticker.attach(toggle_program,20);
00158     start_infrared_timers();
00159     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
00160     set_program(4);//for debug ...was 0 ----------
00161     set_leds(0x00,0x00);
00162     set_center_led(3,0.5);
00163     display.clear_display();
00164     display.set_position(0,0);
00165     display.write_string("BEACON FOUND AT");
00166     display.set_position(1,0);
00167     char degrees_string[16];
00168     sprintf(degrees_string,"%d DEGREES",beacon_heading);
00169     display.write_string(degrees_string);
00170 }
00171 
00172 void toggle_program(){
00173     toggle_program_stage ++;
00174     if(toggle_program_stage == 3) toggle_program_stage = 0;
00175     switch(toggle_program_stage){
00176         case 0: set_program(5); break;
00177         case 1: set_program(6); break;
00178         case 2: set_program(7);break;
00179         case 3: set_program(7); break;
00180     }   
00181 }
00182 
00183 /// Code goes here to handle what should happen when the user switch is pressed
00184 void handle_switch_event(char switch_state)
00185 {
00186     /// 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
00187     /// NB For maximum compatability it is recommended to minimise reliance on center button press
00188     if(choose_program_mode == 0) {
00189         if(switch_state == 8) switch_held = 1;
00190         else if(switch_state == 0 && switch_held == 2) switch_held = 3;
00191         else switch_held = 0;
00192     } else {
00193         // We are in choose program mode
00194         if(switch_state == 8) {
00195             program_selection ++;
00196             if(program_selection > program_count) program_selection = 0;
00197             if(program_selection == program_count) set_program_info("RECHARGE");
00198             else set_program_info(get_program_name(program_selection));
00199         }
00200         if(switch_state == 4) {
00201             if(program_selection == 0) program_selection = program_count; 
00202             else program_selection --;  
00203             if(program_selection == program_count) set_program_info("RECHARGE");
00204             else set_program_info(get_program_name(program_selection));
00205         }
00206         if(switch_state == 1 || switch_state == 2){
00207             if(program_selection == program_count){
00208                   recharging_state = 1;
00209                   set_program(previous_program);
00210                   strcpy(prog_name,"CHARGING PROGRAM");
00211                   set_program_info("HEAD TO BEACON");  
00212                   
00213             }   
00214             else set_program(program_selection);
00215             choose_program_mode = 0; 
00216             switch_held = 0;
00217         }
00218     }
00219     //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);
00220 }
00221 void handle_user_serial_message(char * message, char length, char interface)
00222 {
00223     // This is where user code for handling a (non-system) serial message should go
00224     //
00225     // message = pointer to message char array
00226     // length = length of message
00227     // interface = 0 for PC serial connection, 1 for Bluetooth
00228     
00229     char buffer[255];
00230     sprintf(buffer,message,length);
00231     for(int i=0; i<length; i++) {
00232         buffer[i]=message[i];
00233     }
00234     buffer[length]=0;
00235     if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
00236     else debug("Received USB message:%s [%d chars]\n",buffer,length);
00237     
00238     display.clear_display();
00239     display.set_position(0,0);
00240     display.write_string(buffer);
00241     
00242     //"A" = 0x41
00243     if(buffer[0] == 0x41){
00244 /*        display.clear_display();
00245         display.set_position(0,0);
00246         display.write_string("Gotcha!");*/
00247         
00248         set_program(1); // Head to beacon
00249     }
00250     //"B" = 0x42
00251     else if(buffer[0] == 0x42){
00252         set_program(2); // Head inverse to beacon
00253         }
00254     //"C" = 0x43
00255     else if(buffer[0] == 0x43){
00256         set_program(5); // Find space
00257         }
00258     //"D" = 0x44
00259     else if(buffer[0] == 0x44){
00260         set_program(6); // Clustering
00261         }
00262     else
00263     {
00264 
00265         }
00266 }
00267 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
00268 int main()
00269 {
00270     ///init() in psiswarm.cpp sets up the robot
00271     init();
00272     user_code_setup();
00273     //char prog_index;
00274     //char * p_index = &prog_index;
00275     user_code_running = 1;
00276     while(1) {
00277 /*        prog_index = pc.getc();
00278         switch(prog_index){
00279         case 0x61:
00280             display.clear_display();
00281             display.set_position(0,0);
00282             display.write_string("Facing Beacon");
00283             break;
00284         default:
00285             display.clear_display();
00286             display.set_position(0,0);
00287             display.write_string(p_index);
00288             pc.putc(prog_index);
00289             break;
00290         }*/
00291         wait(1);
00292     }
00293 }
00294 
00295 char * get_program_name(int index)
00296 {
00297     char * ret_name = new char[17];
00298     switch(index) {
00299         case 0:
00300             strcpy(ret_name,"FACE BEACON");
00301             break;
00302         case 1:
00303             strcpy(ret_name,"HEAD TO BEACON");
00304             break;
00305         case 2:
00306             strcpy(ret_name,"HEAD TO SOUTH");
00307             break;
00308         case 3:
00309             strcpy(ret_name,"RANDOM WALK 1");
00310             break;
00311         case 4:
00312             strcpy(ret_name,"WAITING COMMAND");///gus, was random walk2
00313             break;
00314         case 5:
00315             strcpy(ret_name,"FIND SPACE");
00316             break;
00317         case 6:
00318             strcpy(ret_name,"CLUSTERING");
00319             break;
00320         case 7:
00321             strcpy(ret_name,"TAG GAME");
00322             break;
00323         case 255:
00324             strcpy(ret_name,"PROGRAM:");
00325             break;
00326     }
00327     return ret_name;
00328 }
00329 
00330 void set_program(int index)
00331 {
00332     main_program_state = index;
00333     program_changed = 1;
00334     program_run_init = 1;
00335     strcpy(prog_info,"");
00336     strcpy(prog_name,get_program_name(index));
00337 }
00338 
00339 void set_program_info(char * info)
00340 {
00341     strcpy(prog_info,info);
00342     program_changed = 1;
00343 }
00344 
00345 void update_display()
00346 {
00347     if(program_changed == 1) {
00348         program_changed = 0;
00349         display.clear_display();
00350 
00351         if(display_debug_inf==1) display_debug_info();
00352         else {
00353             display.set_position(0,0);
00354             display.write_string(prog_name);
00355         }
00356         display.set_position(1,0);
00357         display.write_string(prog_info);
00358     }
00359 }
00360 
00361 void display_debug_info()
00362 {
00363     char disp_line[16] = "- - - - - - - -";
00364     if(beacon_found==1)disp_line[0]='B';
00365     for(int i=1; i<8; i++) {
00366         if(robots_found[i])disp_line[((i)*2)]=48+i;
00367     }
00368     display.set_position(0,0);
00369     display.write_string(disp_line);
00370 }
00371 
00372 /// Verbose output
00373 void out(const char* format, ...)
00374 {
00375     char buffer[256];
00376     if (debug_mode) {
00377         va_list vl;
00378         va_start(vl, format);
00379         vsprintf(buffer,format,vl);
00380         if(user_code_debug == 1) pc.printf("%s", buffer);
00381         va_end(vl);
00382     }
00383 }