Diff: main.cpp

Revision:

5:24350e6fc9d7

Parent:

4:2ebbd24aa610

--- a/main.cpp	Sun May 21 11:28:15 2017 +0000
+++ b/main.cpp	Sun May 21 15:29:46 2017 +0000
@@ -176,8 +176,8 @@
         counterRight.reset();
     }
     if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
-        desiredSpeedLeft = 1.5 + 60;
-        desiredSpeedRight = 1.5 - 60;
+        desiredSpeedLeft = 1.5 + 90;
+        desiredSpeedRight = 1.5 - 90;
     } else {
         desiredSpeedLeft = 0.0;
         desiredSpeedRight = 0.0;
@@ -303,14 +303,14 @@
         wait(1.0f);
         desiredSpeedLeft = 0.0f;
         desiredSpeedRight = 00.0f;
-        for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
+        for (int pos = 2300; pos > 440; pos -= 30) {      //Arm heben
             Arm.SetPosition(pos);
             wait_ms(5);
         }
-        wait(0.05f);
+        wait(0.1f);
         for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
             Greifer.SetPosition(pos);
-            wait_ms(2);
+            wait_ms(3);
         }
     } else if(Red>Green) {
         for (int pos = 1300; pos < 2250; pos += 25) {    //Deckel schliessen
@@ -324,18 +324,17 @@
         desiredSpeedRight = 00.0f;
         for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
             Arm.SetPosition(pos);
-            wait_ms(7);
+            wait_ms(9);
         }
-        wait_ms(50);
         for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
             Greifer.SetPosition(pos);
-            wait_ms(2);
+            wait_ms(4);
         }
         for (int pos = 440; pos < 700; pos += 25) {      //Arm Runter
             Arm.SetPosition(pos);
             wait_ms(5);
         }
-        for (int pos = 2250; pos > 1300; pos -= 25) {     //Deckel öffnen
+        for (int pos = 2250; pos > 1300; pos -= 30) {     //Deckel öffnen
             Deckel.SetPosition(pos);
             wait_ms(5);
         }
@@ -344,16 +343,20 @@
             wait_ms(5);
         }
     } else {
-        desiredSpeedLeft = -35.0f;
+        for (int pos = 2300; pos > 1600; pos -= 25) {      //Arm heben um alfällig eingeklemmte Klötze von den Sensoren zu entfernen
+            Arm.SetPosition(pos);
+            wait_ms(5);
+        }
+        desiredSpeedLeft = -40.0f;
         desiredSpeedRight = 30.0f;
         wait(1.0f);
         desiredSpeedLeft = 0.0f;
-        desiredSpeedRight = 00.0f;
+        desiredSpeedRight = 0.0f;
         for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
             Greifer.SetPosition(pos);
             wait_ms(2);
         }
-        for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
+        for (int pos = 1600; pos > 440; pos -= 25) {      //Arm heben
             Arm.SetPosition(pos);
             wait_ms(5);
         }
@@ -364,8 +367,8 @@
 //--------------------Status&Main-----------------------------------------------
 
 enum RobotState {
-    START = 0,
-    FAHREN,
+    // START = 0,
+    FAHREN = 0,
     AUSWEICHEN,
     VORSUCHEN,
     SUCHEN,
@@ -423,7 +426,7 @@
     }
     enableSensor = 1;   //Aktiviert die IRSensoren
 
-    status = START;
+    status = FAHREN;
 
     int timer = 0;
     led = 0;
@@ -436,13 +439,13 @@
 
         switch(status) {
 
-            case START: //Start Sequenz
-                status = FAHREN;
-                break;
-
+                /*          case START: //Start Sequenz
+                              status = FAHREN;
+                              break;
+                */
             case FAHREN:
                 status = AUSWEICHEN;
-                if(geradeaus_schnell(80)) {
+                if(geradeaus_schnell(100)) {
                     status = VORSUCHEN;
                 }
                 break;
@@ -465,20 +468,22 @@
 
             case SUCHEN:
                 //for(int i = 0; i < 10; i++)
-                desiredSpeedLeft = 15.0f; //Drehung
-                desiredSpeedRight = 15.0f;
+                desiredSpeedLeft = 20.0f; //Drehung
+                desiredSpeedRight = 20.0f;
 
                 if(z < 500) {
-                    if(sensorZentimeter[0] > 60) sensorOben[z] = 60;
+                    if(sensorZentimeter[0] > 60) sensorOben[z] = 60; //ACHTUUUUNG Z <500 und sensorOben = 60
                     else sensorOben[z] = sensorZentimeter[0];
                     sensorLinks[z] = sensorZentimeter[5];
                     //Prüft Ausschlag && vergleicht ersten mit letztem Wert && prüft Zwischenwerte
                     if(3.0f * sensorLinks[z] > sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] + 13.5f && sensorLinks[z] + 1 > sensorLinks[z-12] && sensorLinks[z] >= sensorLinks[z-2] && sensorLinks[z] >= sensorLinks[z-3] && sensorLinks[z] >= sensorLinks[z-4] && sensorLinks[z-12] >= sensorLinks[z-10] && sensorLinks[z-12] >= sensorLinks[z-9] && sensorLinks[z-12] >= sensorLinks[z-8]) {
-                        //Prüft Hindernis
-                        if(sensorLinks[z-5] + 3.5f < sensorOben[z-5] && sensorLinks[z-6] + 3.5f < sensorOben[z-6] && sensorLinks[z-7] + 3.5f < sensorOben[z-7]) {
-                            status = DREHUNGBACK;
-                            z--;
-                            printf("\n\r");
+                //        if(sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] > 3 * 33.5f) {
+                            //Prüft Hindernis
+                            if(sensorLinks[z-5] + 3.5f < sensorOben[z-5] && sensorLinks[z-6] + 3.5f < sensorOben[z-6] && sensorLinks[z-7] + 3.5f < sensorOben[z-7]) {
+                                status = DREHUNGBACK;
+                                z--;
+                                printf("\n\r");
+                  //          }
                         }
                     }
                     z++;
@@ -490,8 +495,8 @@
 
             case DREHUNGBACK:
                 if(y < 5) {
-                    desiredSpeedLeft = -30.0f;  //Drehung rückwärts
-                    desiredSpeedRight = -30.0f;
+                    desiredSpeedLeft = -25.0f;  //Drehung rückwärts
+                    desiredSpeedRight = -25.0f;
                     y++;
                     /*                } else if {y < 30){
                                         desiredSpeedLeft = 0.0f;    //Stillstand für genauen Sensorwert