File content as of revision 63:fd9af0693e50:

#include "mbed.h"

#include "defines.h"

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


#include "Odometry.h"
//#include "Asserv.h"
#include "planB.h"
#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*/
    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);
    
    DigitalOut led1(LED1);
    DigitalOut led2(LED2);
    DigitalOut led3(LED3);
    
    t.start();
    t.reset();
    while(1)
    {
        odometry.update(t.read());
        t.reset();
        
        logger.printf("---------\r\n");
        logger.printf("%f %f %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta());
        logger.printf("%f %f %f\r\n",odometry.getVitLeft(),odometry.getVitRight(),t.read());
        t.reset();
        wait(0.5);
        motorL.setSpeed(1);
    }
    
    bool testOdo = false;
    
    
    //Asserv<float> instanceAsserv(&odometry);
    
    /*----------------------------------------------------------------------------------------------*/
    //instanceAsserv.setGoal( (float)0,(float)0, (float)PI/2);
    
    aserv_planB asserv(odometry,motorL,motorR);
    
    t.start();
    t.reset();
    
    while(!testOdo)
    {
        //Asservissement :
        
        
        
        asserv.update(t.read());
        t.reset();
          
        //instanceAsserv.update((float)t.read());        
        //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) );                   
    }
    
    
    /*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++;
        }
    }*/
}