Revision 16:ad45ef4fee04, committed 2017-05-26

Comitter:

ZHAW_Prometheus

Date:

Fri May 26 09:11:49 2017 +0000

Parent:

15:26dbcd6ff48d

Commit message:

Vers. 26.05.2017 11:10

Changed in this revision

--- 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;

--- a/Fahren.h	Wed May 24 13:54:02 2017 +0000
+++ b/Fahren.h	Fri May 26 09:11:49 2017 +0000
@@ -45,6 +45,7 @@
     //Variabeln
     uint8_t         state;
     uint8_t         searchTimer;
+    uint8_t         zurTimer;
     enum            state {gerade=0, cam, rechts, links, drehen_ran, drehen_r, drehen_l, zurueck, zurueck_l, zurueck_r, hart_zurueck_r, hart_zurueck_l, camSearchRot, camSearchDrive};
     Serial          *pc;
     float           e;

--- a/Farbauswertung.cpp	Wed May 24 13:54:02 2017 +0000
+++ b/Farbauswertung.cpp	Fri May 26 09:11:49 2017 +0000
@@ -4,7 +4,7 @@
 #include "Farbauswertung.h"
 
 //Konstruktor
-Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& _servoAusw, Servo& _servoFoerder, int verzEin, int verzAus) : servoAusw(_servoAusw), servoFoerder(_servoFoerder)
+Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& _servoAusw, Servo& _servoFoerder, LiftAnsteuerung& _lift, int verzEin, int verzAus) : servoAusw(_servoAusw), servoFoerder(_servoFoerder), lift(_lift)
 {
     this->SensorG = SensorG;
     this->SensorR = SensorR;
@@ -46,29 +46,40 @@
         zustand = gruen;
     }
     merker[p1] = zustand;
-    if (p1 < (merkerSize-1)) {
-        p1++;
-    } else {
-        p1 = 0;
-    }
 
     //Zustände an der Klappe abrufen und Klappe öffnen und schliessen
     sum = 0;
-    for (uint16_t i = 0; i<m_verzAus; i++) {
+    uint8_t i = 0;
+    while (i<m_verzAus) {
         if ((p2+i) < merkerSize) {
             sum += merker[p2+i];
         } else {
             sum += merker[p2+i-merkerSize];
         }
+        i++;
+    }
+    
+    //Klappe öffnen oder schliessen
+    if (sum > 0) {
+        servoAusw = 1.0f;
+    } else {
+        servoAusw = 0.01f;
+    }
+    
+    //Liftmodul alle vier Durchgänge aufrufen
+    if (p1%4 == 0) {
+        lift.steuerung();
+    }
+    
+    //Zähler aufsummieren oder zurücksetzen.
+    if (p1 < (merkerSize-1)) {
+        p1++;
+    } else {
+        p1 = 0;
     }
     if (p2 < (merkerSize-1)) {
         p2++;
     } else {
         p2 = 0;
     }
-    if (sum > 0) {
-        servoAusw = 1.0f;
-    } else {
-        servoAusw = 0.01f;
-    }
 }

--- a/Farbauswertung.h	Wed May 24 13:54:02 2017 +0000
+++ b/Farbauswertung.h	Fri May 26 09:11:49 2017 +0000
@@ -5,12 +5,13 @@
 #include <cstdlib>
 #include "Farbsensor.h"
 #include "Servo.h"
+#include "liftAnsteuerung.h"
 
 class Farbauswertung 
 {
 public:
     //Konstruktoren und Destruktoren
-    Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& servoAusw, Servo& servoFoerder, int verzEin, int verzAus);
+    Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& servoAusw, Servo& servoFoerder, LiftAnsteuerung& lift, int verzEin, int verzAus);
     Farbauswertung();
     virtual     ~Farbauswertung();
     
@@ -40,6 +41,7 @@
     uint8_t     zustand;
     enum        zustand {gruen=0, rot};
     Farbsensor  farbsensor;
+    LiftAnsteuerung& lift;
     Serial      *pc;
 };
 

--- a/main.cpp	Wed May 24 13:54:02 2017 +0000
+++ b/main.cpp	Fri May 26 09:11:49 2017 +0000
@@ -41,9 +41,9 @@
 Pixy            pixy(cam);
 
 //Robotersteuerung
-Farbauswertung  farbauswertung(&SensorG, &SensorR, ServoAusw, ServoFoerder, 4, 8);
+LiftAnsteuerung lift(7, 1, ServoLift);
+Farbauswertung  farbauswertung(&SensorG, &SensorR, ServoAusw, ServoFoerder, lift, 4, 8);
 Fahren          fahren(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight, pixy, pid);
-LiftAnsteuerung lift(7, 1, ServoLift);
 
 
 int main() 
@@ -54,9 +54,8 @@
     //farbauswertung.setSerialOutput(&pc);
     //fahren.setSerialOutput(&pc);
     
-    Ticker farbe;
+    Ticker auswertung;
     Ticker drive;
-    Ticker elevator;
     
     pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
     fahren.fahrInit();
@@ -65,12 +64,10 @@
         wait(1.0);
     }
     
-    farbe.attach(&farbauswertung, &Farbauswertung::auswertung, 0.5);
+    auswertung.attach(&farbauswertung, &Farbauswertung::auswertung, 0.5);
     
     drive.attach(&fahren, &Fahren::fahrRutine, 0.02);
     
-    elevator.attach(&lift, &LiftAnsteuerung::steuerung, 2);
-    
     while (1) {
         wait(10.0);
         //farbauswertung.printState();