Diff: Fahren.cpp

Revision:

0:422088ad7fc5

Child:

1:5c44e2462a8b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Fahren.cpp	Wed May 10 08:59:22 2017 +0000
@@ -0,0 +1,133 @@
+#include "mbed.h"
+#include "cstdlib"
+#include <cmath>
+#include "Fahren.h"
+
+
+Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight)
+{
+    init(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight);
+}
+
+Fahren::Fahren() {}
+
+/**
+ * Deletes the IRSensor object.
+ */
+Fahren::~Fahren() {}
+
+void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight)
+{
+    this->enable = enable;
+    this->bit0 = bit0;
+    this->bit1 = bit1;
+    this->bit2 = bit2;
+    this->distance = distance;
+    this->enableMotorDriver = enableMotorDriver;
+    this->pwmLeft = pwmLeft;
+    this->pwmRight = pwmRight;
+    state = 0;
+}
+
+void Fahren::setSerialOutput(Serial *pc)
+{
+    this->pc = pc;
+}
+
+int Fahren::getState() {
+    return state;
+}
+
+void Fahren::fahrInit() {
+    pwmLeft->write(0.5);
+    pwmRight->write(0.5);
+    
+    for( int ii = 0; ii<6; ++ii) {
+        sensors[ii].init(distance, bit0, bit1, bit2, ii);
+
+        enable->write(1);
+
+
+        pwmLeft->period(0.00005);
+        pwmRight->period(0.00005);
+
+        enableMotorDriver->write(1);
+    }
+}
+
+void Fahren::fahrRutine() {
+    if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
+        //wenn hinten rechts => leichte linkskurve
+        if(sensors[2]<=0.25) {
+            pwmLeft=0.35;
+            pwmRight=0.55;
+            printf("zurueck-linkskurve\n");
+            state=zurueck_l;
+        }
+
+        if(sensors[4]<=0.25) {
+            pwmLeft=0.35;
+            pwmRight=0.55;
+            printf("zurueck-rechtskurve\n");
+            state=zurueck_r;
+        }
+        if(sensors[4]>=0.25 && sensors[2]>=0.25) {
+            pwmLeft=0.4;
+            pwmRight=0.6;
+            state=zurueck;
+            printf("zurueck-gerade\n");
+        }
+    }
+      
+    // Wenn Front etwas sehen => drehen***********************************
+        
+    else if(sensors[1] <= wand || sensors[5]<=wand){
+        pwmLeft=0.35;
+        pwmRight=0.65;
+    }
+    
+    else if(sensors[0]<0.25 && sensors [1]<=wenden) {  
+        pwmLeft=0.4;
+        pwmRight=0.4;   
+        printf("drehen links\n\n\n");
+    }  
+                    
+    else if (sensors[0]<0.25 && sensors [5]<=wenden){    
+        pwmLeft=0.6;
+        pwmRight=0.6;   
+        printf("drehen\n\n\n");
+               }
+    else if(sensors[0]<0.25) {
+    if (rand()%2==0 && state != drehen) {
+            pwmLeft=0.4;
+            pwmRight=0.4;
+        } else if (rand()%2 != 0 && state != drehen) {
+            pwmLeft=0.6;
+            pwmRight=0.6;
+        }
+    state=drehen;
+    }
+    //Wenn Front-Left etwas sehen => nach Rechts**************************
+    else if(sensors[5]<=wenden) {
+        printf("rechts\n");
+        pwmLeft=0.65;
+        pwmRight=0.45;
+        state=rechts;
+    }
+
+    // Wenn Front-Right etwas sehen => Links*******************************
+    else if(sensors[1]<=wenden) {
+        printf("Links\n");
+        pwmLeft=0.55;
+        pwmRight=0.35;
+        state=links;
+    }
+
+    //Wenn kein Sensor anspricht => gerade aus*****************************
+    else if(sensors[0]>=0.26) {
+        printf("gerade\n");
+        pwmLeft=0.65;
+        pwmRight=0.35;
+        state=gerade;
+    }
+}
\ No newline at end of file