File content as of revision 95:33520ead3b94:

#include "mbed.h"

#define PLAN_A

//#define OUT_USB
#include "defines.h"

#include "QEI.h"
#include "Map.h"
#include "AX12.h"

#ifdef PLAN_A
    #include "Odometry.h"
    #include "Asserv.h"
#else
    #include "Odometry2.h"
    #include "planB.h"
#endif

#include "Motor.h"
/*----------------------------------------------------------------------------------------------*/
/*Serial*/    

//Serial logger(OUT_TX, OUT_RX); // tx, rx
Serial logger(USBTX,USBRX);
//logger.baud((int)115200);
/*----------------------------------------------------------------------------------------------*/

/* --- Initialisation de la liste des obstable --- */
int Obstacle::lastId = 0;

int main()
{
    logger.printf("Initialisation...\r\n");
    
    AX12 test(AX12_TX,AX12_RX,5,250000);
    test.setMode(0);
    
    PwmOut pw1(PWM_MOT1);
    DigitalOut dir1(DIR_MOT1);
    PwmOut pw2(PWM_MOT2);
    DigitalOut dir2(DIR_MOT2);
    
        
    Motor motorL(PWM_MOT1,DIR_MOT1);
    Motor motorR(PWM_MOT2,DIR_MOT2);    
    
    Timer t;
    
    //AX12 test(PA_9,NC,0x03,250000);
    
    AnalogIn ain0(PA_0);
    AnalogIn ain1(PA_1);
    AnalogIn ain2(PA_4);
    AnalogIn ain3(PB_0);
    AnalogIn ain4(PC_1);
    
    /*----------------------------------------------------------------------------------------------*/
    /*Odometry*/
    #ifdef PLAN_A
        QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
        QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
        
        Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
        odometry.setTheta(0);
        odometry.setX(0);
        odometry.setY(0);
    #else
        QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
        QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
        
        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
        
        odometry.setTheta(PI/2);
        odometry.setX(0);
        odometry.setY(0);
    #endif
    
    DigitalOut led1(LED1);
    DigitalOut led2(LED2);
    DigitalOut led3(LED3);
    
    bool testOdo = false;
    
    #ifdef PLAN_A
        Asserv<float> instanceAsserv(&odometry);
        //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
        instanceAsserv.setGoal((float)0.0f,(float)0.0f,(float)0.0f);
        logger.printf("GOAL SET... RUNNING!\r\n");
        
        char state = 0;
        
        t.start();
        t.reset();
        
        while(!testOdo)
        {
            float dt = t.read();
            t.reset();
            t.start();
            
            odometry.update(dt);
            instanceAsserv.update(dt);
            float phi_r = (float)instanceAsserv.getPhiR();
            float phi_l = (float)instanceAsserv.getPhiL();
            float phi_max = (float)instanceAsserv.getPhiMax();
                        
            logger.printf("STATE = %d \r\n", state);
            
            motorR.setSpeed( (phi_r > (float)0? (float)+0.08f : (float)-0.08f) +(phi_r/phi_max));
            motorL.setSpeed( (phi_l > (float)0? (float)+0.08f : (float)-0.08f) +(phi_l/phi_max));
#define test
#ifdef test            
            if(state == 0 && instanceAsserv.isArrivedRho())
            {
                state = 1;
                logger.printf("Arrive en 0,0 !!!!!\r\n");
                motorR.setSpeed( (float)0);
                motorL.setSpeed( (float)0);
                wait(5);
                instanceAsserv.setGoal((float)300.0f,(float)0.0f,(float)0.0f);
            }
            else if(state == 1 && instanceAsserv.isArrivedRho())
            {
                state = 2;
                logger.printf("Arrive en 200,0 !!!!!\r\n");
                motorR.setSpeed( (float)0);
                motorL.setSpeed( (float)0);
                wait(5);
                instanceAsserv.setGoal((float)200.0f,(float)200.0f,(float)0.0f);
            }
            else if(state == 2 && instanceAsserv.isArrivedRho())
            {
                state = 3;
                logger.printf("Arrive en 200,200 !!!!!\r\n");
                motorR.setSpeed( (float)0);
                motorL.setSpeed( (float)0);
                wait(5);
                instanceAsserv.setGoal((float)0.0f,(float)200.0f,(float)0.0f);
            }            
            else if(state == 3 && instanceAsserv.isArrivedRho())
            {
                state = 0;
                logger.printf("Arrive en 0,200 !!!!!\r\n");
                motorR.setSpeed( (float)0);
                motorL.setSpeed( (float)0);
                wait(5);
                instanceAsserv.setGoal((float)0.0f,(float)0.0f,(float)0.0f);
            }
#endif
            
        }
    #else
        aserv_planB asserv(odometry,motorL,motorR);
        asserv.setGoal(45,45,0);
        
        t.start();
        t.reset();
        
        while(!testOdo)
        {
            //Parametrage des coef par bluetooth
            /*while(logger.readable())
            {
                char c = logger.getc();
                if(c=='a') asserv.Kd += 0.001;
                else if(c=='z') asserv.Kd -= 0.001;
                logger.printf("%f\n\r",asserv.Kd);
            }*/
            
            //Asservissement :
            float dt = t.read();
            t.reset();
            odometry.update(dt);
            asserv.update(dt);
            wait_ms(10);            
        }
    #endif
}