Diff: main.cpp

Revision:

9:7f62c0329849

Parent:

7:64bfecbb3000

Child:

10:1a69a6f659bd

--- a/main.cpp	Wed May 31 12:17:59 2017 +0000
+++ b/main.cpp	Wed May 31 16:30:10 2017 +0000
@@ -8,23 +8,19 @@
 {
     int etat=0;
     init();
+    t3.reset();
+    t3.start();
     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("set=%.1f CAP=%.1f\n\r",bearing_set,bearing);
+        //printf("etat=%d set=%.1f CAP=%.1f\n\r",etat,bearing_set,bearing);
         //wait(0.1);
 
         bearing=boussole.readBearing()/10.0;
-        t3.reset();
-        t3.start();
+        
         ledsetat.write(etat);
         switch(etat) {
             case 0 :
-                if(t3.read()>4) {
-                    etat=1;
-                    t3.reset();
-                    stop();
-                }
                 if((AN2>10 && AN2<20) || US3<13) {
                     etat=1;
                     stop();
@@ -33,8 +29,16 @@
                     etat=2;
                     stop();
                 }
+                if(t3.read()>10) {
+                    etat=3;                                                                                             
+                    bearing_set=bearing_set+25.0;
+                    t3.reset();
+                    t3.stop();
+                    stop();
+                }
                 break;
             case 1 :
+
                 if(US2>100 && US1<40) {
                     etat=2;
                     stop();
@@ -45,10 +49,6 @@
                     t2.reset();
                     stop();
                 }
-                if(bearing>(bearing-90.0)) {
-                    etat=0;
-                    stop();
-                }
                 break;
             case 2 :
                 if((AN2>10 && AN2<20) || US3<13) {
@@ -59,6 +59,19 @@
                     etat=0;
                 }
                 break;
+            case 3 :
+                if(bearing>bearing_set) {
+                    etat=4;
+                    t3.reset();
+                    stop();
+                }
+                break;
+            case 4 :
+                if((AN2>10 && AN2<22) || US3<22) {
+                    etat=1;
+                    stop();
+                }
+                break;
 
         }
         switch(etat) {
@@ -71,6 +84,12 @@
             case 2 :
                 contournement();
                 break;
+            case 3 :
+                rotation_horaire();
+                break;
+            case 4 :
+                en_avant();
+                break;
         }
 
     }