File content as of revision 89:d05001d85a02:

#include "mbed.h"

#define PLAN_A

#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);
    
#ifdef PLAN_B
    while(1)
    {
        test.setMaxTorque(0x1FF);
        test.setGoal(0);
        logger.printf("0\r\n");
        wait(2);
        test.setMaxTorque(0x1FF);
        test.setGoal(90);
        logger.printf("180\r\n");
        wait(4);
    }
#endif
    
    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)100.0,(float)-150.0,(float)PI/4);
        logger.printf("GOAL SET... RUNNING!\r\n");
        
        char state = 0;
        
        t.start();
        t.reset();
        
        while(!testOdo)
        {
            float dt = t.read();
            t.reset();
            
            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("phi r = %3.f ; phi_l = %3.f \r\n", phi_r,phi_l);
            
            motorR.setSpeed(/*0.08*/+(phi_r/phi_max));
            motorL.setSpeed(/*0.08*/+(phi_l/phi_max));
//#define test
#ifdef test            
            if(state == 0 && instanceAsserv.isArrivedRho())
            {
                state = 1;
                logger.printf("Arrive en 0,0 !!!!!");
                wait(5);
                instanceAsserv.setGoal(200.0,200.0,0);
            }
            else if(state == 1 && instanceAsserv.isArrivedRho())
            {
                state = 2;
                logger.printf("Arrive en 200,200 !!!!!");
                wait(5);
                instanceAsserv.setGoal(0.0,200.0,0);
            }
            else if(state == 2 && instanceAsserv.isArrivedRho())
            {
                state = 0;
                logger.printf("Arrive en 0,200 !!!!!");
                wait(5);
                instanceAsserv.setGoal(0.0,0.0,0);
            }
#endif
            
        }
    #else
        aserv_planB asserv(odometry,motorL,motorR);
        asserv.setGoal(1,-5000,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(100);            
        }
    #endif
}