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         switch(main_program_state) {
00081             case 0: //Case 0 is the initial program: turn to face beacon
00082                 if(step_cycle == 0) {
00083                     char turn_status = turn_to_bearing(0);
00084                     if(turn_status == 0) {
00085                         success_count ++;
00086                         if(success_count > 1) set_program(default_normal_program);
00087                     } else success_count = 0;
00088                 }
00089                 break;
00090             case 1:
00091                 target_reached = 0;
00092                 head_to_bearing_program(0);
00093                 if(target_reached == 1) set_program(2);
00094                 break;
00095             case 2:
00096                 target_reached = 0;
00097                 head_to_bearing_program(180);
00098                 if(target_reached == 1) set_program(1);
00099                 break;
00100             case 3:
00101                 obs_avoid();
00102                 break;
00103             case 4:
00104                 straight_random_walk_with_interaction_program();
00105                 break;
00106             case 5:
00107                 find_space_program(1);
00108                 break;
00109             case 6:
00110                 clustering_program(0,1);
00111                 break;
00112             case 7:
00113                 tag_game_program();
00114                 break;
00115             case 8:
00116                 mockFlocking();
00117                 break;
00118             case 254:
00119                 init();
00120                 break;
00121             case 255:
00122                 stop_program();
00123                 break;
00124         }
00125     }
00126     step_cycle=1-step_cycle;
00127 }
00128 
00129 ///Place user code here that should be run after initialisation but before the main loop
00130 void user_code_setup()
00131 {
00132     // Note for Alex:
00133     //
00134     // This is the code we have been working on for the beautiful meme project
00135     //
00136     // If you were to comment out (or delete...) all the code here, the robot will just
00137     // sit in an 'idle' loop when you turn it on - but should respond to any command codes
00138     // that are sent.
00139     
00140     wait(0.8);
00141     display.clear_display();
00142     display.set_position(0,0);
00143     display.write_string("SCI MUSEUM");
00144     display.set_position(1,0);
00145     display.write_string("   PROJECT");
00146     wait(0.2);
00147     out("------------------------------------------------------\n");
00148     out("Dank Meme Project Demo Code                      \n");
00149     out("------------------------------------------------------\n");
00150     locate_beacon();
00151     while(beacon_found == 0) {
00152         wait(0.5);
00153         locate_beacon();
00154     } 
00155     //toggle_program_ticker.attach(toggle_program,20);
00156     start_infrared_timers();
00157     main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10);
00158     set_program(4);//for debug ...was 0 ----------
00159     set_leds(0x00,0x00);
00160     set_center_led(3,0.5);
00161     display.clear_display();
00162     display.set_position(0,0);
00163     display.write_string("BEACON FOUND AT");
00164     display.set_position(1,0);
00165     char degrees_string[16];
00166     sprintf(degrees_string,"%d DEGREES",beacon_heading);
00167     display.write_string(degrees_string);
00168 }
00169 
00170 void toggle_program(){
00171     toggle_program_stage ++;
00172     if(toggle_program_stage == 3) toggle_program_stage = 0;
00173     switch(toggle_program_stage){
00174         case 0: set_program(5); break;
00175         case 1: set_program(6); break;
00176         case 2: set_program(7);break;
00177         case 3: set_program(7); break;
00178     }   
00179 }
00180 
00181 /// Code goes here to handle what should happen when the user switch is pressed
00182 void handle_switch_event(char switch_state)
00183 {
00184     /// 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
00185     /// NB For maximum compatability it is recommended to minimise reliance on center button press
00186     if(choose_program_mode == 0) {
00187         if(switch_state == 8) switch_held = 1;
00188         else if(switch_state == 0 && switch_held == 2) switch_held = 3;
00189         else switch_held = 0;
00190     } else {
00191         // We are in choose program mode
00192         if(switch_state == 8) {
00193             program_selection ++;
00194             if(program_selection > program_count) program_selection = 0;
00195             if(program_selection == program_count) set_program_info("RECHARGE");
00196             else set_program_info(get_program_name(program_selection));
00197         }
00198         if(switch_state == 4) {
00199             if(program_selection == 0) program_selection = program_count; 
00200             else program_selection --;  
00201             if(program_selection == program_count) set_program_info("RECHARGE");
00202             else set_program_info(get_program_name(program_selection));
00203         }
00204         if(switch_state == 1 || switch_state == 2){
00205             if(program_selection == program_count){
00206                   recharging_state = 1;
00207                   set_program(previous_program);
00208                   strcpy(prog_name,"CHARGING PROGRAM");
00209                   set_program_info("HEAD TO BEACON");  
00210                   
00211             }   
00212             else set_program(program_selection);
00213             choose_program_mode = 0; 
00214             switch_held = 0;
00215         }
00216     }
00217     //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);
00218 }
00219 void handle_user_serial_message(char * message, char length, char interface)
00220 {
00221     // This is where user code for handling a (non-system) serial message should go
00222     //
00223     // message = pointer to message char array
00224     // length = length of message
00225     // interface = 0 for PC serial connection, 1 for Bluetooth
00226     
00227     char buffer[255];
00228     sprintf(buffer,message,length);
00229     for(int i=0; i<length; i++) {
00230         buffer[i]=message[i];
00231     }
00232     buffer[length]=0;
00233     if(interface) debug("Received BT message:%s [%d chars]\n",buffer,length);
00234     else debug("Received USB message:%s [%d chars]\n",buffer,length);
00235     
00236     display.clear_display();
00237     display.set_position(0,0);
00238     //display.write_string(buffer);
00239     
00240     //"A" = 0x41
00241     if(buffer[0] == 'A'){
00242 
00243         main_program_state = 255;
00244         display.write_string("MALLARD QUACK QUACK");
00245     }
00246     //"B" = 0x42
00247     else if(buffer[0] == 'B'){
00248         //set_program(1); 
00249         main_program_state = 1;
00250         display.write_string("LAPS (BEACON)");
00251         }
00252     //"C" = 0x43
00253     else if(buffer[0] == 'C'){
00254         main_program_state = 2;
00255         display.write_string("LAPS (SOUTH)");
00256         }
00257     //"D" = 0x44
00258     else if(buffer[0] == 'D'){
00259         main_program_state = 8;
00260         display.write_string("FLOCKING");
00261         }
00262     else if(buffer[0] == 'E'){
00263         main_program_state = 3;
00264         display.write_string("RANDOM WALK");
00265         }
00266     else if(buffer[0] == 'F'){
00267         main_program_state = 6;
00268         display.write_string("AGGREGATION");
00269         }
00270     else if(buffer[0] == 'G'){
00271         main_program_state = 5;
00272         display.write_string("FIND SPACE");
00273         }
00274     else if(buffer[0] == 'H'){
00275         main_program_state = 6;
00276         display.write_string("CLUSTERING");
00277         }
00278     else if(buffer[0] == 'I'){
00279         main_program_state = 7;
00280         display.write_string("TAG");
00281         }
00282     else
00283     {
00284 
00285         }
00286 }
00287 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
00288 int main()
00289 {
00290     ///init() in psiswarm.cpp sets up the robot
00291     init();
00292     user_code_setup();
00293     user_code_running = 1;
00294     while(1) {
00295 
00296         wait(1);
00297     }
00298 }
00299 
00300 char * get_program_name(int index)
00301 {
00302     char * ret_name = new char[17];
00303     switch(index) {
00304         case 0:
00305             strcpy(ret_name,"FACE BEACON");
00306             break;
00307         case 1:
00308             strcpy(ret_name,"HEAD TO BEACON");
00309             break;
00310         case 2:
00311             strcpy(ret_name,"HEAD TO SOUTH");
00312             break;
00313         case 3:
00314             strcpy(ret_name,"RANDOM WALK 1");
00315             break;
00316         case 4:
00317             strcpy(ret_name,"WAITING COMMAND");///gus, was random walk2
00318             break;
00319         case 5:
00320             strcpy(ret_name,"FIND SPACE");
00321             break;
00322         case 6:
00323             strcpy(ret_name,"CLUSTERING");
00324             break;
00325         case 7:
00326             strcpy(ret_name,"TAG GAME");
00327             break;
00328         case 255:
00329             strcpy(ret_name,"PROGRAM:");
00330             break;
00331     }
00332     return ret_name;
00333 }
00334 
00335 void set_program(int index)
00336 {
00337     main_program_state = index;
00338     program_changed = 1;
00339     program_run_init = 1;
00340     strcpy(prog_info,"");
00341     strcpy(prog_name,get_program_name(index));
00342 }
00343 
00344 void set_program_info(char * info)
00345 {
00346     strcpy(prog_info,info);
00347     program_changed = 1;
00348 }
00349 
00350 void update_display()
00351 {
00352     if(program_changed == 1) {
00353         program_changed = 0;
00354         display.clear_display();
00355 
00356         if(display_debug_inf==1) display_debug_info();
00357         else {
00358             display.set_position(0,0);
00359             display.write_string(prog_name);
00360         }
00361         display.set_position(1,0);
00362         display.write_string(prog_info);
00363     }
00364 }
00365 
00366 void display_debug_info()
00367 {
00368     char disp_line[16] = "- - - - - - - -";
00369     if(beacon_found==1)disp_line[0]='B';
00370     for(int i=1; i<8; i++) {
00371         if(robots_found[i])disp_line[((i)*2)]=48+i;
00372     }
00373     display.set_position(0,0);
00374     display.write_string(disp_line);
00375 }
00376 
00377 /// Verbose output
00378 void out(const char* format, ...)
00379 {
00380     char buffer[256];
00381     if (debug_mode) {
00382         va_list vl;
00383         va_start(vl, format);
00384         vsprintf(buffer,format,vl);
00385         if(user_code_debug == 1) pc.printf("%s", buffer);
00386         va_end(vl);
00387     }
00388 }