Diff: Fahren.cpp

Revision:

16:ad45ef4fee04

Parent:

15:26dbcd6ff48d

--- a/Fahren.cpp	Wed May 24 13:54:02 2017 +0000
+++ b/Fahren.cpp	Fri May 26 09:11:49 2017 +0000
@@ -11,6 +11,7 @@
     e = 0.0;
     diff = 0.0;
     searchTimer = 0;
+    zurTimer = 40;
 }
 
 //Destruktor
@@ -32,6 +33,12 @@
             case cam:
                 pc->printf("Kamerafahrt\n\r");
                 break;
+            case camSearchRot:
+                pc->printf("Kamerafahrt Rotation\n\r");
+                break;
+            case camSearchDrive:
+                pc->printf("Kamerafahrt Fahrsuche\n\r");
+                break;
             case rechts:
                 pc->printf("Rechts\n\r");
                 break;
@@ -83,13 +90,20 @@
 
 void Fahren::fahrRutine()
 {
+    //IR-Sensoren auslesen
     s0 = sensors[0];
     s1 = sensors[1];
     s2 = sensors[2];
     s4 = sensors[4];
     s5 = sensors[5];
+    
+    //Erst prüfen ob aus einem vorhergegangenen Schritt noch eine Aktion ausgeführt werden muss
+    if ((state == hart_zurueck_r || state == hart_zurueck_l) && zurTimer<40) {
+        zurTimer++;
+    }
+    
     //Alle Sensoren genügend abstand -> Kamerafahrt
-    if (sensors[0]>0.25 && sensors[1]>0.22 && sensors[2]>0.25 && sensors[4]>0.25 && sensors[5]>0.22) {
+    else if (s0>0.25 && s1>0.22 && s2>0.25 && s4>0.25 && s5>0.22) {
         if ((pixy.getX()>=5 && pixy.getX()<=315) && pixy.getDetects() != 0 ) {
             e = 160-pixy.getX();
             diff = pid.calc( e, 0.005f );
@@ -110,28 +124,28 @@
             }
         }
     } else {
-
         //Front Links und Recht prüfen ob viel zu nahe
-        if (sensors[1]<=wand || sensors[5]<=wand) {
-            if(sensors[1]<=wand) {
+        if (s1<=wand || s5<=wand) { //Abfrage noch ändern
+            if(s1<=wand) {
                 pwmLeft = 0.45;
                 pwmRight = 0.65;
                 state = hart_zurueck_r;
+                zurTimer = 0;
             } else {
                 pwmLeft = 0.35;
                 pwmRight = 0.55;
                 state = hart_zurueck_l;
+                zurTimer = 0;
             }
-            wait(0.8);
         }
 
         //Kontrolle ob zurückgefahren werden muss
-        else if (sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
-            if(sensors[2]<=0.25) {
+        else if (s0<=0.1 && s1<=0.12 && s5<=0.12) {
+            if(s2<=0.25) {
                 pwmLeft = 0.65;
                 pwmRight = 0.65;
                 state=zurueck_l;
-            } else if(sensors[4]<=0.25) {
+            } else if(s4<=0.25) {
                 pwmLeft = 0.35;
                 pwmRight = 0.35;
                 state=zurueck_r;
@@ -143,13 +157,13 @@
         }
 
         //Kontrolle ob gedreht werden muss
-        else if(sensors[0]<0.25) {
+        else if(s0<0.25) {
             if(sensors[1]<=wenden) {
                 //Drehen Links
                 pwmLeft = 0.3;
                 pwmRight = 0.3;
                 state = drehen_l;
-            } else if(sensors[5]<=wenden) {
+            } else if(s5<=wenden) {
                 //Drehen Rechts
                 pwmLeft = 0.7;
                 pwmRight = 0.7;
@@ -168,14 +182,14 @@
         }
 
         //Rechtsfahren
-        else if(sensors[5]<=wenden && sensors[5] >= 0.08) {
+        else if(s5<=wenden && s5 >= 0.08) {
             pwmLeft = 0.65;
             pwmRight = 0.45;
             state=rechts;
         }
 
         //Linksfahren
-        else if(sensors[1]<=wenden && sensors[1] >= 0.08) {
+        else if(s1<=wenden && s1 >= 0.08) {
             pwmLeft = 0.55;
             pwmRight = 0.35;
             state=links;