Diff: main.cpp

Revision:

0:ee556de472a9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 11 08:44:55 2014 +0000
@@ -0,0 +1,68 @@
+#include "mbed.h"
+#include "m3pi.h"
+
+m3pi m3pi;
+
+enum State {RUN, ROTATE};
+
+int main() {
+    float speed = 0.2;
+    float correction = 0.05;   
+    int threshold = 500;
+    float turn30 = 0.2;
+
+    int sens[5]; //left, sub-l, center, sub-r, right
+    State state = RUN;
+        
+    m3pi.cls();
+    m3pi.locate(0,0);
+    m3pi.printf("Line");
+    m3pi.locate(0,1);
+    m3pi.printf("  Tracer");
+
+    wait(2.0);
+    
+    m3pi.sensor_auto_calibrate();
+
+    
+    while(1) {
+        m3pi.putc(0x87); //get values of calibrated sensors (0-1000)
+        for(int i=0; i < 5; ++i) {
+            sens[i] = m3pi.getc();
+            sens[i] += m3pi.getc() << 8;
+        }
+
+        switch(state) {
+        case RUN:
+            /* Turn left (over 30deg.) */
+            if(sens[0] > threshold ||
+               sens[2] <= threshold && sens[1] <= threshold && sens[3] <= threshold) {
+                m3pi.stop();
+                wait(0.1);
+                m3pi.left(speed);
+                wait(turn30);
+                m3pi.stop();
+            
+                state = ROTATE;
+            }
+            /* Go forward */
+            else {
+                if(sens[2] > threshold) {  // Go straight
+                  m3pi.forward(speed);
+                } else if(sens[1] > threshold) { //Correction (left)
+                    m3pi.left_motor(speed - correction);
+                } else { //Correction (right)
+                    m3pi.right_motor(speed - correction);
+                }
+            }
+            break;
+        case ROTATE: // Rotate clockwise and find a road
+            if(sens[2] > threshold) {
+                m3pi.stop();
+                state = RUN;
+            }
+            m3pi.right(speed);
+            break;
+        }
+    }
+}