BingBong

Dependencies:   BertlLib mbed

Fork of BertlTemplate2 by michael hollegha

Revision:
4:3aca65c975f0
Parent:
3:57fc35fcd060
--- a/BertlTemplate.cpp	Mon May 02 15:49:14 2016 +0000
+++ b/BertlTemplate.cpp	Mon May 30 15:46:55 2016 +0000
@@ -1,87 +1,100 @@
-
+ 
 #include "mbed.h"
 #include "Serial_HL.h"
 #include "Bertl14.h"
 #include "BertlObjects.h"
 
+SerialBLK pc(USBTX, USBRX);
+SvProtocol ua0(&pc);
+
 //              main=2^0  LS    ENC 2^2
 BusOut boardPow(p30,      P1_6, P1_7);
 
-Timer t1,t2;
+// ls5 nur beim Betrl15
+// AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17); // B14
+
+AnalogInHL ls1(p18), ls2(p16), ls3(p20), ls4(p19), ls5(p17); // B15
+
+
+void rightCurve()
+{
+  mL.SetPow(0.3); //rechts
+  mR.SetPow(0.0);  
+}
 
-void BlinkTask_Forward()
+void leftCurve()
+{
+    mL.SetPow(0.0);   //links
+    mR.SetPow(0.3);
+}
+
+void straight()
+{
+    mL.SetPow(0.3); //gerade
+    mR.SetPow(0.3);
+}
+
+void stop()
 {
-    if(t1.read_ms()>100) {
-        t1.reset();
-        pex.ToggleLeds(LED_ALL_FRONT);}
-        
+    mL.SetPow(0.0);
+    mR.SetPow(0.0);
+    wait_ms(1000);
 }
-void BlinkTask_Backward()
+
+const int LS_THRSH = 400;
+
+void LineFollow()
 {
-    if(t2.read_ms()>200) {
-        t2.reset();
-        pex.ToggleLeds(LED_ALL_BACK);
-        
+    while(1){
+    if (ls1.Read() > LS_THRSH || ls2.Read() > LS_THRSH )
+        {
+          leftCurve();
+        }
+            
+         else if(ls4.Read() > LS_THRSH || ls5.Read() > LS_THRSH )
+        {
+          rightCurve();
+        }    
+        else if (ls3.Read() > LS_THRSH)
+        {
+           straight(); 
+        }
+        else if (ls1.Read() < LS_THRSH || ls2.Read() < LS_THRSH || ls3.Read() < LS_THRSH || ls4.Read() < LS_THRSH || ls5.Read() < LS_THRSH)
+        {
+         stop();
+         return;   
+        }
+        else
+         straight();
     }
 }
 
-void ForwardUntilWall()
+ 
+void TurnUntilLine()
 {
-    mL.SetPow2(0.3);
-    mR.SetPow2(0.3); //Leistung 30% .. 0.3
-    
+    mL.SetPow2(-0.3);
+    mR.SetPow2(0.3);
     
-    while(1) {
-        pex.ReadButtons();
-        if (pex.IsButton (BTN_FM))
-            break;
-        BlinkTask_Forward();
+    while(1){
+    
+    if (ls3.Read()>400){
+        mL.SetPow2(0.0);
+        mR.SetPow2(0.0);
+        return;
+        }
     }
-    mL.SetPow2(0.0);
-    mR.SetPow2(0.0);
-    pex.ClearLeds();
 }
 
-void BackwardUntilWall()
-{
-    
-    mL.SetPow2(-0.3);
-    mR.SetPow2(-0.3); //Leistung 30% .. 0.3
-   
-    
-    while(1) {
-        pex.ReadButtons();
-        if (pex.IsButton (BTN_BM))
-            break;
-            BlinkTask_Backward();
-    }
-    mL.SetPow2(0.0);
-    mR.SetPow2(0.0);
-    pex.ClearLeds();
-}
-
-
 int main(void)
 {
-    boardPow=1;
-    wait_ms(10);
-    InitBertl();
-    pex.useISR=0;
-    leds=0;
-    pex.ClearLeds();
-
-    pex.WaitUntilFrontButtonPressed();
-    t1.start();
-    t2.start();
-
+    pc.format(8,SerialBLK::None,1);
+    pc.baud(115200); // 115200
+    boardPow = 3;
+        
     while(1) {
-
-        ForwardUntilWall();
-        BackwardUntilWall();
-
+        LineFollow();
+        wait_ms(1000);
+        TurnUntilLine();
+        wait_ms(1000);
     }
-    return 1;
-
 }
-
-