Diff: Fahren.cpp

Revision:

8:077d0bb213a2

Parent:

7:5949f408b6da

Child:

9:b83994ef4b08

--- a/Fahren.cpp	Wed May 17 07:38:41 2017 +0000
+++ b/Fahren.cpp	Thu May 18 09:12:46 2017 +0000
@@ -5,9 +5,9 @@
 
 
 
-Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Camauswertung* cam)
+Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Pixy* pixy, PID_Control* pid)
 {
-    init(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight, cam);
+    init(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight, pixy, pid);
 }
 
 Fahren::Fahren() {}
@@ -17,7 +17,7 @@
  */
 Fahren::~Fahren() {}
 
-void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Camauswertung* cam)
+void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Pixy* pixy, PID_Control* pid)
 {
     this->enable = enable;
     this->bit0 = bit0;
@@ -27,7 +27,7 @@
     this->enableMotorDriver = enableMotorDriver;
     this->pwmLeft = pwmLeft;
     this->pwmRight = pwmRight;
-    this->cam = cam;
+    this->pixy = pixy;
     state = 0;
     
 
@@ -63,7 +63,7 @@
 void Fahren::fahrRutine()
 {
     if (pc) {
-        pc->printf("left:%f\n\r", sensors[5].read());
+        //pc->printf("left:%f\n\r", sensors[5].read());
     }
     if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
         //wenn hinten rechts => leichte linkskurve
@@ -180,16 +180,24 @@
         state=links;
     }
 
-    //Wenn kein Sensor anspricht => gerade aus*****************************
+    //Wenn kein Sensor anspricht => PixyCam wird abgerufen*****************************
     else if(sensors[0]>=0.26) {
-        if(pc) {
-            pc->printf("gerade\n");
+        if (pixy->getX()){
+            if (pc) {
+                pc->printf("Cam Fahren\tX:\t%d\n\r", pixy->getX());
+            }
+            e = 160-pixy->getX();
+            diff = pid->calc( e, 0.005f );
+            pwmLeft->write(0.65-diff);
+            pwmRight->write(0.35-diff);
+        } else {
+            if(pc) {
+                pc->printf("gerade\n\r");
+            }
+            pwmLeft->write(0.65);
+            pwmRight->write(0.35);
+            state=gerade;
         }
-        
-        pwmLeft->write(0.65);
-        pwmRight->write(0.35);
-        
-        state=gerade;
     } else {
         if(pc) {
             pc->printf("gerade2\n\r");