Diff: main.cpp

Revision:

10:1a69a6f659bd

Parent:

9:7f62c0329849

Child:

11:cb477f6ec781

diff -r 7f62c0329849 -r 1a69a6f659bd main.cpp
--- a/main.cpp	Wed May 31 16:30:10 2017 +0000
+++ b/main.cpp	Thu Jun 01 07:54:55 2017 +0000
@@ -13,14 +13,22 @@
     while(1) {
         // printf("etat=%d US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG);
         //wait(0.05);
-        //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing);
+        //printf("etat=%d set=%.1f CAP=%.1f jack=%d\n\r",etat,bearing_set,bearing,jack.read());
         //wait(0.1);
-
+            
         bearing=boussole.readBearing()/10.0;
+        ledsetat.write(etat);
         
-        ledsetat.write(etat);
+        if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) {
+            t3.start();   
+        }else {
+            t3.stop();
+        }
+        
         switch(etat) {
-            case 0 :
+            case 0 : if(jack.read()==1) etat=1; break;
+            
+            case 1 :          
                 if((AN2>10 && AN2<20) || US3<13) {
                     etat=1;
                     stop();
@@ -30,15 +38,14 @@
                     stop();
                 }
                 if(t3.read()>10) {
-                    etat=3;                                                                                             
+                    etat=3;
                     bearing_set=bearing_set+25.0;
                     t3.reset();
                     t3.stop();
                     stop();
                 }
                 break;
-            case 1 :
-
+            case 2 :
                 if(US2>100 && US1<40) {
                     etat=2;
                     stop();
@@ -50,23 +57,24 @@
                     stop();
                 }
                 break;
-            case 2 :
+            case 3 :
                 if((AN2>10 && AN2<20) || US3<13) {
                     etat=1;
                     stop();
                 }
-                if(t2.read()>2.5) {
+                if(US1>100 && US2<10) {
                     etat=0;
+                    stop();
                 }
                 break;
-            case 3 :
+            case 4 :
                 if(bearing>bearing_set) {
                     etat=4;
                     t3.reset();
                     stop();
                 }
                 break;
-            case 4 :
+            case 5 :
                 if((AN2>10 && AN2<22) || US3<22) {
                     etat=1;
                     stop();
@@ -75,19 +83,22 @@
 
         }
         switch(etat) {
-            case 0 :
-                suivi_mur();
+            case 0 : 
+                stop();
                 break;
             case 1 :
-                rotation_horaire();
+                suivi_mur();
                 break;
             case 2 :
-                contournement();
+                rotation_horaire();
                 break;
             case 3 :
+                contournement();
+                break;
+            case 4 :
                 rotation_horaire();
                 break;
-            case 4 :
+            case 5 :
                 en_avant();
                 break;
         }