File content as of revision 32:148147c0755e:

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

/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/
/*KalmanFilter*/
#include "EKF.h"
Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
Mat<double> jmotion_bicycle3_command(Mat<double> state, Mat<double> command, double dt = 0.5);
Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
void measurementCallback( Mat<double>* z, Odometry* odometry);
bool setPWM(PwmOut *servo,float p);

Mat<double> bicycle(3,1);
int reduc = 16;
/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------------------------*/
    /*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 = false;
    /*----------------------------------------------------------------------------------------------*/
    
    
    
    /*----------------------------------------------------------------------------------------------*/
    /*KalmanFilter*/
    //double phi_max = 100;
    /*en millimetres*/
    bicycle.set((double)36, 1,1);  /*radius*/
    bicycle.set((double)36, 2,1);
    bicycle.set((double)220, 3,1);   /*entre-roue*/
    
    int nbrstate = 5;
    int nbrcontrol = 2;
    int nbrobs = 5;
    double dt = (double)0.05;
    double stdnoise = (double)0.05;

    Mat<double> initX((double)0, nbrstate, 1);  
    initX.set( (double)0, 3,1);
    
    bool extended = true;
    bool filterOn = true;
    pcs.printf("mise a jour des pwm.\n");
    EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
    
    instance.initMotion(motion_bicycle3);
    instance.initSensor(sensor_bicycle3);
    instance.initJMotion(jmotion_bicycle3);
    //instance.initJMotionCommand(jmotion_bicycle3_command);
    instance.initJSensor(jsensor_bicycle3);
    
    /*desired State : (x y theta phiright phileft)*/
    Mat<double> dX((double)0, nbrstate, 1);
    dX.set( (double)50, 1,1);
    dX.set( (double)0, 2,1);
    dX.set( (double)0, 3,1);
    dX.set( (double)0, 4,1);
    dX.set( (double)0, 5,1);    
    
    Mat<double> ki((double)0, nbrcontrol, nbrstate);        
    Mat<double> kp((double)0, nbrcontrol, nbrstate);
    Mat<double> kd((double)0, nbrcontrol, nbrstate);
    //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
    
    for(int i=1;i<=nbrstate;i++)
    {
        kp.set( (double)0.01, i, i);
        kd.set( (double)0.0001, i, i);
        ki.set( (double)0.0001, i, i);
    }   
    
    instance.setKi(ki);
    instance.setKp(kp);
    instance.setKd(kd);
    //instance.setKdd(kdd);
    
    Mat<double> u(transpose( instance.getCommand()) );
    
    /*Observations*/
    /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
    Mat<double> z((double)0,5,1);
    measurementCallback(&z, &odometry);    
    
    /*----------------------------------------------------------------------------------------------*/
    
    
    while(abs(instance.getX().get(1,1)-50) > 10)
    {       
        t.start(); 
        //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                
                
        
        /*------------------------------------------------------------------------------------------*/
        /*Asservissement*/        
        
        measurementCallback(&z, &odometry);                        
        instance.measurement_Callback( z, dX, true );
         
        instance.state_Callback();
        //instance.setX(z);
        
        //instance.computeCommand(dX, (double)dt, -2);                    
        instance.computeCommand(dX, (double)t.read(), -2);                    
        double phi_r = instance.getCommand().get(1,1);
        double phi_l = instance.getCommand().get(2,1);
        
        double phi_max = 1.0;                    
                
        //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();
        //blue.printf(" x : %f ;\t y : %f ;\t theta : %f \n\r",z.get(1,1),z.get(2,1),z.get(3,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);
    }
}

void measurementCallback( Mat<double>* z, Odometry* odometry)
{
    z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
    z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
    double theta = odometry->getTheta();
    theta = atan21(sin(theta),cos(theta));
    z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
    double vx = (double)odometry->getVx();
    double vy = (double)odometry->getVy();
    z->set( sqrt(vx*vx+vy*vy),4,1);
    z->set( (double)odometry->getW(),5,1); 
    
    //z->afficherM();
}

Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)
{
    Mat<double> r(state);
    //double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
    //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
    double v = state.get(4,1);
    double w = state.get(5,1);
    
    r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
    r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
    
    double angle = (r.get(3,1) + dt*w);
    if( angle < -PI)
    {
        angle = angle - PI*ceil(angle/PI);
    }
    else if( angle > PI)
    {
        angle = angle - PI*floor(angle/PI);
    }
    
    r.set( atan21(sin(angle), cos(angle)), 3,1);
    
        
    /*----------------------------------------*/
    /*Modele du moteur*/
    /*----------------------------------------*/
    //double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
    //double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));   
    double r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
    double r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
    
    
    r.set( r1, 4,1);
    r.set( r2, 5,1);    
    
    
    /*----------------------------------------*/
    /*----------------------------------------*/    
    
    return r;
}


Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
{    
    /*
    double angle = state.get(3,1);
    if( angle < -PI)
    {
        angle = angle - PI*ceil(angle/PI);
    }
    else if( angle > PI)
    {
        angle = angle - PI*floor(angle/PI);
    }
    
    state.set( atan21(sin(angle), cos(angle)), 3,1);
    */
    return state;
}


Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
{
    double h = sqrt(numeric_limits<double>::epsilon())*norme2(state);
    Mat<double> var( (double)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<double> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
    
    for(int i=2;i<=state.getLine();i++)
    {
        var.set( (double)0, i-1,1);
        var.set( h, i,1);
        G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
    }       
    
    
    return (1.0/(2*h))*G;
}

Mat<double> jmotion_bicycle3_command(Mat<double> state, Mat<double> command, double dt)
{
    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
    Mat<double> var( (double)0, command.getLine(), command.getColumn());
    var.set( h, 1,1);
    Mat<double> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
    
    for(int i=2;i<=command.getLine();i++)
    {
        var.set( (double)0, i-1,1);
        var.set( h, i,1);
        G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );        
    }       
    
    
    return (1.0/(2*h))*G;
}

Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
{
    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
    Mat<double> var((double)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
    
    for(int i=2;i<=state.getLine();i++)
    {
        var.set( (double)0, i-1,1);
        var.set( h, i,1);
        H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
    }       
    
    
    return (1.0/(2*h))*H;
    /*
    double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
    Mat<double> var((double)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
        
    for(int i=2;i<=state.getLine();i++)
    {
        var.set( (double)0, i-1,1);
        var.set( h, i,1);
        H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
        
    }       
    
    
    return (1.0/(2*h))*H;
    */
}

bool setPWM(PwmOut *servo,float p)
{
    if(p <= 1.0f && p >= 0.0f)
    {
        servo->write(p);        
        return true;
    }
    
    return false;
}