File content as of revision 71:95d76c181b22:

#include "mbed.h"

#include "defines.h"

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

#define PLAN_B

#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");
    
    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.,280);
    #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.,280);
        
        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,(float)0, (float)PI/2);
        
        t.start();
        t.reset();
        
        while(!testOdo)
        {
            //logger.printf("%f %f\r\n",odometry.getVitLeft(),odometry.getVitRight());
            float dt = t.read();
            t.reset();
            odometry.update(dt);
            instanceAsserv.update(dt);        
            //Mat<float> X( instanceAsserv.getX() );        
            //float phi_r = (float)instanceAsserv.getPhiR();
            //float phi_l = (float)instanceAsserv.getPhiL();
            //float phi_max = (float)instanceAsserv.getPhiMax();        
            //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t VD = %f ;\t W = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
            //logger.printf(" x : %f ;\t y : %f ;\t theta : %f ",X.get(1,1),X.get(2,1),X.get(3,1) );
            
            //logger.printf("PhiR = %f ; \t PhiL = %f ; \n\r",phi_r,phi_l);//X.get(4,1),X.get(5,1));
            
            //transpose(X).afficherMblue();
            
            
            //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                                            
            //blue.printf("State : \n\r");
            //(instanceAsserv.getX()).afficherMblue();
            //blue.printf("Odometry : \n\r");
            //z.afficherMblue();  
            
            motorR.setSpeed(0.08+(phi_r/phi_max) );
            motorL.setSpeed(0.06+(phi_l/phi_max) );                   
        }
    #else
        aserv_planB asserv(odometry,motorL,motorR);
        asserv.setGoal(5000,-5000,0);
        
        t.start();
        t.reset();
        
        while(!testOdo)
        {
            //Parametrage des coef par bluetooth
            while(logger.readable()) {
                char c = logger.getc();
                if(c=='a') asserv.Kp += 0.001;
                else if(c=='z') asserv.Kp -= 0.001;
                //logger.printf("%f\n\r",asserv.Kp);
            }
            
            //Asservissement :
            float dt = t.read();
            t.reset();
            odometry.update(dt);
            asserv.update(dt);
            
            wait(0.1);             
        }
    #endif
    
    
    
    
    /*int i=0;
    int nbrech = 100;
    float tableR[nbrech], tableL[nbrech];
    
    motorR.setSpeed(0.0);
    motorL.setSpeed(0.0);
    while(i<nbrech)
    {        
        tableR[i] = odometry.getPhiright();
        tableL[i] = odometry.getPhileft();
        motorR.setSpeed( ((float)i)/100);
        motorL.setSpeed( ((float)i)/100);
        logger.printf("vitesse = %f \r\n", (float)i/100);
        wait(0.5);
        i++;            
    }
    
    i=0;
    while(1)
    {        
        motorR.setSpeed(0.0);
        motorL.setSpeed(0.0);
        
        if(i<nbrech)
        {
            logger.printf("%f , %f \r\n", tableL[i],tableR[i]);
            wait(1);
            i++;
        }
    }*/
}