Diff: main.cpp

Revision:

1:4f0d5546aa02

Parent:

0:7107985673dc

Child:

2:035fc65548f6

--- a/main.cpp	Tue Apr 25 11:48:00 2017 +0000
+++ b/main.cpp	Fri May 05 11:48:16 2017 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "Sensor_auslesen.h"
 #include "cstdlib"
+#include "Servo.h"
 
 DigitalOut enable(PC_1);
 DigitalOut bit0(PH_1);
@@ -11,6 +12,7 @@
 PwmOut pwmLeft(PA_8);
 PwmOut pwmRight(PA_9);
 IRSensor sensors[6];
+Servo Servo1(PB_7);
 
 //LED's
 DigitalOut led0(PC_8);
@@ -27,10 +29,15 @@
 int robot_state;
 enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r};
 double wenden =0.26;
+double wand =0.07;
 
 
 int main()
 {
+    //Motoren Bremsen 
+    
+ pwmLeft=0.5;
+ pwmRight=0.5;
 
 
 //Sensoren auslesen*********************************************************
@@ -50,14 +57,16 @@
     float valInG = 0;
     float valInR = 0;
 
+ 
     while( 1 ) {
+        
 
 
 
 //Farbsensor auslesen***************************************************
 
 
-        //   int range = 10000;
+       
 
         valInG = Sensor1*100.0;
         valInR = Sensor2*100.0;
@@ -65,19 +74,19 @@
         if ((valInG >= 596 && valInG <= 665) && (valInR >=370 && valInR <= 470)) {
             //roter Klotz
             zustand = 1;
+             Servo1 =0.8f;      //Grenze 0..7.4
         } else {
             if ((valInG >= 370 && valInG <= 380) && (valInR >=400 && valInR <= 610)) {
                 //gruener Klotz
                 zustand = 2;
+                 Servo1 =0.3f;      //Grenze 0..7.4
             } else {
                 //kein Klotz oder falscher Klotz
+                 Servo1 =0.8f;      //Grenze 0..7.4
                 zustand = 0;
             }
         }
 
-
-
-
 //State setzen****************************************************************
         switch (robot_state) {
 
@@ -117,8 +126,10 @@
         }
 //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) {
@@ -141,19 +152,36 @@
                 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) {
-
-            printf("drehen\n\n\n");
-            if (rand()%2==0 && robot_state != drehen && sensors[1]>=wenden) {
+        if (rand()%2==0 && robot_state != drehen) {
                 pwmLeft=0.4;
                 pwmRight=0.4;
-            } else if (rand()%2 != 0 && robot_state != drehen && sensors[5]>=wenden) {
+            } else if (rand()%2 != 0 && robot_state != drehen) {
                 pwmLeft=0.6;
                 pwmRight=0.6;
             }
-            robot_state=drehen;
+        robot_state=drehen;
         }
 
 
@@ -183,11 +211,14 @@
 
 //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());
+        printf("\n\r");
+       // printf("front:%f\n\r", sensors[0].read());
+        //printf("right:%f\n\r", sensors[1].read());
+        //printf("left:%f\n\r", sensors[5].read());
+        printf("Gruen:%f\n\r", valInG);
+        printf("Rot:%f\n\r", valInR);
 
-        wait( 0.001);
+        wait( 0.01);
 
     }