File content as of revision 96:5a60caa2410a:

/*KalmanFilter*/
#include "EKF.h"
#include "Odometry.h"
//#define debugAsserv
#define verboseGOAL

/*
template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
template<typename T>
Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5 );
template<typename T>
Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt = 0.5);
template<typename T>
Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt = 0.5);
template<typename T>
Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5);
template<typename T>
bool setPWM(PwmOut *servo,T p);
*/

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

template<typename T>
Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt)
{    
    Mat<T> bicycle(3,1);
    bicycle.set((T)36, 1,1);  /*radius*/
    bicycle.set((T)36, 2,1);
    bicycle.set((T)220, 3,1);   /*entre-roue*/
    Mat<T> r(state);    
    
    /*state v w */
    /*
    T v = state.get(4,1);
    T w = state.get(5,1);
    */
    /*state phi_l phi_r */    
    T v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
    T w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.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);
    
    T angle = (r.get(3,1) + dt*w);
    
    while(angle > (T)PI)
    {
         angle -= 2.0f*(T)PI;                 
    }
    while(angle <= -(T)PI)
    {
        angle += 2.0f*(T)PI;                
    }
    
    /*
    if( angle < -(T)PI)
    {
        angle = angle - (T)PI*ceil(angle/PI);
    }
    else if( angle > (T)PI)
    {
        angle = angle - (T)PI*floor(angle/PI);
    }
    */
    
    r.set( 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));  
    /*state v w */
    /* 
    T r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
    T r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
    */
    /*state phi_l phi_r*/
    T r1 = command.get(1,1);
    T r2 = command.get(2,1);
    
    
    r.set( r1, 4,1);
    r.set( r2, 5,1);    
    
    
    /*----------------------------------------*/
    /*----------------------------------------*/    
    
    return r;
}

template<typename T>
Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T 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;
}

