Diff: main.cpp

Revision:

13:4fdd8ad91c69

Parent:

12:6151eb503170

--- a/main.cpp	Thu Jun 01 09:30:37 2017 +0000
+++ b/main.cpp	Mon Jan 22 19:32:00 2018 +0000
@@ -1,9 +1,5 @@
-
 #include "fct.h"
 
-
-BusOut ledsetat(p12,p13);
-
 int main()
 {
 
@@ -11,48 +7,48 @@
     int etat=0;
     init();
     t3.reset();
+    t3.stop();
     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 US1=%.0f US2=%.0f US3=%.0f erreur=%.0f AN1=%.0f AN2=%.0f cmdD=%.0f cmdG=%.0f jack=%d\n\r",etat,US1,US2,US3,(US2-US1),AN1,AN2,cmdD,cmdG,jack.read());
         //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.stop();
-        }
+        //printf("etat=%d set=%.1f CAP=%.1f t3=%f capt1=%d capt2=%d\n\r",etat,bearing_set,bearing,t3.read(),capt1.read(),capt2.read());
+        //wait(0.1);
+        capt=capt2.read();
 
         switch(etat) {
             case 0 :
-                if(jack.read()==0) {
+                if(jack.read()==1) {
                     etat=1;
                     t3.reset();
+                    t3.start();
+                    t5.reset();
+                    t5.start();
                 }
                 break;
             case 1 :
-                if(jack.read()!=0) etat=0;
-                if((AN2>40 && AN2<45) || US3<40) {
+
+                if(jack.read()!=1) etat=0;
+                if((AN2>10 && AN2<40) || US3<18) {
                     etat=2;
                     stop();
                 }
-                if(US2>100 && AN1<40) {
+                if(US2>100 && US1<40) {
                     etat=3;
                     stop();
                 }
-                if(t3.read()>8) {
+                /*if(t3.read()>7) {
                     etat=4;
-                    bearing_set=bearing_set+25.0;
-                    t3.reset();
-                    t3.stop();
+                    stop();
+                }*/
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
                     stop();
                 }
                 break;
             case 2 :
-                if(jack.read()!=0) etat=0;
+                if(jack.read()!=1) etat=0;
                 if(US2>100 && US1<40) {
                     etat=3;
                     stop();
@@ -63,33 +59,108 @@
                     t2.reset();
                     stop();
                 }
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
+                    stop();
+                }
                 break;
             case 3 :
-                if(jack.read()!=0) etat=0;
-                if((AN2>40 && AN2<45) || US3<40) {
+                if(jack.read()!=1) etat=0;
+                if((AN2>10 && AN2<30) || US3<18) {
                     etat=2;
                     stop();
                 }
-                if(t2.read()>2) {
+                if(t2.read()>5) {
                     etat=1;
                     stop();
                 }
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
+                    stop();
+                }
                 break;
             case 4 :
-                if(jack.read()!=0) etat=0;
-                if(bearing>bearing_set) {
-                    etat=5;
-                    t3.reset();
+                etat=5;
+                t3.reset();
+                stop();
+
+                break;
+            case 5 :
+                if(US3<40 || US2<50) {
+                    etat=6;
                     stop();
                 }
                 break;
-            case 5 :
-                if(jack.read()!=0) etat=0;
-                if((AN2>40 && AN2<42) || US3<40) {
-                    etat=2;
+            case 6 :
+                t3.start();
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
+                    stop();
+                }
+                if(jack.read()!=1) etat=0;
+                if((AN2>10 && AN2<30) || US3<18) {
+                    etat=7;
+                    stop();
+                }
+                if(US2>100 && US1<40) {
+                    etat=8;
+                    stop();
+                }
+                if(t3.read()>58) {
+                    etat=9;
+                    }
+                break;
+            case 7 :
+                if(jack.read()!=1) etat=0;
+                if(US2>100 && US1<40) {
+                    etat=8;
+                    stop();
+                }
+                if(AN2>30) {
+                    etat=6;
+                    t2.start();
+                    t2.reset();
+                    stop();
+                }
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
                     stop();
                 }
                 break;
+            case 8 :
+                if(jack.read()!=1) etat=0;
+                if((AN2>10 && AN2<30) || US3<18) {
+                    etat=7;
+                    stop();
+                }
+                if(t2.read()>4) {
+                    etat=6;
+                    stop();
+                }
+                if(capt1.read()==1 && capt2.read()==1 && t5.read()>5) {
+                    etat=9;
+                    t4.reset();
+                    t4.start();
+                    stop();
+                }
+                break;
+            case 9 :
+                if(t4.read()>2) etat=10;
+                stop();
+                break;
+            case 10 :
+                if(jack.read()!=1) etat=0;
+                stop();
+                break;
+
 
         }
         switch(etat) {
@@ -106,11 +177,31 @@
                 contournement();
                 break;
             case 4 :
-                rotation_sharp();
+                rotation_horaire();
+                wait_ms(700);
                 break;
             case 5 :
                 en_avant();
                 break;
+            case 6 :
+                suivi_mur_dist();
+                break;
+            case 7 :
+                rotation_sharp();
+                break;
+            case 8 :
+                contournement();
+                break;
+            case 9 :
+                ballon();
+                smoke.write(1);
+                break;
+            case 10 :
+                stop();
+                if(t4.read()>3) {
+                    smoke.write(0);
+                }
+                break;
         }
 
     }