Diff: main.cpp

Revision:

0:7107985673dc

Child:

1:4f0d5546aa02

diff -r 000000000000 -r 7107985673dc main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 25 11:48:00 2017 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+#include "Sensor_auslesen.h"
+#include "cstdlib"
+
+DigitalOut enable(PC_1);
+DigitalOut bit0(PH_1);
+DigitalOut bit1(PC_2);
+DigitalOut bit2(PC_3);
+
+DigitalOut enableMotorDriver(PB_2);
+PwmOut pwmLeft(PA_8);
+PwmOut pwmRight(PA_9);
+IRSensor sensors[6];
+
+//LED's
+DigitalOut led0(PC_8);
+DigitalOut led1(PC_6);
+DigitalOut led5(PC_9);
+DigitalOut led3(PC_7);
+
+// Analog In
+AnalogIn distance(PB_1);
+AnalogIn Sensor1(PA_0);
+AnalogIn Sensor2(PA_1);
+
+//Variablen definieren*********************************************************
+int robot_state;
+enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r};
+double wenden =0.26;
+
+
+int main()
+{
+
+
+//Sensoren auslesen*********************************************************
+    for( int ii = 0; ii<6; ++ii) {
+        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
+
+        enable = 1;
+
+
+        pwmLeft.period(0.00005);
+        pwmRight.period(0.00005);
+
+        enableMotorDriver =1;
+    }
+
+    int zustand=0;           // variable to store the value read
+    float valInG = 0;
+    float valInR = 0;
+
+    while( 1 ) {
+
+
+
+//Farbsensor auslesen***************************************************
+
+
+        //   int range = 10000;
+
+        valInG = Sensor1*100.0;
+        valInR = Sensor2*100.0;
+
+        if ((valInG >= 596 && valInG <= 665) && (valInR >=370 && valInR <= 470)) {
+            //roter Klotz
+            zustand = 1;
+        } else {
+            if ((valInG >= 370 && valInG <= 380) && (valInR >=400 && valInR <= 610)) {
+                //gruener Klotz
+                zustand = 2;
+            } else {
+                //kein Klotz oder falscher Klotz
+                zustand = 0;
+            }
+        }
+
+
+
+
+//State setzen****************************************************************
+        switch (robot_state) {
+
+            case gerade:
+                led0=1;
+                led1=0;
+                led3=0;
+                led5=0;
+                break;
+
+            case rechts:
+                led0=0;
+                led1=1;
+                led3=0;
+                led5=0;
+                break;
+
+            case links:
+                led0=0;
+                led1=0;
+                led3=0;
+                led5=1;
+                break;
+
+            case drehen:
+                led0=1;
+                led1=1;
+                led3=0;
+                led5=0;
+                break;
+
+            case zurueck:
+                led0=0;
+                led1=0;
+                led3=1;
+                led5=0;
+        }
+//Sensorden abfrgagen und Motoren stellen***************************************
+
+// Wenn Front + Front-Right + Front-Left etwas sehen => gerade zurück bis nur noch einer etwas sieht
+        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");
+                robot_state=zurueck_l;
+            }
+
+            if(sensors[4]<=0.25) {
+                pwmLeft=0.35;
+                pwmRight=0.55;
+                printf("zurueck-rechtskurve\n");
+                robot_state=zurueck_r;
+            }
+            if(sensors[4]>=0.25 && sensors[2]>=0.25) {
+                pwmLeft=0.4;
+                pwmRight=0.6;
+                robot_state=zurueck;
+                printf("zurueck-gerade\n");
+            }
+        }
+
+        // Wenn Front etwas sehen => drehen
+        else if(sensors[0]<0.25) {
+
+            printf("drehen\n\n\n");
+            if (rand()%2==0 && robot_state != drehen && sensors[1]>=wenden) {
+                pwmLeft=0.4;
+                pwmRight=0.4;
+            } else if (rand()%2 != 0 && robot_state != drehen && sensors[5]>=wenden) {
+                pwmLeft=0.6;
+                pwmRight=0.6;
+            }
+            robot_state=drehen;
+        }
+
+
+        //Wenn Front-Left etwas sehen => nach Rechts
+        else if(sensors[5]<=wenden) {
+            printf("rechts\n");
+            pwmLeft=0.65;
+            pwmRight=0.45;
+            robot_state=rechts;
+        }
+
+        // Wenn Front-Right etwas sehen => Links
+        else if(sensors[1]<=wenden) {
+            printf("Links\n");
+            pwmLeft=0.55;
+            pwmRight=0.35;
+            robot_state=links;
+        }
+
+        //Wenn kein Sensor anspricht => gerade aus
+        else if(sensors[0]>=0.26) {
+            printf("gerade\n");
+            pwmLeft=0.65;
+            pwmRight=0.35;
+            robot_state=gerade;
+        }
+
+//printf("sensorG: %f\t", valInG);
+//printf("sensorR: %f\n\r", valInR);
+        printf("front:%f\n\r", sensors[0].read());
+        printf("right:%f\n\r", sensors[1].read());
+        printf("left:%f\n\r", sensors[5].read());
+
+        wait( 0.001);
+
+    }
+
+}