Diff: RasenMaeher.cpp

Revision:

6:5d2d394eb087

Parent:

5:952090ddc8eb

--- a/RasenMaeher.cpp	Tue Jun 12 11:19:48 2018 +0000
+++ b/RasenMaeher.cpp	Thu Nov 15 18:09:30 2018 +0000
@@ -10,8 +10,10 @@
 
 // Alle Funktionen die den Bertl bewegen werden mit
 // ausgeschalteten Motoren betreten und mit ausgeschalteten Motoren
-// wieder verlassen
-
+// wieder verlassen ls1 rechts, ls4 links
+ int rechts = 0, links = 0, linesens = 0;
+ Timer t1;
+ 
 void ForewardUntilWall();
 
 void SomeStepsBack();
@@ -24,27 +26,35 @@
 
 void BackBlinkerTask();
 
+void BlinkTask();
+
 // Line==left ret 1   Line==right ret 2
 int CheckLineSensors();
 
 int main(void)
 {
-    boardPow=3; wait_ms(10);
+    boardPow=1; wait_ms(10);
     InitBertl(); leds=9;
     pex.ClearLeds();
-    
-    int dir;
+    pex.WaitUntilButtonPressed();
+
+ //   int dir;
     while(1)
     {
         ForewardUntilWall();
-        dir = CheckLineSensors();
-        wait_ms(200); // for debugging
+        wait_ms(1000); // for debugging
         SomeStepsBack();
-        wait_ms(200); // for debugging
-        if( dir==1 )
-          TurnLeft();
-        else
+        wait_ms(1000); // for debugging
+        if( linesens == 1 )
+        {
+            rechts = 1;
+            TurnLeft();
+        }
+        else if (linesens == 4)
+        {
+          links = 1;
           TurnRight();
+        }
         wait_ms(200); // for debugging
     }
     
@@ -53,38 +63,106 @@
 
 void ForewardUntilWall()
 {
-  // Motorten einschalten
+  mL.SetPow(0.2); mR.SetPow(0.2);// Motoren einschalten (geradeaus)
   while(1)
   {
     FrontBlinkerTask();
-    // CheckLineSensors();
+    linesens = CheckLineSensors();
     // für Aufgabe 3 CheckButtons()
     // if( Button_pressed or LinSensor )
     //    break;
+    if (linesens != 0 )//|| pex.IsButton(BTN_FLL) || pex.IsButton(BTN_FL) || pex.IsButton(BTN_FM) || pex.IsButton(BTN_FR) || pex.IsButton(BTN_FRR))//Linie erkannt oder Front-Button gedrückt
+    {
+            break;
+    }
   }
-  // Motoren ausschalten
+  mL.SetPow(0.0); mR.SetPow(0.0);// Motoren ausschalten
+  pex.ClearLeds();//LEDs ausschalten
 }
 
 void SomeStepsBack()
-{
-  // Motoren auf rückwärts
-  // wait_ms(100);
-  // Motoren ausschalten
+{  
+    mL.SetPow(-0.2); mR.SetPow(-0.2);// Motoren einschalten (rückwärts)
+    BackBlinkerTask();
+    wait_ms(1000);
+    mL.SetPow(0.0); mR.SetPow(0.0);// Motoren ausschalten
+    pex.ClearLeds();//LEDs ausschalten
+    if (rechts == 1)
+    {
+        TurnLeft();
+        rechts = 0;
+    }
+    else if (links == 1)
+    {
+        TurnRight();
+        links = 0;
+    }
 }
 
 void TurnLeft()
 {
+        mL.SetPow(0.1); mR.SetPow(0.2);// Motor einschalten Linkskurve
+        BlinkTask();
+        wait_ms(800);
+        mL.SetPow(0.0); mR.SetPow(0.0);// Motor ausschalten
+        pex.ClearLeds();
 }
 
 void TurnRight()
 {
+        mL.SetPow(0.2); mR.SetPow(0.1);// Motor einschalten Rechtskurve
+        BlinkTask();
+        wait_ms(800);
+        mL.SetPow(0.0); mR.SetPow(0.0);// Motor ausschalten
+        pex.ClearLeds();
+
 }
 
 void FrontBlinkerTask()
 {
+    pex.SetLeds(LED_FL1);
+    pex.SetLeds(LED_FL2);
+    pex.SetLeds(LED_FR1);
+    pex.SetLeds(LED_FR2);
 }
 
 void BackBlinkerTask()
 {
+    pex.SetLeds(LED_BL1);
+    pex.SetLeds(LED_BL2);
+    pex.SetLeds(LED_BR1);
+    pex.SetLeds(LED_BR2);
 }
 
+int CheckLineSensors()
+{
+        if (ls1.Read()>600)//linesensor rechts meldet dunkel
+    {
+        return 1;//rechts
+    }
+    else if (ls4.Read()>600)//linesensor links meldet dunkel
+    {
+       return 4;//links
+    }
+    else
+        return 0;
+}
+
+void BlinkTask()
+{                   //TIMER
+    if (t1.read_ms()>100) // alle 100ms = 10Hz
+    { 
+        t1.reset(); // timer neu starten
+        if (links == 1)
+        {
+            pex.ToggleLeds(LED_BL1 | LED_BL2);
+            pex.ToggleLeds(LED_FL1 | LED_FL2);
+        }
+        else if (rechts == 1)
+        {
+            pex.ToggleLeds(LED_BR1 | LED_BR2);
+            pex.ToggleLeds(LED_FR1 | LED_FR2);
+        }
+    }
+}
+