Diff: main3.cpp

Revision:

6:9690e497319b

Parent:

5:bc86a4fb4784

Child:

7:ca53c1bde37c

--- a/main3.cpp	Thu Feb 22 08:58:20 2018 +0000
+++ b/main3.cpp	Thu Feb 22 09:36:38 2018 +0000
@@ -20,7 +20,7 @@
 #define MAX     0.3
 #define MIN    -0.2
 
-#define seuil(x)  (x>400 ? 1 : 0)
+#define seuil(x)  (x>300 ? 1 : 0)
 
 float current_pos_of_line,
       previous_pos_of_line,
@@ -70,9 +70,6 @@
  */
 char PIDf(char commande) {
     if(commande==1) {
-        m3pi.cls();
-        m3pi.print("PID", 4);
-        m3pi.cls();
         char result;
         current_state();
         switch(sensors) {
@@ -89,11 +86,21 @@
                 // m3pi.stop();
                 step();                
                 current_state();
-                if ((sensors == 0x00) || (sensors == 0x10)) {
+                if ((sensors == 0x10) || (sensors == 0x18)) {
                     // Turn Left
                     m3pi.cls();
                     m3pi.print("Left", 5);
                     result = 3;
+                } else if (sensors == 0x1F) {
+                    // End
+                    m3pi.cls();
+                    m3pi.print("The End2", 8);
+                    result = 0;
+                } else if((sensors == 0x01) || (sensors == 0x03)) {
+                    // Turn right
+                    m3pi.cls();
+                    m3pi.print("Right2", 7);
+                    result = 1;
                 } else {
                     // Forward
                     m3pi.cls();
@@ -101,7 +108,7 @@
                     result = 1;
                 }
                 break;
-            case 0x07: case 0x03: case 0x0F:
+            case 0x07: case 0x03:
                 // Forward/Right or Right Only -> RF
                 // m3pi.stop();
                 m3pi.cls();
@@ -109,7 +116,7 @@
                 step();
                 result = 4;
                 break;
-            case 0x1F:
+            case 0x1F: case 0x0F:
                 // 'T' or Intersection or End -> LR or RFL
                 // m3pi.stop();
                 step();
@@ -126,18 +133,6 @@
                     m3pi.print("Inter", 6);
                     result = 4;
                 }
-                /*
-                if ((sensors == 0x00) || (sensors == 0x01) || (sensors == 0x10) || (sensors == 0x11)) {
-                    // Turn Right
-                    result = 4;
-                } else if (sensors == 0x1F) {
-                    // End
-                    result = 0;
-                } else {
-                    // Turn Right
-                    result = 4;
-                }
-                */
                 break;
             case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
                 //PID