Diff: main.cpp

Revision:

11:cb477f6ec781

Parent:

10:1a69a6f659bd

Child:

12:6151eb503170

--- a/main.cpp	Thu Jun 01 07:54:55 2017 +0000
+++ b/main.cpp	Thu Jun 01 08:18:04 2017 +0000
@@ -6,6 +6,8 @@
 
 int main()
 {
+
+    jack.mode(PullUp);
     int etat=0;
     init();
     t3.reset();
@@ -13,32 +15,34 @@
     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 jack=%d\n\r",etat,bearing_set,bearing,jack.read());
+        //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing);
         //wait(0.1);
-            
+
         bearing=boussole.readBearing()/10.0;
         ledsetat.write(etat);
-        
+
         if(bearing<bearing_set+10.0 && bearing>bearing_set-10.0) {
-            t3.start();   
-        }else {
+            t3.start();
+        } else {
             t3.stop();
         }
-        
+
         switch(etat) {
-            case 0 : if(jack.read()==1) etat=1; break;
-            
-            case 1 :          
+            case 0 :
+                if(jack.read()==0) etat=1;
+                break;
+            case 1 :     
+                if(jack.read()!=0) etat=0;
                 if((AN2>10 && AN2<20) || US3<13) {
-                    etat=1;
+                    etat=2;
                     stop();
                 }
                 if(US2>100 && US1<40) {
-                    etat=2;
+                    etat=3;
                     stop();
                 }
-                if(t3.read()>10) {
-                    etat=3;
+                if(t3.read()>t_set) {
+                    etat=4;
                     bearing_set=bearing_set+25.0;
                     t3.reset();
                     t3.stop();
@@ -46,44 +50,49 @@
                 }
                 break;
             case 2 :
+                if(jack.read()!=0) etat=0;
                 if(US2>100 && US1<40) {
-                    etat=2;
+                    etat=3;
                     stop();
                 }
                 if(AN2>30) {
-                    etat=0;
+                    etat=1;
                     t2.start();
                     t2.reset();
                     stop();
                 }
                 break;
             case 3 :
+                if(jack.read()!=0) etat=0;
                 if((AN2>10 && AN2<20) || US3<13) {
-                    etat=1;
+                    etat=2;
                     stop();
                 }
                 if(US1>100 && US2<10) {
-                    etat=0;
+                    etat=1;
                     stop();
                 }
                 break;
             case 4 :
+                if(jack.read()!=0) etat=0;
                 if(bearing>bearing_set) {
-                    etat=4;
+                    etat=5;
                     t3.reset();
                     stop();
                 }
                 break;
             case 5 :
+                if(jack.read()!=0) etat=0;
                 if((AN2>10 && AN2<22) || US3<22) {
-                    etat=1;
+                    etat=2;
+                    t_set=8;
                     stop();
                 }
                 break;
 
         }
         switch(etat) {
-            case 0 : 
+            case 0 :
                 stop();
                 break;
             case 1 :