File content as of revision 35:54b13e154801:

#include "mbed.h"
#include "QEI.h"
#include "Map.h"

#include "Asserv.h"
/*----------------------------------------------------------------------------------------------*/
    /*Serial*/    
    Serial pcs(USBTX, USBRX); // tx, rx
/*----------------------------------------------------------------------------------------------*/

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

int main()
{
    pcs.printf("demarrage");
    //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(PA_0);
    DigitalOut dir1(PB_8);
    PwmOut pw2(PA_1);
    DigitalOut dir2(PB_9);
    pw1.period_us(10);
    pw2.period_us(10);
    
    
    dir1.write(0);
    dir2.write(0);    
    
    pw1.write(0.0);
    pw2.write(0.0);
    //setPWM(&pw1,0.9);
    pcs.printf("mise a jour des pwm.\n");
    Timer t;    
    
    /*----------------------------------------------------------------------------------------------*/
    /*Odometry*/
    QEI qei_left(D2,D3,NC,1024*4,QEI::X4_ENCODING);
    QEI qei_right(D4,D5,NC,1024*4,QEI::X4_ENCODING);
    Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
    bool testOdo = true;
    Asserv<float> instanceAsserv(&odometry);
    
    /*----------------------------------------------------------------------------------------------*/
    
    while(1)
    {       
        t.start(); 
        //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                                
        instanceAsserv.update();        
        //pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", ((double)phi_r/phi_max), ((double)phi_l/phi_max));
        //(instance.getCommand()).afficher();    
        //blue.printf("State : \n\r");
        //(instance.getX()).afficherMblue();
        //blue.printf("Odometry : \n\r");
        //z.afficherMblue();
        Mat<float> X( instanceAsserv.getX() );
        float phi_r = instanceAsserv.getPhiR();
        float phi_l = instanceAsserv.getPhiL();
        float phi_max = instanceAsserv.getPhiMax();
        blue.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t phiD = %f ;\t phiG = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
        //pcs.printf("Sigma : \n\r");
        //(instance.getSigma()).afficher();
        //pcs.printf("Kalman Gain : \n\r");
        //(instance.getKGain()).afficherM();
        
        
        if(phi_r >= 0)
            dir1.write(1);
        else
            dir1.write(0);
            
        if(phi_l <= 0)
            dir2.write(0);
        else
            dir2.write(1);
            
        if(!testOdo)
        {    
            if(abs(phi_r/phi_max) <= 1.0)
            {
                //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
                setPWM(&pw1, (float)abs((double)phi_r/phi_max));
            }
            else
            {
                pcs.printf("P1...");
                setPWM(&pw1, (float)abs((double)phi_max/phi_max));
            }
                
            if(abs(phi_l/phi_max) <= 1.0)
            {
                //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
                setPWM(&pw2,(float)abs((double)phi_l/phi_max));
            }
            else
            {
                pcs.printf("P2...");
                setPWM(&pw2, (float)abs((double)phi_max/phi_max));
            }
        }
            
        //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
        t.stop();
        pcs.printf("%f s \n\r", t.read());
        t.reset();
        t.start();
                
    }
        
    
    DigitalOut led(LED1);
    while(1)
    {
        //setPWM(&pw1,0.0);
        //setPWM(&pw2,0.0);
        pw1.write(0.0);
        pw2.write(0.0);
        led = !led;
        wait(1);
    }
}