Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Files at this revision

API Documentation at this revision

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

Fahren.cpp Show annotated file Show diff for this revision Revisions of this file
Fahren.h Show annotated file Show diff for this revision Revisions of this file
Farbauswertung.cpp Show annotated file Show diff for this revision Revisions of this file
Farbauswertung.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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();