File content as of revision 7:2f4660e9cf92:

#include "mbed.h"
#include "fct.h"
#include "Pixy.h"
#include "PixyLink.h"
#include "CMPS03.h"


Serial pc(USBTX,USBRX);
CMPS03 compass(p9, p10, CMPS03_DEFAULT_I2C_ADDRESS);

int main()
{

    init();
    Pixy pixy = Pixy(p28,p27);
    pixy.setSerialOutput(&pc);
    int tempo=0;
    int i=0;
    while(true) {
        //capg=capb1.read();
        //capd=capb2.read();
        capg=0;
        capd=0;

        //i++;pc.printf("%d\n\r",i);
        jck=jack.read();
        leds.write(etat);
        //pc.printf("g=%d,d=%d\n\r",capd,capg);
        //pc.printf("etat=%d,taille=%f,capt_eg=%3.0f,capt_g=%3.0f,capt_m=%3.0f,capt_d=%3.0f,capt_ed=%3.0f\n\r",etat,taille,capt_eg,capt_g,capt_m,capt_d,capt_ed);
        //pc.printf("etat=%d, cap_lum=%f, cap=%f, block=%d,taille=%f\n\r",etat,cap_lum,cap,blocks,taille);
        cap=compass.readBearing()/10.0;
        blocks = pixy.getBlocks();
        if (blocks) {
            taille=pixy.blocks[j].width*pixy.blocks[j].height;
            errorX = (160-pixy.blocks[j].x);
            errorY = (100-pixy.blocks[j].y);
        } /*else if(!blocks) {
            blocks = pixy.getBlocks();
            taille=pixy.blocks[j].width*pixy.blocks[j].height;
            errorX = (160-pixy.blocks[j].x);
            errorY = (100-pixy.blocks[j].y);
        }*/
        if(tr.read()>10) {
            tempo=1;
        }

        //pc.printf("taille=%f\n\r",taille);
        switch(etat) {
            case(0) :
                if(jck==1) {
                    etat=1;
                    cap_lum=cap;
                    tr.stop();
                    tr.reset();
                    tr.start();
                }
                break;

            case(1) :

                cap_lum=cap;
                if(!blocks) {
                    etat=3;
                }
                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
                    etat=2;
                }


                if((taille>seuil_taille)&&(tempo==1)) {
                    tb.reset();
                    tb.start();
                    etat=100;
                }
                break;

            case(2) :
                if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(blocks))  {
                    etat=1;
                }
                if((capt_eg>Dist_limit)&&(capt_g>Dist_limit)&&(capt_m>Dist_limit)&&(capt_d>Dist_limit)&&(capt_ed>Dist_limit)&&(!blocks))  {

                    etat=3;
                }

                if((taille>seuil_taille)&&(tempo==1)) {
                    tb.reset();
                    tb.start();
                    etat=100;
                }
                break;


            case(3) :
                if(cap_lum<180) {
                    if(cap<=cap_lum+180 && cap>cap_lum) {
                        etat=5;
                    } else {
                        etat=4;
                    }
                } else {
                    if(cap<=cap_lum && cap>cap_lum-180) {
                        etat=4;
                    }

                    else {
                        etat=5;
                    }
                }

                if((taille>seuil_taille)&&(tempo==1)) {
                    tb.reset();
                    tb.start();
                    etat=100;
                }
                break;

            case(4) : //pc.printf("tourner a gauche");

                if (blocks) {
                    etat=1;
                }
                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
                    etat=2;
                }

                if((taille>seuil_taille)&&(tempo==1)) {
                    tb.reset();
                    tb.start();
                    etat=100;
                }
                break;

            case(5) :

                //pc.printf("touner a droite");
                if (blocks) {
                    etat=1;
                }
                if((capt_eg<Dist_limit)||(capt_g<Dist_limit)||(capt_m<Dist_limit)||(capt_d<Dist_limit)||(capt_ed<Dist_limit)) {
                    etat=2;
                }

                if((taille>seuil_taille)&&(tempo==1)) {
                    tb.reset();
                    tb.start();
                    etat=100;
                }
                break;

            case(10) :

                if(capt_m>=40) {
                    etat=2;
                }
                break;


            case(100) :

                if((capt_m<30) || (capt_g<25)) {
                    etat=101;
                    t.stop();
                    t.reset();
                    t.start();
                }
                break;

            case(101) :
                if(t.read()<10) {
                    servo_start();
                } else if(t.read()>=10) {
                    servo_stop();
                }

                break;


        }
        switch(etat) {
            case(0) :
                vitesse(0,0);
                break;

            case(1) :
                vitesse(V_max-K*errorX,V_max+K*errorX);
                break;
            case(2) :
                erreur_cap=(cap_lum+(80-(capt_eg+capt_g+capt_m)/3)*Kdist-(80-(capt_ed+capt_d)/2)*Kdist)-cap;
                if(erreur_cap>180) {
                    erreur_cap=erreur_cap-360;
                }
                if(erreur_cap<-180) {
                    erreur_cap=erreur_cap+360;
                }
                if(erreur_cap>0) {
                    cmdD=V_max-Kcap*erreur_cap;
                    cmdG=V_max;
                } else {
                    cmdD=V_max;
                    cmdG=V_max+Kcap*erreur_cap;
                }
                if(cmdD<0) {
                    cmdD=0;
                }
                if(cmdG<0) {
                    cmdG=0;
                }
                vitesse(cmdG,cmdD);
                break;
            case(3) :
                break;

            case(4) :
                vitesse(V_moy,lent);
                break;

            case(5) :
                vitesse(lent,V_moy);
                break;

            case(10):
                vitesse(lent,lent);
                break;

            case(100) :

                vitesse(V_max-K*errorX,V_max+K*errorX);
                break;

            case(101) :

                vitesse(0,0);
                break;
            case(111) :
                vitesse(-30,0);
                break;

            case(112) :
                vitesse(0,-30);
                break;

        }
    }
}