Diff: Bertel_PingDONG.cpp

Revision:

2:d40301306dd7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Bertel_PingDONG.cpp	Mon Feb 29 07:50:08 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "Serial_HL.h"
+#include "Bertl14.h"
+#include "BertlObjects.h"
+
+BusOut boardPow(p30, P1_6,P1_7);
+
+Timer t_main;
+Timer t_blink;
+
+void Forward();
+void RightCurve();
+void LeftCurve();
+
+int main(void) {
+    boardPow=1; wait_ms(10);
+    InitBertl();
+    pex.useISR=0; leds=9;
+    pex.ClearLeds();
+    pex.WaitUntilFrontButtonPressed();
+    t_main.start();
+    t_blink.start();
+    
+    while(1) {
+        pex.ReadButtons(); 
+        if(pex.IsButton(BTN_FM)) Forward();
+        if(pex.IsButton(BTN_FRR)) RightCurve();
+        if(pex.IsButton(BTN_FLL)) LeftCurve();
+    }
+}
+
+void Forward() {
+    t_main.reset();
+    t_blink.reset();
+    mL.SetPow(0.3);mR.SetPow(0.3);
+    
+    while (t_main.read_ms() < 5000) {
+        if (t_blink.read_ms() > 100) {
+            t_blink.reset();
+            pex.ToggleLeds(LED_FL1|LED_FR1);
+        }
+    }
+    pex.ClearLeds();
+    mL.SetPow(0);mR.SetPow(0);
+}
+
+void RightCurve() {
+    t_main.reset();
+    t_blink.reset();
+    mL.SetPow(0.3);mR.SetPow(0.15);
+    
+    while (t_main.read_ms() < 5000) {
+        if (t_blink.read_ms() > 100) {
+            t_blink.reset();
+            pex.ToggleLeds(LED_FR1);
+        }
+    }
+    pex.ClearLeds();
+    mL.SetPow(0);mR.SetPow(0);
+}
+    
+void LeftCurve() {
+    t_main.reset();
+    t_blink.reset();
+    mL.SetPow(0.15);mR.SetPow(0.3);
+    
+    while (t_main.read_ms() < 5000) {
+        if (t_blink.read_ms() > 100) {
+            t_blink.reset();
+            pex.ToggleLeds(LED_FL1);
+        }
+    }
+    pex.ClearLeds();
+    mL.SetPow(0);mR.SetPow(0);
+}
\ No newline at end of file