Diff: BertlTemplate.cpp

Revision:

5:ea4229d52ab6

Parent:

4:4b71af20b3b7

--- a/BertlTemplate.cpp	Tue May 24 11:44:20 2016 +0000
+++ b/BertlTemplate.cpp	Tue May 30 09:46:24 2017 +0000
@@ -1,3 +1,4 @@
+
 
 #include "mbed.h"
 #include "Serial_HL.h"
@@ -8,23 +9,64 @@
 BusOut boardPow(p30,      P1_6, P1_7);
 
 // 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
+AnalogInHL ls1(p18), ls2(p16), ls3(p19), ls4(p17); // B14
+void FollowLineUntilEnd();
+void TurnUntilLine();
 
 int main(void)
 {
-    boardPow=1; wait_ms(10);
     InitBertl();
     pex.useISR=0; leds=9;
     pex.ClearLeds();
+    pex.WaitUntilButtonPressed();
+    boardPow = 3;
+    wait_ms(10);
+        
+    while(1)
+    {
+
+     FollowLineUntilEnd();
+     wait_ms(1000);
+     TurnUntilLine();
+     wait_ms(1000);
+     }
+}   
+
+void FollowLineUntilEnd()
+{
+    mL.SetPow(0.2); mR.SetPow(0.2);
     
     while(1)
     {
-        mL.SetPow(0.2); mR.SetPow(0.2);
+        if(ls1.Read() > 900)
+        {
+            mL.SetPow(0); mR.SetPow(0.8);
+        }  
+        else
+        {
+            mL.SetPow(0.8); mR.SetPow(0);
+        }  
+        
+        if(ls1.Read() < 300 && ls2.Read() < 300 && ls3.Read() < 300 && ls4.Read() < 300)
+        {
+            mL.SetPow(0); mR.SetPow(0);
+            return;
+        }
+        
     }
+}
     
-    return 1;
+void TurnUntilLine()
+{
+    mL.SetPow(0.8); mR.SetPow(-0.8);
+    while(1)
+    {        
+        if(ls1.Read() > 900 || ls2.Read() > 900 || ls3.Read() > 900 || ls4.Read() > 900)
+        {
+            mL.SetPow(0);mR.SetPow(0); 
+            return;
+        }
+        
+    }
 }
-
-
+        
\ No newline at end of file