template<typename T>
Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt)
{    
    T h = pow(numeric_limits<T>::epsilon(),(T)0.5)*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
    Mat<T> var( (T)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<T> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
    
    for(int i=2;i<=state.getLine();i++)
    {
        var.set( (T)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 ((T)1.0/(2*h))*G;
}

template<typename T>
Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt)
{
    T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
    Mat<T> var( (T)0, command.getLine(), command.getColumn());
    var.set( h, 1,1);
    Mat<T> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
    
    for(int i=2;i<=command.getLine();i++)
    {
        var.set( (T)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;
}


template<typename T>
Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
{
    T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
    Mat<T> var((T)0, state.getLine(), state.getColumn());
    var.set( h, 1,1);
    Mat<T> 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( (T)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 ((T)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;
    */
}
//int reduc = 16;

extern Serial logger;

/*---------------------------------------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------------------------------------*/

template<typename T>
class Asserv
{
    private :  
    
    Odometry* odometry;      
    EKF<T>* instanceEKF;
    int nbrstate;
    int nbrcontrol;
    int nbrobs;
    T dt;
    
    T stdnoise;
    Mat<T> X;
    Mat<T> dX;
    
    Mat<T> dXalpha;
    Mat<T> dXrho;
    Mat<T> dXbeta;
    
    Mat<T> initX;
    Mat<T> z;
    Mat<T> u;
    bool extended;
    bool filterOn;
    
    Mat<T> ki;
    Mat<T> kp;
    Mat<T> kd;
    
    public :
        
    T phi_r;
    T phi_l;
    T phi_max;  
    bool execution;
    bool isarrived;
    bool isarrivedalpha;
    bool isarrivedrho;  
    int behaviour;
    
    Asserv(Odometry* odometry)
    {        
        /*Odometry*/
        this->odometry = odometry;
        phi_max = (T)100.0;                       
        
        /*KalmanFilter*/
        //double phi_max = 100;
        /*en millimetres*/        
        
        nbrstate = 5;
        nbrcontrol = 2;
        nbrobs = 5;
        dt = (T)0.05;
        stdnoise = (T)0.05;
    
        initX = Mat<T>((T)0, nbrstate, 1);  
        initX.set( (T)0, 3,1);
        X = initX;
        
        extended = true;
        filterOn = true;
        
        instanceEKF = new EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
        
        instanceEKF->initMotion(motion_bicycle3);
        instanceEKF->initSensor(sensor_bicycle3);
        instanceEKF->initJMotion(jmotion_bicycle3);
        //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
        instanceEKF->initJSensor(jsensor_bicycle3);
        
        /*desired State : (x y theta phiright phileft)*/
        dX = Mat<T>((T)0, nbrstate, 1);        
        
        dXalpha = dX;
        dXrho = dX;
        
        ki = Mat<T>((T)0, nbrcontrol, nbrstate);        
        kp = Mat<T>((T)0, nbrcontrol, nbrstate);
        kd = Mat<T>((T)0, nbrcontrol, nbrstate);
        //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
        
        for(int i=1;i<=nbrstate;i++)
        {
            kp.set( (T)0.01, i, i);
            kd.set( (T)0.0001, i, i);
            ki.set( (T)0.0001, i, i);
        }   
        
        instanceEKF->setKi(ki);
        instanceEKF->setKp(kp);
        instanceEKF->setKd(kd);
        //instance.setKdd(kdd);
        
        u = Mat<T>(transpose( instanceEKF->getCommand()) );
        
        /*Observations*/
        /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
        z = Mat<T>((T)0,5,1);
        this->measurementCallback(&z);    
        
        /*----------------------------------------------------------------------------------------------*/
        isarrived = true;
        isarrivedalpha = true;
        isarrivedrho = true;
        execution = false;
        behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
            
    }
    
    ~Asserv()
    {
        delete instanceEKF;
    }
    
    void setGoal(T x, T y, T theta)
    {
        instanceEKF->resetPID();
        dX.set(x,1,1);
        dX.set(y,2,1);
        dX.set(theta,3,1);
        dX.set((T)0,4,1);
        dX.set((T)0,5,1);
        execution = true;
        isarrived = false;
        isarrivedalpha = false;
        isarrivedrho = false;
        behaviour = 0;
        
        T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));                        
        alpha = alpha - X.get(3,1);//atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
        
        while(alpha > (T)PI)
        {
             alpha -= 2.0f*(T)PI;                 
        }
        while(alpha <= -(T)PI)
        {
            alpha += 2.0f*(T)PI;                
        }
        
        dXalpha.set( X.get(1,1), 1,1);
        dXalpha.set( X.get(2,1), 2,1);
        dXalpha.set( alpha, 3,1);
        dXalpha.set((T)0, 4,1);
        dXalpha.set((T)0, 5,1);
        
        dXrho = dX;
        dXrho.set( alpha, 3,1);
        
    }
    
    bool isArrived() { return isarrived;}  
    bool isArrivedRho() {   return isarrivedrho;}      
    
    void stop()
    {
        execution = false;
    }
    
    void update(T deltat)
    {
        if(execution)
        {            
            dt = deltat;    
                
            /*Asservissement*/                    
            this->measurementCallback(&z);                       
            //transpose(z).afficherMblue();
            
            
            /*Gestion des comportements : alpha --> rho --> beta --> isarrived = true;*/
            /*------------------------------------------------------------------------*/
                        
            Mat<T> dXbehaviour(dX);            
            if(isarrivedalpha && phi_l+phi_r <= phi_max/20 && behaviour == 0)
            {
                behaviour=1;
            }
            if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/20 && behaviour == 1)
            {
                behaviour = 2;
            }
            
            switch(behaviour)
            {
                case 0:
                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.1)
                {
                    behaviour = 1;
                    dXbehaviour = dXrho;
                    isarrivedalpha = true;                    
                }
                else
                {
                    dXbehaviour = dXalpha;
                    isarrivedalpha = false;
                }
                
                break;
                
                case 1:
                if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)10)
                {
                    behaviour = 2;
                    dXbehaviour = dX;
                    isarrivedrho = true;
                }
                else
                {
                    dXbehaviour = dXrho;
                    isarrivedrho = false;
                }
                break;
                
                default:
                if(norme2(dX-X) <=5)
                {
                    isarrived = true;
                }
                else
                {
                    isarrived = false;                
                }
                dXbehaviour = dX;
                break;
            }
            
#ifdef verboseGOAL
            logger.printf("GOAL : ");
            transpose(dXbehaviour).afficherMblue();
            logger.printf("\r\nPOSE : ");
            transpose(X).afficherMblue();
#endif            
            /*------------------------------------------------*/
            //if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
            //    isarrivedrho = true;
                
#ifdef debugAsserv
            logger.printf("BEHAVIOUR ASSERV : %d\r\n", behaviour);
#endif 
            instanceEKF->measurement_Callback( z, dXbehaviour, true );  
            
#ifdef debugAsserv            
            logger.printf("MEASUREMENT CALLBACK : DONE.\r\n");          
#endif            
            //instanceEKF->state_Callback();
            instanceEKF->setX(z);
            X = instanceEKF->getX();
            
#ifdef debugAsserv            
            logger.printf("STATE CALLBACK : DONE. \r\n");
            transpose(X).afficherMblue();
            transpose(z).afficherMblue();
#endif            
            
            //instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
            instanceEKF->computeCommand(dXbehaviour, (T)dt, 1);                    
#ifdef debugAsserv            
            logger.printf("COMMAND CALLBACK : DONE. \r\n");
#endif            
            //instanceEKF->computeCommand(dX, (T)dt, -2);                    
            phi_l = instanceEKF->getCommand().get(1,1);
            phi_r = instanceEKF->getCommand().get(2,1);                
        }
        else
        {
            phi_r = (T)0;
            phi_l = (T)0;
        }
    }
    
    void measurementCallbackVW( Mat<T>* z)
    {
        z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
        z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
        
        T theta = (T)this->odometry->getTheta();
        theta = atan21(sin(theta),cos(theta));
        
        z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
        
        T vx = (T)this->odometry->getVx();
        T vy = (T)this->odometry->getVy();
        z->set( sqrt(vx*vx+vy*vy),4,1);
        z->set( (T)odometry->getW(),5,1); 
        
        //transpose(*z).afficherMblue();
    }
    
    void measurementCallback( Mat<T>* z)
    {
        z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
        z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
        T theta = (T)this->odometry->getTheta();
        //theta = atan2(sin(theta),cos(theta));
        z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
        
        //State : phi_l phi_r
        z->set( (T)this->odometry->getPhileft(),4,1);
        z->set( (T)this->odometry->getPhiright(),5,1); 
        
        //transpose(*z).afficherMblue();
    }            
    
    Mat<T> getX()
    {
        return X;
    }
    
    T getPhiR()
    {
        return phi_r;
    }
    
    T getPhiL()
    {
        return phi_l;
    }
    
    T getPhiMax()
    {
        return phi_max;
    }
    
    
};