File content as of revision 50:16c033eea17d:

#include "mbed.h"

#include "defines.h"

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


#include "Odometry.h"
#include "Asserv.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");
    //mbed
    /*
    PwmOut pw1(p22);
    DigitalOut dir1(p21);
    PwmOut pw2(p24);
    DigitalOut dir2(p23);
    */
    
    //mbuino
    /*
    PwmOut pw1(P0_17);
    DigitalOut dir1(P0_18);
    PwmOut pw2(P0_23);
    DigitalOut dir2(P0_19);
    */
    /*
    //nucleo
    PwmOut pw1(PB_8);
    DigitalOut dir1(D12);
    PwmOut pw2(PB_9);
    DigitalOut dir2(D13);
    */
    //nucleo
    
    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);    
    logger.printf("mise a jour des pwm.\r\n");
    Timer t;
    
    AX12 test(PA_9,NC,0x05,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 led(LED1);
    
    test.SetMode(0); // Position
    
    /*char dataOff[] = {0};
    char dataOn[] = {1};
    while(1)
    {
        logger.printf("0: %f\r\n",ain0.read()*3.3);
        logger.printf("1: %f\r\n",ain1.read()*3.3);
        logger.printf("2: %f\r\n",ain2.read()*3.3);
        logger.printf("3: %f\r\n",ain3.read()*3.3);
        logger.printf("4: %f\r\n\r\n",ain4.read()*3.3);
        led = !led;
        wait(1);
        motorR.setSpeed(1.0);
        wait(1);
        motorR.setSpeed(0.0);
    }*/
    bool testOdo = false;
    
    
    Asserv<float> instanceAsserv(&odometry);
    
    /*----------------------------------------------------------------------------------------------*/
    instanceAsserv.setGoal( (float)10,(float)10, (float)PI/2);        
    
    while(!testOdo)
    {       
        //Asservissement :
        
        t.start();
        
        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) );     
        
        //Timer Handling :
        t.stop();        
        //logger.printf("%f s ecoulee.\n\r", t.read());
        t.reset();
        t.start();                
    }
        
    
    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++;
        }
    }
}