Diff: main3.cpp

Revision:

7:ca53c1bde37c

Parent:

6:9690e497319b

Child:

8:de6ca481b136

--- a/main3.cpp	Thu Feb 22 09:36:38 2018 +0000
+++ b/main3.cpp	Tue Mar 06 08:35:43 2018 +0000
@@ -20,7 +20,7 @@
 #define MAX     0.3
 #define MIN    -0.2
 
-#define seuil(x)  (x>300 ? 1 : 0)
+#define seuil(x)  (x>400 ? 1 : 0)
 
 float current_pos_of_line,
       previous_pos_of_line,
@@ -56,8 +56,8 @@
 }
 
 void step() {
-    m3pi.forward(0.4*speed);
-    wait(0.22);
+    m3pi.forward(0.12);
+    wait(0.18);
     // m3pi.stop();    
 }
 
@@ -77,35 +77,34 @@
                 // Deadend
                 // Back
                 m3pi.cls();
-                m3pi.print("Case 0", 7);
+                m3pi.print("Back", 5);
                 m3pi.stop();
                 result = 2;
                 break;
-            case 0x1C: case 0x18: case 0x1E:
+            case 0x18: case 0x1C: case 0x1E:
                 // Forward/Left or Left Only
-                // m3pi.stop();
                 step();                
                 current_state();
-                if ((sensors == 0x10) || (sensors == 0x18)) {
-                    // Turn Left
+                if ((sensors == 0x04) || (sensors == 0x06) || (sensors == 0x0C) || (sensors == 0x14) || (sensors == 0x16) || (sensors == 0x07)) {
+                    // Forward
                     m3pi.cls();
-                    m3pi.print("Left", 5);
-                    result = 3;
+                    m3pi.print("Forw2", 5);
+                    result = 1;
                 } else if (sensors == 0x1F) {
                     // End
                     m3pi.cls();
                     m3pi.print("The End2", 8);
                     result = 0;
-                } else if((sensors == 0x01) || (sensors == 0x03)) {
-                    // Turn right
+                } else if((sensors == 0x00) || (sensors == 0x10) || (sensors == 0x18)) {
+                    // Turn Left
                     m3pi.cls();
-                    m3pi.print("Right2", 7);
-                    result = 1;
+                    m3pi.print("Left", 5);
+                    result = 3;
                 } else {
-                    // Forward
+                    // Turn Right
                     m3pi.cls();
-                    m3pi.print("Fowar", 5);
-                    result = 1;
+                    m3pi.print("Right2", 6);
+                    result = 4;
                 }
                 break;
             case 0x07: case 0x03:
@@ -121,7 +120,6 @@
                 // m3pi.stop();
                 step();
                 current_state();
-                m3pi.print("End", 4);
                 if (sensors == 0x1F) {
                     // End
                     m3pi.cls();
@@ -183,13 +181,11 @@
         switch(command) {
             case 2:
                 // Turn Back
-                //m3pi.cls();
-                //m3pi.print("Back", 5);
                 if(sensors != 0x01) {
                     m3pi.right(speed);
                     result = 2;
                 } else {
-                    m3pi.right(0.4*speed);
+                    m3pi.right(0.12);
                     wait(0.12);
                     m3pi.stop();
                     result = 1;
@@ -197,13 +193,11 @@
                 break;
             case 3:
                 // Turn Left
-                //m3pi.cls();
-                //m3pi.print("Left", 5);
                 if(sensors != 0x10) {
                     m3pi.left(speed);
                     result = 3;
                 } else {
-                    m3pi.left(0.4*speed);
+                    m3pi.left(0.12);
                     wait(0.1);
                     m3pi.stop();
                     result = 1;
@@ -211,13 +205,11 @@
                 break;
             case 4:
                 // Turn Right
-                //m3pi.cls();
-                //m3pi.print("Right", 6);
                 if(sensors != 0x01) {
                     m3pi.right(speed);
                     result = 4;
                 } else {
-                    m3pi.right(0.4*speed);
+                    m3pi.right(0.12);
                     wait(0.1);
                     m3pi.stop();
                     result = 1;