Diff: Regler.cpp

Revision:

6:7bbcdd07bc2d

Child:

7:862d80e0ea2d

diff -r e715d157ced5 -r 7bbcdd07bc2d Regler.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Regler.cpp	Thu May 03 19:36:16 2018 +0000
@@ -0,0 +1,94 @@
+/*Roboshark V5
+Regler.cpp
+Erstellt: V.Ahlers
+geändert: V.Ahlers
+V.5.18
+Regler zum geradeaus fahren
+*/
+
+#include <cmath>
+#include "Regler.h"
+
+
+using namespace std;
+
+const float Regler :: PERIOD = 0.2f;
+const int Regler :: FIXSPEED = 50;
+float faktor = 0.02;
+
+
+
+Regler::Regler(AnalogIn& IrRight, AnalogIn& IrLeft):
+IrRight (IrRight), IrLeft (IrLeft) {
+        
+        SpeedR = 0;
+        SpeedL = 0;
+        ticker.attach(callback(this, &Regler::setSpeed), PERIOD);
+    }
+     
+Regler::~Regler(){
+    ticker.detach();
+    }
+    
+    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
+        
+        if((measR2 < 100)) {
+            if(measL2 > 280){
+                div3 = measL2 - 280;
+                kor3 = faktor*div3;
+                div2 = 0;
+                div1 = 0;
+                div4 = 0;
+                SpeedR = FIXSPEED;
+                SpeedL = FIXSPEED + kor3;
+            } else if (measR2 < 280){
+                div4 = 280 - measR2;
+                kor4 = faktor*div4;
+                div2 = 0;
+                div1 = 0;
+                div3 = 0;
+                SpeedR = FIXSPEED + kor4;
+                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;
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED + kor1;
+        } else if ((measR2 < measL2)&& meas2R > 100) {
+            div2 = measL2 - measR2;
+            kor2 = 0.02f*div2;
+            div1 = 0;
+            SpeedR = FIXSPEED + kor2;
+            SpeedL = FIXSPEED;
+        } else { 
+            SpeedR = FIXSPEED;
+            SpeedL = FIXSPEED;
+            }
+//printf("Div1 = %f\n",div1);
+//printf("Div2 = %f\n",div2);  
+//printf("SpeedR1 = %f\n",SpeedR);
+//printf("SpeedL1 = %f\n",SpeedL);        
+}
+float Regler :: get_SpeedR (){
+        SpeedR = SpeedR;
+        //printf("SpeedR2 = %f\n",SpeedR);
+        return SpeedR;
+    }
+float Regler :: get_SpeedL (){
+        SpeedL = SpeedL;
+        //printf("SpeedL2 = %f\n",SpeedL);  
+        return SpeedL;
+    }
+        
+        
+        
+            
+            
+