Diff: main.cpp

Revision:

3:b91371837109

Parent:

2:82b72fa8dbcd

Child:

4:78a9354fcee8

--- a/main.cpp	Sun May 28 17:36:57 2017 +0000
+++ b/main.cpp	Mon May 29 12:01:43 2017 +0000
@@ -1,26 +1,43 @@
 #include "mbed.h"
 #include "fct.h"
+BusOut ledsetat(p12,p13);
 
 int main()
 {
     int etat=0;
     init();
     while(1) {
-        //printf("etat=%d US1=%.0f US2=%.0f erreur=%.0f cmdD=%.0f cmdG=%.0f\n\r",etat,US1,US2,(US2-US1),cmdD,cmdG);
+        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);
+        ledsetat.write(etat);
         switch(etat) {
             case 0 :
-                if(AN2<20 || US3<13) {
+                if((AN2>10 && AN2<20) || US3<13) {
                     etat=1;
                     stopMotor();
                 }
+                if(US2>100 && US1<40) {
+                    etat=2;
+                    stopMotor();
+                }
                 break;
             case 1 :
                 if(AN2>30) {
                     etat=0;
+                    t2.start();
+                    t2.reset();
                     stopMotor();
                 }
                 break;
+            case 2 :
+                if((AN2>10 && AN2<20) || US3<13) {
+                    etat=1;
+                    stopMotor();
+                }
+                if(t2.read()>2) {
+                    etat=0;
+                }
+                break;
         }
         switch(etat) {
             case 0 :
@@ -29,6 +46,9 @@
             case 1 :
                 rotation_horaire();
                 break;
+            case 2 :
+                contournement();
+                break;
         }
 
     }