Diff: main.cpp

Revision:

7:64bfecbb3000

Parent:

6:5f7df5c74a77

Child:

8:72d684a69957

Child:

9:7f62c0329849

diff -r 5f7df5c74a77 -r 64bfecbb3000 main.cpp
--- a/main.cpp	Wed May 31 08:48:23 2017 +0000
+++ b/main.cpp	Wed May 31 12:17:59 2017 +0000
@@ -1,5 +1,7 @@
-#include "mbed.h"
+
 #include "fct.h"
+
+
 BusOut ledsetat(p12,p13);
 
 int main()
@@ -7,42 +9,57 @@
     int etat=0;
     init();
     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);
+        // 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);
+        //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;
-                    stopMotor();
+                    stop();
                 }
                 if(US2>100 && US1<40) {
                     etat=2;
-                    stopMotor();
+                    stop();
                 }
                 break;
             case 1 :
                 if(US2>100 && US1<40) {
                     etat=2;
-                    stopMotor();
+                    stop();
                 }
                 if(AN2>30) {
                     etat=0;
                     t2.start();
                     t2.reset();
-                    stopMotor();
+                    stop();
                 }
-
+                if(bearing>(bearing-90.0)) {
+                    etat=0;
+                    stop();
+                }
                 break;
             case 2 :
                 if((AN2>10 && AN2<20) || US3<13) {
                     etat=1;
-                    stopMotor();
+                    stop();
                 }
                 if(t2.read()>2.5) {
                     etat=0;
                 }
                 break;
+
         }
         switch(etat) {
             case 0 :