Diff: Regler.cpp

Revision:

7:862d80e0ea2d

Parent:

6:7bbcdd07bc2d

Child:

8:a7f1ee7840d0

--- a/Regler.cpp	Thu May 03 19:36:16 2018 +0000
+++ b/Regler.cpp	Fri May 04 16:26:59 2018 +0000
@@ -8,18 +8,19 @@
 
 #include <cmath>
 #include "Regler.h"
+#include "IRSensor.h"
 
 
 using namespace std;
 
 const float Regler :: PERIOD = 0.2f;
 const int Regler :: FIXSPEED = 50;
-float faktor = 0.02;
+float faktor = 0.1;
 
 
 
-Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft):
-IrRight (IrRight), IrLeft (IrLeft) {
+Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft, IRSensor& iRSensor):
+IrRight (IrRight), IrLeft (IrLeft), iRSensor (iRSensor) {
         
         SpeedR = 0;
         SpeedL = 0;
@@ -31,40 +32,79 @@
     }
     
     void Regler::setSpeed (){
-        measR2 = IrRight.read();            // Converts and read the analog input value (value from 0.0 to 1.0)
-        measR2 = measR2* 1000;              // Change the value to be in the 0 to 1000 range
-        measL2 = IrLeft.read();             // Converts and read the analog input value (value from 0.0 to 1.0)
-        measL2 = measL2 * 1000;             // Change the value to be in the 0 to 1000 range
+        measR2 = iRSensor.readR();            // Converts and read the analog input value (value from 0.0 to 1.0)
+        measL2 = iRSensor.readL();             
         
-        if((measR2 < 100)) {
-            if(measL2 > 280){
-                div3 = measL2 - 280;
+        if((measR2 > 100) && (measL2 < 100)) {                //keine Wnad rechts
+            if(measL2 > 47){
+                div3 = measL2 - 47;
                 kor3 = faktor*div3;
                 div2 = 0;
                 div1 = 0;
                 div4 = 0;
+                div5 = 0;
+                div6 = 0;
                 SpeedR = FIXSPEED;
                 SpeedL = FIXSPEED + kor3;
-            } else if (measR2 < 280){
-                div4 = 280 - measR2;
+            } else if (measL2 < 52){
+                div4 = 52 - measR2;
                 kor4 = faktor*div4;
                 div2 = 0;
                 div1 = 0;
                 div3 = 0;
-                SpeedR = FIXSPEED + kor4;
+                div5 = 0;
+                div6 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor4;
+                }else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+        }
+        if((measL2 > 100) &&(measR2 < 100)) {                //keine Wnad links
+            if(measR2 > 47){
+                div5 = measR2 - 47;
+                kor5 = faktor*div5;
+                div2 = 0;
+                div1 = 0;
+                div4 = 0;
+                div6 = 0;
+                div3 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor5;
+            } else if (measR2 < 52){
+                div6 = 52 - measR2;
+                kor6 = faktor*div4;
+                div2 = 0;
+                div1 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                SpeedR = FIXSPEED + kor6;
                 SpeedL = FIXSPEED;
-                }
-    
-        if ((measR2 > measL2) && (meas2R > 100)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
-            div1 = measR2 - measL2;
-            kor1 = faktor*div1;
-            div2 = 0;
+            } else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+        }
+        if ((measR2 < measL2)&& (measL2 - measR2 > 3)) {              //IR Sensor werte werden verglichen und die Korrektur wird berechnet
+            div1 = measR2 - measL2;                             // An beiden seinen Wände
+            kor1 = 0.1f*div1;
+                div2 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                div6 = 0;
             SpeedR = FIXSPEED;
             SpeedL = FIXSPEED + kor1;
-        } else if ((measR2 < measL2)&& meas2R > 100) {
+        } else if ((measR2 > measL2) && (measR2 - measL2 >3)) {
             div2 = measL2 - measR2;
-            kor2 = 0.02f*div2;
-            div1 = 0;
+            kor2 = 0.1f*div2;
+                div1 = 0;
+                div3 = 0;
+                div4 = 0;
+                div5 = 0;
+                div6 = 0;
             SpeedR = FIXSPEED + kor2;
             SpeedL = FIXSPEED;
         } else { 
@@ -76,17 +116,16 @@
 //printf("SpeedR1 = %f\n",SpeedR);
 //printf("SpeedL1 = %f\n",SpeedL);        
 }
-float Regler :: get_SpeedR (){
+    float Regler :: getSpeedR (){
         SpeedR = SpeedR;
         //printf("SpeedR2 = %f\n",SpeedR);
         return SpeedR;
     }
-float Regler :: get_SpeedL (){
+    float Regler :: getSpeedL (){
         SpeedL = SpeedL;
         //printf("SpeedL2 = %f\n",SpeedL);  
         return SpeedL;
-    }
-        
+    }