Diff: Classes/Spurhaltung.cpp

Revision:

5:64d7b4b69fdf

Parent:

3:f44ef28cfb2d

--- a/Classes/Spurhaltung.cpp	Tue May 01 11:42:38 2018 +0000
+++ b/Classes/Spurhaltung.cpp	Sat May 12 12:49:18 2018 +0000
@@ -3,6 +3,15 @@
 
 using namespace std;
 
+const int Spurhaltung::DISTANCE = 100;
+const float Spurhaltung::SPEED = 100;
+const int Spurhaltung::DIFF = 1;
+const int Spurhaltung::DIFF2 = 120;
+const int Spurhaltung::DIFF3 = 180;
+const int Spurhaltung::OFFSET = 30;
+const int Spurhaltung::TOLERANCE1 = 8;
+const int Spurhaltung::TOLERANCE2 = -8;
+
 Spurhaltung::Spurhaltung(IRSensorG& Sensor2, IRSensorG& Sensor3):
     Sensor2(Sensor2), Sensor3(Sensor3)
 
@@ -17,25 +26,90 @@
 
 int Spurhaltung::speedl()
 {
-    if(abs(Sensor3.read()-Sensor2.read())<100) {
-        
-        int sl = Sensor2.read() * 1.825;
-        return sl;
-    } else {
-        int sl = 150;
-        return sl;
+    // Wand links und rechts
+    if(abs(Sensor2.read()+OFFSET-Sensor3.read())<DIFF3) {
+        int sl = Sensor3.read() * 1.825 - 30;
+        printf("");
+        if(Sensor3.read() > (DISTANCE + 50)) {
+            sl = SPEED;
+            return sl;
+        } else {
+            return sl;
+        }
     }
-}
+    // nur Wand links
+    else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
+        if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
+            int sl = 80 - DIFF;
+            //printf("Fridolin\r\n");
+            return sl;
+        } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
+            int sl = 80 + DIFF;
+            return sl;
+        } else {
+            sl = SPEED;
+            return sl;
+        }
+    }
+
+    // nur Wand rechts
+    else if ((Sensor3.read()<DIFF2) and (Sensor2.read()>DIFF3)) {
+        if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
+            sl = 80 + DIFF;
+            //printf("Fridolin\r\n");
+            return sl;
+        } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
+            int sl = 80 - DIFF;
+            return sl;
+        } else {
+            sl = SPEED;
+            return sl;
+        }
+        }
+    }
+
 
 //------------------------------------------------------------------------------
 
 int Spurhaltung::speedr()
 {
-    if(abs(Sensor2.read()-Sensor3.read())<100) {
-        int sr = Sensor3.read() * 1.825;
-        return sr;
-    } else {
-        int sr = 150;
-        return sr;
+    // Wand links und rechts
+    if(abs(Sensor3.read()-Sensor2.read()+OFFSET)<DIFF3) {
+        int sr = (Sensor2.read()) * 1.825;
+        printf("");
+        if(Sensor2.read() > (DISTANCE - OFFSET + 50)) {
+            sr = SPEED;
+            return sr;
+        } else {
+            return sr;
+        }
     }
-}
\ No newline at end of file
+    // nur Wand links
+    else if ((Sensor2.read() < DIFF2) and (Sensor3.read() > DIFF3)) {
+        if ((Sensor2.read() - DISTANCE + OFFSET) > TOLERANCE1) {
+            int sr = 80 + DIFF;
+            //printf("Karolina\r\n");
+            return sr;
+        } else if ((Sensor2.read() - DISTANCE + OFFSET) < TOLERANCE2) {
+            int sr = 80 - DIFF;
+            return sr;
+        } else {
+            sr = SPEED;
+            return sr;
+        }
+    }
+    // nur Wand rechts
+    else if((Sensor3.read() < DIFF2) and (Sensor2.read() > DIFF3)) {
+        if ((Sensor3.read() - DISTANCE) > TOLERANCE1) {
+            int sr = 80 - DIFF;
+            //printf("Karolina\r\n");
+            return sr;
+        } else if ((Sensor3.read() - DISTANCE) < TOLERANCE2) {
+            int sr = 80 + DIFF;
+            return sr;
+        } else {
+            sr = SPEED;
+            return sr;
+        }
+    }
+}