Diff: Roboter.cpp

Revision:

21:69ee872b8ee9

Parent:

20:a90e5b54bf7b

--- a/Roboter.cpp	Sat May 13 16:50:57 2017 +0000
+++ b/Roboter.cpp	Tue May 16 16:09:08 2017 +0000
@@ -28,12 +28,11 @@
 int Roboter::readSensors()            // Sensoren auslesen
 {
     int lr = 1;
-    float x = 0.13;                 // Distanz ab welcher sensoren reagieren sollen
+    float x = 0.1;                 // Distanz ab welcher sensoren reagieren sollen
 
     if(sensors[1] < x || sensors[5] < x) {            // Sensor rechts und links
         lr = 6;
     }
-    //printf("%d",lr);
     return lr;
 
 }
@@ -41,15 +40,15 @@
 
 void Roboter::ausweichen1()         // Hindernissen ausweichen
 {
-    float x = 0.15;       // Distanz ab welcher sensoren reagieren sollen
+    float x = 0.14;       // Distanz ab welcher sensoren reagieren sollen
 
-    while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
-        desiredSpeedLeft = -v*2;
-        desiredSpeedRight = v*2;
+    while(sensors[0] < (x-0.01f) && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
+        desiredSpeedLeft = -v;
+        desiredSpeedRight = v*4;
 
     }
 
-    while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links
+    while(sensors[0] < x) { // sensor vorne, roboter dreht nach links
         desiredSpeedLeft = -v*2;
         desiredSpeedRight = -v*2;
 
@@ -61,7 +60,7 @@
 
     }
 
-    while(sensors[5] < x && sensors[1] > x) {     // sensor links, roboter dreht nach rechts
+    while(sensors[5] < x) {     // sensor links, roboter dreht nach rechts
         desiredSpeedLeft = v*2;
         desiredSpeedRight = v*2;
 
@@ -71,7 +70,7 @@
 
 int Roboter::ausweichen2()         // Hindernissen ausweichen beim Anfahren des Klotzes
 {
-    float x = 0.13;                 // Distanz ab welcher sensoren reagieren sollen
+    float x = 0.1;                 // Distanz ab welcher sensoren reagieren sollen
     static int l = 0;
     static int r = 0;
     static int ii = 0;
@@ -86,24 +85,24 @@
     }
 
     if(r == 1) {
-        if(ii < 500) {                      // linksdrehend rückwärts fahren
+        if(ii < 500) {                      // rückwärts fahren
             desiredSpeedLeft = -(v+30);
             desiredSpeedRight = v+30;
         }
-        if(ii > 499 && ii < 1100) {         // links drehen
+        if(ii > 499 && ii < 900) {         // links drehen
             desiredSpeedLeft = -v*2;
             desiredSpeedRight = -v*2;
         }
-        if(ii > 1099 && ii < 2900) {         // gerade fahren
+        if(ii > 899 && ii < 1800) {         // gerade fahren
             desiredSpeedLeft = v*2;
             desiredSpeedRight = -v*2;
         }
-        if(ii > 2899 && ii < 3900) {        // rechts drehen
+        if(ii > 1799 && ii < 2300) {        // rechts drehen
             desiredSpeedLeft = v*2;
             desiredSpeedRight = v*2;
         }
         ii++;
-        if(ii > 3899) {
+        if(ii > 2299) {                     // rücksetzen
             desiredSpeedLeft = 0;
             desiredSpeedRight = 0;
             r = 0;
@@ -117,20 +116,20 @@
             desiredSpeedLeft = -(v+30);
             desiredSpeedRight = v+30;
         }
-        if(ii > 499 && ii < 1100) {       // rechts drehen
+        if(ii > 499 && ii < 900) {       // rechts drehen
             desiredSpeedLeft = v*2;
             desiredSpeedRight = v*2;
         }
-        if(ii > 1099 && ii < 2900) {      // gerade fahren
+        if(ii > 899 && ii < 1800) {      // gerade fahren
             desiredSpeedLeft = v*2;
             desiredSpeedRight = -v*2;
         }
-        if(ii > 2899 && ii < 3900) {      // links drehen
+        if(ii > 1799 && ii < 2300) {      // links drehen
             desiredSpeedLeft = -v*2;
             desiredSpeedRight = -v*2;
         }
         ii++;
-        if(ii > 3899) {
+        if(ii > 2299) {
             desiredSpeedLeft = 0;
             desiredSpeedRight = 0;
             l = 0;
@@ -149,7 +148,7 @@
     desiredSpeedLeft = 0;
     desiredSpeedRight = 0;
 
-    if(ii < 650) {                // Arm nach unten fahren
+    if(ii < 650) {                   // Arm nach unten fahren
         servo2->SetPosition(2250);
     }
     if(ii > 649 && ii < 1100) {      // gerade fahren
@@ -177,7 +176,7 @@
     }
 
     ii++;
-    r = 5;                      // Rückegabewert
+    r = 5;                          // Rückegabewert
 
     if( ii > 2999 ) {
         r = 1;
@@ -223,7 +222,7 @@
 {
     int r;
     static int time2 = 0;
-    if(time2 < 10) {    // 1s gerade aus fahren
+    if(time2 < 25) {    // 1s gerade aus fahren
         desiredSpeedLeft = v*4;
         desiredSpeedRight = -v*4;
         time2 ++;
@@ -247,14 +246,14 @@
         desiredSpeedRight = v;
     }
 
-    if(ii > 999 && ii < 2000) {
+    if(ii > 999 && ii < 1800) {
         desiredSpeedLeft = v*2;
         desiredSpeedRight = v*2;
     }
 
     ii++;
 
-    if(ii > 1999) {
+    if(ii > 1799) {
         desiredSpeedLeft = 0;
         desiredSpeedRight = 0;
         ii = 0;
@@ -267,14 +266,23 @@
 int Roboter::kreisFahren()
 {
     int r;
+    static int anzahl = 0;
     static int time1 = 0;
-    if(time1 < 10) {       // 1s im Kreis drehen
-        desiredSpeedLeft = v*1.7f;
-        desiredSpeedRight = v*1.7f;
+
+    if(time1 < 6 && anzahl%2 == 0) {       // 1s im Kreis drehen
+        desiredSpeedLeft = v*2;
+        desiredSpeedRight = v*2;
         time1 ++;
         r = 1;          // state
     } else {
+        desiredSpeedLeft = -(v*2);
+        desiredSpeedRight = -(v*2);
+        time1 ++;
+        r = 1;          // state
+    }
+    if(time1 == 6) {
         time1 = 0;
+        anzahl ++;
         desiredSpeedLeft = 0.0f;
         desiredSpeedRight = 0.0f;
         r = 3;          // state
@@ -291,7 +299,7 @@
     if(cam == 400.0f && temp != 5) {     // Roboter in Position
         s = 5;
     }
-    if(cam == 400.0f && temp == 5) {
+    if(cam == 400.0f && temp == 5) {     // Roboter immer noch beim selben Klotz -> back()
         s = 7;
     }
     if(cam < 1) {