Diff: Roboter.cpp

Revision:

4:fdcf3b5009c6

Parent:

3:24e098715b78

Child:

6:4af735d26b7a

diff -r 24e098715b78 -r fdcf3b5009c6 Roboter.cpp
--- a/Roboter.cpp	Tue Apr 04 13:27:59 2017 +0000
+++ b/Roboter.cpp	Tue Apr 11 11:47:04 2017 +0000
@@ -1,7 +1,8 @@
 #include "Roboter.h"
 
 
-Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2)
+Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2,DigitalIn* switch1, DigitalIn* switch2)
+
 {
 
     sensors[0].init( distance, bit0, bit1, bit2, 0);
@@ -13,6 +14,11 @@
 
     this->pwmR = pwmR;
     this->pwmL = pwmL;
+    this->s1 = s1;
+    this->s2 = s2;
+    this->switch1 = switch1;
+    this->switch2 = switch2;
+
 
     pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
     pwmR->period(0.00005f);
@@ -54,7 +60,6 @@
     *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
     *pwmR = 0.5f+offsetDir-offsetLin;
 
-    wait(0.1);
 }
 
 
@@ -67,10 +72,22 @@
 
 }
 
-void Roboter::turn()      //im Kreis drehen
+void Roboter::findBlock()      //im Kreis drehen
 {
+    static int count =0;
+    count++;
+
+    if(count <= 10);      //1s lang (10*0.1s) im Kreis drehen
     *pwmL = 0.55f;
-    *pwmR = 0.45f;
+    *pwmR = 0.55f;
+
+    if(count > 10 && count <= 20); //zwischen Sekunde 1 und 2 geradeaus fahren
+    *pwmL = 0.6f;
+    *pwmR = 0.4f;
+
+    if(count > 20 && count <=30); //zwischen Sekunde 2 und 3 im Kreis drehen
+    *pwmL = 0.55f;
+    *pwmR = 0.55f;
 
 }