Diff: main.cpp

Revision:

5:1afe17a20a5b

Parent:

4:cc391b3f3e57

Child:

6:9eb153e1d472

--- a/main.cpp	Tue Aug 04 09:05:16 2015 +0000
+++ b/main.cpp	Thu Aug 13 15:52:19 2015 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "include/EngineDriver.h"
 #include "include/SerialPrompt.h"
+#include "include/SR04.h"
 Serial pc(USBTX, USBRX);
 
 int initDrivingUnit(int testLoops)
@@ -19,13 +20,28 @@
 }
 int main()
 {
-   // if(initDrivingUnit(1) == 0)
- //   {
-   //     pc.printf("Ready to accept commands.\n\r");
- //       displayPrompt(&pc);
-  //  }
+    if(initDrivingUnit(1) == 0)
+    {
+        pc.printf("Ready to accept commands.\n\r Netrunner will start moving in 1 second");
+        displayPrompt(&pc);
+    }
+    inState[0] = 0;
+    inState[1] = 1;
+    inState[2] = 1;
+    inState[3] = 0;
+    wait(1);
     while (true) 
     {
         int action = getMenuInput(&pc);
+        float dist = getDistance(0);
+        displayMessage(distMsg, &pc, dist);
+        if(dist <= 10)
+        {
+            resetEngines();
+        }else
+        {
+            forward();
+        }
+        applyStates();
     }
 }
\ No newline at end of file