Diff: Fahren.cpp

Revision:

8:d0a27278c108

Parent:

7:862d80e0ea2d

Child:

9:feabe0b7cea4

--- a/Fahren.cpp	Fri May 04 16:26:59 2018 +0000
+++ b/Fahren.cpp	Mon May 07 14:11:06 2018 +0000
@@ -12,14 +12,15 @@
 
 using namespace std;
 
-const float Fahren :: PERIOD = 0.8f;
+const float Fahren :: PERIOD = 0.2f;
 
 //Konstruktor
-Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin):
-    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler), reglerEin(reglerEin){
+Fahren::Fahren(Controller& controller, EncoderCounter& counterLeft, EncoderCounter& counterRight, Regler& regler, int reglerEin, IRSensor& iRSensor):
+    controller(controller), counterLeft(counterLeft), counterRight(counterRight), regler(regler), reglerEin(reglerEin), iRSensor (iRSensor){
     
-    SpeedR = 50;
-    SpeedL = 50;
+    SpeedR = 80;
+    SpeedL = 80;
+    disF = 0;
     ticker.attach(callback(this, &Fahren::getSpeed), PERIOD);
 }
 
@@ -31,12 +32,13 @@
 
 //Korrigierte Geschwindigkeit aus der Klasse Regler aufrufen
 void Fahren::getSpeed(){
-    if(reglerEin == 1){
-    SpeedR = regler.getSpeedR();
-    SpeedL = regler.getSpeedL();
-    }else {SpeedR = 50;
-            SpeedL = 50;
-    }
+    //if((reglerEinR == 1) && (reglerEinL == 1)){
+    //SpeedR = regler.getSpeedR();
+    //SpeedL = regler.getSpeedL();
+    SpeedR = 80;
+    SpeedL = 80;
+    
+    //disF = iRSensor.readF();
     
     //printf("SpeedR in F = %f\n",SpeedR);
     //printf("SpeedL in F = %f\n",SpeedL);
@@ -61,6 +63,7 @@
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
+
     
     //Drehrichtung der Motoren
     controller.setDesiredSpeedRight(SpeedR);
@@ -100,8 +103,8 @@
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 1773;
-    wegLinks = 1773;
+    wegRechts = 1776;
+    wegLinks = 1776;
         
     //Geschwindigkeit
     //speedRight = 50;//SpeedR
@@ -112,28 +115,40 @@
     //Zustand, dass der Roboter nicht gestoppt hat
     stopRight = false;
     stopLeft = false;
+    reglerEinR = 1;
+    reglerEinL = 1;
     
     //Drehrichtung der Motoren
-    controller.setDesiredSpeedRight(SpeedR);
-    controller.setDesiredSpeedLeft(-(SpeedL));
-        printf("SpeedR in F2= %f\n",SpeedR);
-        printf("SpeedL in F2= %f\n",SpeedL);
+   
+        //printf("SpeedR in F2= %f\n",SpeedR);
+        //printf("SpeedL in F2= %f\n",SpeedL);
     
     while(1){
+    float diff = (iRSensor.readR() - iRSensor.readL())*0.9f;
+    if(diff > 7.0f) diff = 0;
+    if(iRSensor.readR()>60.0f) diff=0;
+    if(iRSensor.readL()>60.0f) diff=0;
+    //printf("diff =%f \n",diff);
+    controller.setDesiredSpeedRight(SpeedR+diff);
+    controller.setDesiredSpeedLeft(-(SpeedL-diff));
+    
+    wait(0.05);
        
        // printf("Encoderrechts %d \n\r", counterRight.read());                   //Debugging Zweck (Putty benutzen!)
        // printf("Encoderlinks %d \n\r", counterLeft.read());                     //Debugging Zweck (Putty benutzen!)
         
         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen
-        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts){    
+        if(counterRight.read() - initialClicksRight >= wegRechts || counterRight.read() - initialClicksRight <= -wegRechts || iRSensor.readF() < 50.0f){    
             controller.setDesiredSpeedRight(0);
             stopRight = true;
+            reglerEinR = 0;
             }
             
         //If Schleife, wenn Clicks erreicht sind, muss der Motor stoppen    
-        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks ){
+        if(counterLeft.read() - initialClicksLeft >= wegLinks || counterLeft.read() - initialClicksLeft <= -wegLinks || iRSensor.readF() < 50.0f){
             controller.setDesiredSpeedLeft(0);
             stopLeft = true;
+            reglerEinL = 0;
             }
      
         if(stopRight == true && stopLeft == true){
@@ -252,8 +267,8 @@
     initialClicksLeft = counterLeft.read();
     
     //Anzahl Clicks die der Encoder zählen soll
-    wegRechts = 878;
-    wegLinks = 878;
+    wegRechts = 868;
+    wegLinks = 868;
     
     //Geschwindigkeit
     //speedRight = 50;