Diff: main.cpp

Revision:

0:a29bcf098632

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 18 09:27:10 2012 +0000
@@ -0,0 +1,180 @@
+/**
+Name: mpi hunter
+Description:
+
+Based on work done by Giles Barton-Owen
+Cut down for use at Linton Village College
+
+This program keeps a m3pi inside a ring and looks for the other robot
+
+**/
+
+#include "mbed.h"
+#include "SharpDigiDist100.h"
+#include "m3pi.h"
+
+DigitalOut Left[2] = {LED1,LED2};           //Some indicator LEDs for the range finders
+DigitalOut Right[2] = {LED4,LED3};
+
+SharpDigiDist100 right(p30);                // The range finder class initialisations
+SharpDigiDist100 left(p11);
+
+m3pi m3pi;                                  // Initialise the m3pi
+
+InterruptIn button(p21);                    // SW1 on the shield, for strategy switching
+
+Ticker guidance;                            // The main guidance caller
+
+Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
+
+int previousLine;                           // A set of variables to sort out the line detection, previousLine is pretty much not used
+int isLine;
+int lineCatch;
+
+Timeout liner;                              // A timeout to stop it ignoring the line
+
+void CycleMode();                           // Function defs
+void guideCall();
+void clearLiner();
+
+
+int main() {
+    guidance.attach(&guideCall,0.1);        // Sets up the control loop
+    
+    m3pi.locate(0,0);                       // Write the name to the screen
+    m3pi.printf("m3PiRngr");                
+    
+    m3pi.get_white_levels();                // Saves the current levels of the sensors to know what is white
+    
+    while (1) {
+       
+        switch (right.getDistance()) {      // Sets up the distance indicator LEDs for the right side
+            case SharpDigiDist100::Far :
+                Right[0] = true;
+                Right[1] = false;
+                break;
+            case SharpDigiDist100::Near :
+                Right[1] = true;
+                Right[0] = false;
+                break;
+            case SharpDigiDist100::Mid :
+                Right[0] = true;
+                Right[1] = true;
+                break;
+            default:
+                break;
+        }
+        switch (left.getDistance()) {       // Sets up the distance indicator LEDs for the left side
+            case SharpDigiDist100::Far :
+                Left[0] = true;
+                Left[1] = false;
+                break;
+            case SharpDigiDist100::Near :
+                Left[1] = true;
+                Left[0] = false;
+                break;
+            case SharpDigiDist100::Mid :
+                Left[0] = true;
+                Left[1] = true;
+                break;
+            default:
+                break;
+        }
+}
+}
+void guideCall() {
+
+    isLine = m3pi.is_line();            // Gets whether the m3pi is on a line, and if so front/back
+    
+    if (lineCatch == 0) {               // Has it been off a line for long enough?
+        isLine = isLine;                // Yes - then go ahead
+    } else {
+        isLine = lineCatch;             // No - pretend to still be on that line
+        
+    }
+    float position;                     
+    
+    switch (isLine) {
+        case 0:                         // No line, not even recently so go ahead with the strategies
+            {
+            bool atRight = false;
+            bool atLeft = false;
+             
+                // Runs forward until something is really close
+
+                    if (right.getDistance() == SharpDigiDist100::Mid || right.getDistance() == SharpDigiDist100::Near) {
+                        atRight = true;
+                    } else atRight = false;
+                    if (left.getDistance() == SharpDigiDist100::Mid || left.getDistance() == SharpDigiDist100::Near) {
+                        atLeft = true;
+                    } else atLeft = false;
+
+                    if (atRight && atLeft) {
+                        m3pi.stop();
+                    } else {
+                        if (atRight == true) {
+                            m3pi.left_motor(0.1);
+                            m3pi.right_motor(0.2);
+                        } else {
+                            if (atLeft == true) {
+                                m3pi.left_motor(0.2);
+                                m3pi.right_motor(0.1);
+                            } else {
+                                m3pi.forward(0.3);
+                            }
+                        }
+                    }}
+             break;       
+        case 1:                 // Line in front, reverse
+            if (lineCatch == 0) {
+                lineCatch = 1;
+
+                liner.attach(&clearLiner, 0.3);
+            }
+
+            position = m3pi.line_position();
+            if (position < 0) {
+                m3pi.left_motor(-1);
+                m3pi.right_motor(-0.8);
+            } else if (position == 0) {
+                m3pi.backward(1);
+            } else if (position > 0) {
+                m3pi.left_motor(-0.8);
+                m3pi.right_motor(-1);
+            }
+
+            //m3pi.locate(0,1);
+            //m3pi.printf("LINE_FWD");
+          
+
+            break;
+
+        case -1:            // Line behind, forward
+
+            if (lineCatch == 0) {
+                lineCatch = -1;
+                liner.attach(&clearLiner, 0.3);
+            }
+
+
+            position = m3pi.line_position();
+            if (position < 0) {
+                m3pi.left_motor(1);
+                m3pi.right_motor(0.8);
+            } else if (position == 0) {
+                m3pi.forward(1);
+            } else if (position > 0) {
+                m3pi.left_motor(0.8);
+                m3pi.right_motor(1);
+            }
+ 
+            break;
+    }
+
+    //previousLine = isLine;
+}
+
+void clearLiner() {             // Gets called a bit after a line is detected
+    lineCatch = 0;
+}
+