Diff: main3.cpp

Revision:

5:bc86a4fb4784

Parent:

4:cdd8e61cd8c7

Child:

6:9690e497319b

--- a/main3.cpp	Thu Feb 22 08:21:12 2018 +0000
+++ b/main3.cpp	Thu Feb 22 08:58:20 2018 +0000
@@ -49,6 +49,7 @@
 void current_state() {
     unsigned char i;
     sensors=0;
+    m3pi.calibrated_sensors(tabsensor);
     for(i=0; i<5; i++) {
         sensors = (sensors << 1) | seuil(tabsensor[i]);
     }
@@ -56,7 +57,7 @@
 
 void step() {
     m3pi.forward(0.4*speed);
-    wait(0.2);
+    wait(0.22);
     // m3pi.stop();    
 }
 
@@ -69,33 +70,42 @@
  */
 char PIDf(char commande) {
     if(commande==1) {
+        m3pi.cls();
+        m3pi.print("PID", 4);
+        m3pi.cls();
         char result;
-        m3pi.calibrated_sensors(tabsensor);
         current_state();
         switch(sensors) {
             case 0x00:
                 // Deadend
+                // Back
+                m3pi.cls();
+                m3pi.print("Case 0", 7);
                 m3pi.stop();
-                // Back
                 result = 2;
                 break;
             case 0x1C: case 0x18: case 0x1E:
                 // Forward/Left or Left Only
                 // m3pi.stop();
                 step();                
-                m3pi.calibrated_sensors(tabsensor);
                 current_state();
                 if ((sensors == 0x00) || (sensors == 0x10)) {
                     // Turn Left
+                    m3pi.cls();
+                    m3pi.print("Left", 5);
                     result = 3;
                 } else {
                     // Forward
+                    m3pi.cls();
+                    m3pi.print("Fowar", 5);
                     result = 1;
                 }
                 break;
             case 0x07: case 0x03: case 0x0F:
                 // Forward/Right or Right Only -> RF
                 // m3pi.stop();
+                m3pi.cls();
+                m3pi.print("Right", 6);
                 step();
                 result = 4;
                 break;
@@ -103,13 +113,17 @@
                 // 'T' or Intersection or End -> LR or RFL
                 // m3pi.stop();
                 step();
-                m3pi.calibrated_sensors(tabsensor);
                 current_state();
+                m3pi.print("End", 4);
                 if (sensors == 0x1F) {
                     // End
+                    m3pi.cls();
+                    m3pi.print("The End", 8);
                     result = 0;
                 } else {
                     // Turn Right
+                    m3pi.cls();
+                    m3pi.print("Inter", 6);
                     result = 4;
                 }
                 /*
@@ -170,11 +184,12 @@
 char turn(char command) {
     if(command > 1 && command < 5) {
         char result;
-        m3pi.calibrated_sensors(tabsensor);
         current_state();
         switch(command) {
             case 2:
                 // Turn Back
+                //m3pi.cls();
+                //m3pi.print("Back", 5);
                 if(sensors != 0x01) {
                     m3pi.right(speed);
                     result = 2;
@@ -187,6 +202,8 @@
                 break;
             case 3:
                 // Turn Left
+                //m3pi.cls();
+                //m3pi.print("Left", 5);
                 if(sensors != 0x10) {
                     m3pi.left(speed);
                     result = 3;
@@ -199,6 +216,8 @@
                 break;
             case 4:
                 // Turn Right
+                //m3pi.cls();
+                //m3pi.print("Right", 6);
                 if(sensors != 0x01) {
                     m3pi.right(speed);
                     result = 4;