main.cpp

00001 /**
00002 Author: Giles Barton-Owen
00003 Name: mbed ranger
00004 Description:
00005 
00006 This program keeps a m3pi inside a ring, and has 6 (4 Active) different game modes.
00007 It relies on a Sharp Digital Distance sensor (GP2Y0D810) attached to pin 30 and 11 
00008 (the two headers on the right and left of the board). The system refreshes at 10Hz
00009 when it reads the line sensors on the 3pi and the distance states. The states are
00010 then handled in a big FSM to reach the conclusions.
00011 
00012 To keep the robot saving itself for longer a timeout was programmed, such that the
00013 robot belives itself on a line (as was reported when the timeout was set) until 
00014 0.3 seconds has elapsed. This could be adjusted.
00015 
00016 The fore-ground loop just switches the LEDs on the board as instructed. The current
00017 state of the robot is reported on line 2 of the LCD screen on the 3pi, displaying
00018 first mode and the last character being line detection state (none, front, back).
00019 
00020 
00021 **/
00022 
00023 #include "mbed.h"
00024 #include "SharpDigiDist100.h"
00025 #include "m3pi.h"
00026 
00027 DigitalOut Left[2] = {LED1,LED2};           //Some indicator LEDs for the range finders
00028 DigitalOut Right[2] = {LED4,LED3};
00029 
00030 SharpDigiDist100 right(p30);                // The range finder class initialisations
00031 SharpDigiDist100 left(p11);
00032 
00033 m3pi m3pi;                                  // Initialise the m3pi
00034 
00035 InterruptIn button(p21);                    // SW1 on the shield, for stratergy switching
00036 
00037 Ticker guidance;                            // The main guidance caller
00038 
00039 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
00040 
00041 Timer debounce;                             // To debounce the switch
00042 
00043 int previousLine;                           // A set of variables to sort out the line detection, previousLine is pretty much not used
00044 int isLine;
00045 int lineCatch;
00046 
00047 Timeout liner;                              // A timeout to stop it ignoring the line
00048 
00049 void CycleMode();                           // Function defs
00050 void guideCall();
00051 void clearLiner();
00052 
00053 enum Stratergies {                          // The enum for stratergies
00054     Off,
00055     Dormant,
00056     RunMid,
00057     RunNear,
00058     AttackMid,
00059     AttackFar,
00060     Hunt
00061 };
00062 
00063 void updateScreen(int line, enum Stratergies stratergy);
00064 
00065 enum Stratergies stratMode = Off;       // The current stratergy
00066 
00067 int main() {
00068     guidance.attach(&guideCall,0.1);        // Sets up the control loop
00069     
00070     button.mode(PullUp);                    // Sets the internal pull up on SW1
00071     button.fall(&CycleMode);                // Attaches an interupt for when it is pressed
00072     
00073     m3pi.locate(0,0);                       // Write the name to the screen
00074     m3pi.printf("m3PiRngr");                
00075     
00076     m3pi.get_white_levels();                // Saves the current levels of the sensors to know what is white
00077     
00078     debounce.start();                       // Starts the debounce timer so that the switch thinks it isnt a bounce first time
00079     
00080     //pc.printf("\n\n\rMbedRanger\n\r");      // Prints a hello to the computer
00081     
00082     while (1) {
00083         //pc.printf("Loop\n\r");
00084         switch (right.getDistance()) {      // Sets up the distance indicator LEDs for the right side
00085             case SharpDigiDist100::Far :
00086                 Right[0] = true;
00087                 Right[1] = false;
00088                 break;
00089             case SharpDigiDist100::Near :
00090                 Right[1] = true;
00091                 Right[0] = false;
00092                 break;
00093             case SharpDigiDist100::Mid :
00094                 Right[0] = true;
00095                 Right[1] = true;
00096                 break;
00097             default:
00098                 break;
00099         }
00100         switch (left.getDistance()) {       // Sets up the distance indicator LEDs for the left side
00101             case SharpDigiDist100::Far :
00102                 Left[0] = true;
00103                 Left[1] = false;
00104                 break;
00105             case SharpDigiDist100::Near :
00106                 Left[1] = true;
00107                 Left[0] = false;
00108                 break;
00109             case SharpDigiDist100::Mid :
00110                 Left[0] = true;
00111                 Left[1] = true;
00112                 break;
00113             default:
00114                 break;
00115         }
00116         /*int calibrated[5];
00117         m3pi.get_raw_sensors(calibrated);
00118          pc.printf("Sensors read:");
00119         for (int i = 0; i<5;i++) {
00120             pc.printf(" %i ",calibrated[i]);
00121         }
00122         if (m3pi.is_line() > 0) {
00123             pc.printf("LINE");
00124             m3pi.locate(0,1);
00125             m3pi.printf("LINE");
00126         }
00127         pc.printf("\n\r");
00128         wait(0.1);*/
00129     }
00130 }
00131 
00132 
00133 
00134 void CycleMode() {                      // Cycles through the modes, probably could have written this with a simple ++ statement...
00135     debounce.stop();                    // Stops and reads the timer since the last press of the button
00136     if (debounce.read_ms() > 100) {     // If it was more than 100ms ago it treats it as a button press, otherwise it just ignores it
00137         switch (stratMode) {
00138             case Off:
00139                 stratMode = Dormant;
00140                 m3pi.locate(0,1);
00141                 m3pi.printf("\nDormant ");
00142                 break;
00143             case Dormant:
00144                 stratMode = RunMid;
00145                 m3pi.locate(0,1);
00146                 m3pi.printf("\nRunMid ");
00147                 break;
00148             case RunMid:
00149                 stratMode = RunNear;
00150                 m3pi.locate(0,1);
00151                 m3pi.printf("\nRunNear ");
00152                 break;
00153             case RunNear:
00154                 stratMode = AttackMid;
00155                 m3pi.locate(0,1);
00156                 m3pi.printf("\nTakNear ");
00157                 break;
00158             case AttackMid:
00159                 stratMode = AttackFar;
00160                 m3pi.locate(0,1);
00161                 m3pi.printf("\nTakFar ");
00162                 break;
00163             case AttackFar:
00164                 stratMode = Hunt;
00165                 m3pi.locate(0,1);
00166                 m3pi.printf("\nHUNTING! ");
00167                 break;
00168             case Hunt:
00169                 stratMode = Off;
00170                 m3pi.printf("\nOFF ");
00171                 break;
00172         }
00173     }
00174     debounce.reset();
00175     debounce.start();
00176 }
00177 
00178 void guideCall() {
00179 
00180     isLine = m3pi.is_line();            // Gets whether the m3pi is on a line, and if so front/back
00181     
00182     if (lineCatch == 0) {               // Has it been off a line for long enough?
00183         isLine = isLine;                // Yes - then go ahead
00184     } else {
00185         isLine = lineCatch;             // No - pretend to still be on that line
00186         
00187     }
00188     float position;                     
00189     
00190     switch (isLine) {
00191         case 0:                         // No line, not even recently so go ahead with the stratergies
00192             
00193             updateScreen(isLine, stratMode);
00194             bool atRight = false;
00195             bool atLeft = false;
00196             
00197             switch (stratMode) {
00198                 case Off:               // No motors
00199                 case Dormant:           // Will take action against lines though
00200                     m3pi.stop();
00201                     break;
00202 
00203                 case RunNear:           // Runs if something is near
00204                     if (right.getDistance() == SharpDigiDist100::Near) {
00205                         atRight = true;
00206                     } else atRight = false;
00207                     if (left.getDistance() == SharpDigiDist100::Near) {
00208                         atLeft = true;
00209                     } else atLeft = false;
00210 
00211                 case RunMid:            // Runs if something is in the middle distance
00212 
00213                     if (right.getDistance() == SharpDigiDist100::Mid) {
00214                         atRight = true;
00215                     }
00216                     if (left.getDistance() == SharpDigiDist100::Mid) {
00217                         atLeft = true;
00218                     }
00219 
00220 
00221                     if (atRight && atLeft) {
00222                         m3pi.backward(0.5);
00223                     } else {
00224                         if (atRight == true) {
00225                             m3pi.left_motor(-0.3);
00226                             m3pi.right_motor(-0.5);
00227                         } else {
00228                             if (atLeft == true) {
00229                                 m3pi.left_motor(-0.5);
00230                                 m3pi.right_motor(-0.3);
00231                             } else {
00232                                 m3pi.stop();
00233                             }
00234                         }
00235                     }
00236                     break;
00237 
00238                 case AttackMid:         // Attacks something in the middle distance
00239                     if (right.getDistance() == SharpDigiDist100::Mid) {
00240                         atRight = true;
00241                     }
00242                     if (left.getDistance() == SharpDigiDist100::Mid) {
00243                         atLeft = true;
00244                     }
00245 
00246 
00247                     if (atRight && atLeft) {
00248                         m3pi.forward(0.6);
00249                     } else {
00250                         if (atRight == true) {
00251                             m3pi.left_motor(0.5);
00252                             m3pi.right_motor(0.7);
00253                         } else {
00254                             if (atLeft == true) {
00255                                 m3pi.left_motor(0.7);
00256                                 m3pi.right_motor(0.5);
00257                             } else {
00258                                 m3pi.stop();
00259                             }
00260                         }
00261                     }
00262 
00263                     if (right.getDistance() == SharpDigiDist100::Near) {
00264                         atRight = true;
00265                     } else atRight = false;
00266                     if (left.getDistance() == SharpDigiDist100::Near) {
00267                         atLeft = true;
00268                     } else atLeft = false;
00269 
00270 
00271                     if (atRight && atLeft) {
00272                         m3pi.forward(0.5);
00273                     } else {
00274                         if (atRight == true) {
00275                             m3pi.left_motor(0.1);
00276                             m3pi.right_motor(0.2);
00277                         } else {
00278                             if (atLeft == true) {
00279                                 m3pi.left_motor(0.2);
00280                                 m3pi.right_motor(0.1);
00281                             }
00282                         }
00283                     }
00284 
00285                     break;
00286 
00287                 case AttackFar:
00288 
00289                     break;
00290                 case Hunt:                  // Runs forward until something is really close
00291 
00292                     if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) {
00293                         atRight = true;
00294                     } else atRight = false;
00295                     if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) {
00296                         atLeft = true;
00297                     } else atLeft = false;
00298 
00299                     if (atRight && atLeft) {
00300                         m3pi.stop();
00301                     } else {
00302                         if (atRight == true) {
00303                             m3pi.left_motor(0.1);
00304                             m3pi.right_motor(0.2);
00305                         } else {
00306                             if (atLeft == true) {
00307                                 m3pi.left_motor(0.2);
00308                                 m3pi.right_motor(0.1);
00309                             } else {
00310                                 m3pi.forward(0.3);
00311                             }
00312                         }
00313                     }
00314                     break;
00315                 default:
00316                     break;
00317             }
00318             break;
00319 
00320         case 1:                 // Line in front, reverse
00321             if(stratMode != Off)
00322             {
00323             if (lineCatch == 0) {
00324                 lineCatch = 1;
00325 
00326                 liner.attach(&clearLiner, 0.3);
00327             }
00328 
00329             position = m3pi.line_position();
00330             if (position < 0) {
00331                 m3pi.left_motor(-1);
00332                 m3pi.right_motor(-0.8);
00333             } else if (position == 0) {
00334                 m3pi.backward(1);
00335             } else if (position > 0) {
00336                 m3pi.left_motor(-0.8);
00337                 m3pi.right_motor(-1);
00338             }
00339 
00340             //m3pi.locate(0,1);
00341             //m3pi.printf("LINE_FWD");
00342             }
00343             else
00344             {
00345                 m3pi.stop();
00346             }
00347             updateScreen(isLine, stratMode);
00348             break;
00349 
00350         case -1:            // Line behind, forward
00351             if(stratMode != Off)
00352             {
00353             if (lineCatch == 0) {
00354                 lineCatch = -1;
00355                 liner.attach(&clearLiner, 0.3);
00356             }
00357 
00358 
00359             position = m3pi.line_position();
00360             if (position < 0) {
00361                 m3pi.left_motor(1);
00362                 m3pi.right_motor(0.8);
00363             } else if (position == 0) {
00364                 m3pi.forward(1);
00365             } else if (position > 0) {
00366                 m3pi.left_motor(0.8);
00367                 m3pi.right_motor(1);
00368             }
00369             }
00370             else
00371             {
00372                 m3pi.stop();
00373             }
00374             //m3pi.locate(0,1);
00375             //m3pi.printf("LINE_BKD");
00376             updateScreen(isLine, stratMode);
00377             break;
00378     }
00379 
00380     //previousLine = isLine;
00381 }
00382 
00383 void clearLiner() {             // Gets called a bit after a line is detected
00384     lineCatch = 0;
00385     //pc.printf("Cleared liner\n\r");
00386 }
00387 
00388 void updateScreen(int line, enum Stratergies stratergy) {       // Update the bottom line with the running info
00389     m3pi.locate(0,1);
00390     
00391     char lineState;
00392     
00393     switch (line) {
00394         case 0:
00395             lineState = 'N';
00396             break;
00397         case 1:
00398             lineState = 'F';
00399             break;
00400         case -1:
00401             lineState = 'B';
00402             break;
00403         default:
00404             break;
00405     }
00406 
00407     char strat[6];
00408     switch (stratergy) {
00409         case Off:
00410             sprintf(strat,"OFF   ");
00411             break;
00412         case Dormant:
00413             sprintf(strat,"DORMNT");
00414             break;
00415         case RunMid:
00416             sprintf(strat,"RUNMID");
00417 
00418             break;
00419         case RunNear:
00420             sprintf(strat,"RNNEAR");
00421 
00422             break;
00423         case AttackMid:
00424             sprintf(strat,"TAKMID");
00425 
00426             break;
00427         case AttackFar:
00428             sprintf(strat,"TAKFAR");
00429 
00430             break;
00431         case Hunt:
00432             sprintf(strat,"HUNTER");
00433 
00434             break;
00435     }
00436 
00437     m3pi.printf("%s %c",strat,lineState);
00438 }