File content as of revision 10:1a69a6f659bd:

#include "fct.h"


BusOut ledsetat(p12,p13);

int main()
{
    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("etat=%d set=%.1f CAP=%.1f jack=%d\n\r",etat,bearing_set,bearing,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();
        }
        
        switch(etat) {
            case 0 : if(jack.read()==1) etat=1; break;
            
            case 1 :          
                if((AN2>10 && AN2<20) || US3<13) {
                    etat=1;
                    stop();
                }
                if(US2>100 && US1<40) {
                    etat=2;
                    stop();
                }
                if(t3.read()>10) {
                    etat=3;
                    bearing_set=bearing_set+25.0;
                    t3.reset();
                    t3.stop();
                    stop();
                }
                break;
            case 2 :
                if(US2>100 && US1<40) {
                    etat=2;
                    stop();
                }
                if(AN2>30) {
                    etat=0;
                    t2.start();
                    t2.reset();
                    stop();
                }
                break;
            case 3 :
                if((AN2>10 && AN2<20) || US3<13) {
                    etat=1;
                    stop();
                }
                if(US1>100 && US2<10) {
                    etat=0;
                    stop();
                }
                break;
            case 4 :
                if(bearing>bearing_set) {
                    etat=4;
                    t3.reset();
                    stop();
                }
                break;
            case 5 :
                if((AN2>10 && AN2<22) || US3<22) {
                    etat=1;
                    stop();
                }
                break;

        }
        switch(etat) {
            case 0 : 
                stop();
                break;
            case 1 :
                suivi_mur();
                break;
            case 2 :
                rotation_horaire();
                break;
            case 3 :
                contournement();
                break;
            case 4 :
                rotation_horaire();
                break;
            case 5 :
                en_avant();
                break;
        }

    }
}