main.cpp

00001 /**
00002 Name: mpi hunter
00003 Description:
00004 
00005 Based on work done by Giles Barton-Owen
00006 Cut down for use at Linton Village College
00007 
00008 This program keeps a m3pi inside a ring and looks for the other robot (and runs away from it)
00009 
00010 **/
00011 
00012 #include "mbed.h"
00013 #include "SharpDigiDist100.h"
00014 #include "m3pi.h"
00015 
00016 DigitalOut Left[2] = {LED1,LED2};           //Some indicator LEDs for the range finders
00017 DigitalOut Right[2] = {LED4,LED3};
00018 
00019 SharpDigiDist100 right(p30);                // The range finder class initialisations
00020 SharpDigiDist100 left(p11);
00021 
00022 m3pi m3pi;                                  // Initialise the m3pi
00023 
00024 InterruptIn button(p21);                    // SW1 on the shield, for strategy switching
00025 
00026 Ticker guidance;                            // The main guidance caller
00027 
00028 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
00029 
00030 int previousLine;                           // A set of variables to sort out the line detection, previousLine is pretty much not used
00031 int isLine;
00032 int lineCatch;
00033 
00034 Timeout liner;                              // A timeout to stop it ignoring the line
00035 
00036 void CycleMode();                           // Function defs
00037 void guideCall();
00038 void clearLiner();
00039 
00040 
00041 int main() {
00042     guidance.attach(&guideCall,0.1);        // Sets up the control loop
00043     
00044     m3pi.locate(0,0);                       // Write the name to the screen
00045     m3pi.printf("m3PiRngr");                
00046     
00047     m3pi.get_white_levels();                // Saves the current levels of the sensors to know what is white
00048     
00049     while (1) {
00050        
00051         switch (right.getDistance()) {      // Sets up the distance indicator LEDs for the right side
00052             case SharpDigiDist100::Far :
00053                 Right[0] = true;
00054                 Right[1] = false;
00055                 break;
00056             case SharpDigiDist100::Near :
00057                 Right[1] = true;
00058                 Right[0] = false;
00059                 break;
00060             case SharpDigiDist100::Mid :
00061                 Right[0] = true;
00062                 Right[1] = true;
00063                 break;
00064             default:
00065                 break;
00066         }
00067         switch (left.getDistance()) {       // Sets up the distance indicator LEDs for the left side
00068             case SharpDigiDist100::Far :
00069                 Left[0] = true;
00070                 Left[1] = false;
00071                 break;
00072             case SharpDigiDist100::Near :
00073                 Left[1] = true;
00074                 Left[0] = false;
00075                 break;
00076             case SharpDigiDist100::Mid :
00077                 Left[0] = true;
00078                 Left[1] = true;
00079                 break;
00080             default:
00081                 break;
00082         }
00083 }
00084 }
00085 void guideCall() {
00086 
00087     isLine = m3pi.is_line();            // Gets whether the m3pi is on a line, and if so front/back
00088     
00089     if (lineCatch == 0) {               // Has it been off a line for long enough?
00090         isLine = isLine;                // Yes - then go ahead
00091     } else {
00092         isLine = lineCatch;             // No - pretend to still be on that line
00093         
00094     }
00095     float position;                     
00096     
00097     switch (isLine) {
00098         case 0:                         // No line, not even recently so go ahead with the strategies
00099             {
00100             bool atRight = false;
00101             bool atLeft = false;
00102              
00103                     if (right.getDistance() == SharpDigiDist100::Near) {
00104                         atRight = true;
00105                     } else atRight = false;
00106                     if (left.getDistance() == SharpDigiDist100::Near) {
00107                         atLeft = true;
00108                     } else atLeft = false;
00109          
00110                    if (atRight && atLeft) {
00111                         m3pi.backward(0.5);
00112                     } else {
00113                         if (atRight == true) {
00114                             m3pi.left_motor(-0.3);
00115                             m3pi.right_motor(-0.5);
00116                         } else {
00117                             if (atLeft == true) {
00118                                 m3pi.left_motor(-0.5);
00119                                 m3pi.right_motor(-0.3);
00120                             } else {
00121                                 m3pi.stop();
00122                             }
00123                         }
00124                     }
00125                     }   
00126              break;       
00127         case 1:                 // Line in front, reverse
00128             if (lineCatch == 0) {
00129                 lineCatch = 1;
00130 
00131                 liner.attach(&clearLiner, 0.3);
00132             }
00133 
00134             position = m3pi.line_position();
00135             if (position < 0) {
00136                 m3pi.left_motor(-1);
00137                 m3pi.right_motor(-0.8);
00138             } else if (position == 0) {
00139                 m3pi.backward(1);
00140             } else if (position > 0) {
00141                 m3pi.left_motor(-0.8);
00142                 m3pi.right_motor(-1);
00143             }
00144 
00145             //m3pi.locate(0,1);
00146             //m3pi.printf("LINE_FWD");
00147           
00148 
00149             break;
00150 
00151         case -1:            // Line behind, forward
00152 
00153             if (lineCatch == 0) {
00154                 lineCatch = -1;
00155                 liner.attach(&clearLiner, 0.3);
00156             }
00157 
00158 
00159             position = m3pi.line_position();
00160             if (position < 0) {
00161                 m3pi.left_motor(1);
00162                 m3pi.right_motor(0.8);
00163             } else if (position == 0) {
00164                 m3pi.forward(1);
00165             } else if (position > 0) {
00166                 m3pi.left_motor(0.8);
00167                 m3pi.right_motor(1);
00168             }
00169  
00170             break;
00171     }
00172 
00173     //previousLine = isLine;
00174 }
00175 
00176 void clearLiner() {             // Gets called a bit after a line is detected
00177     lineCatch = 0;
00178 }
00179