Diff: Asservissement/Asserv.h

Revision:

36:54f86bc6fd80

Child:

41:c04c2ec37aad

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asserv.h	Sun Mar 08 21:24:27 2015 +0000
@@ -0,0 +1,365 @@
+/*KalmanFilter*/
+#include "EKF.h"
+#include "Odometry.h"
+
+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);
+int reduc = 16;
+
+
+/*---------------------------------------------------------------------------------------------------------*/
+/*---------------------------------------------------------------------------------------------------------*/
+
+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> 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;  
+    
+    Asserv(Odometry* odometry)
+    {        
+        /*Odometry*/
+        this->odometry = odometry;
+        phi_max = (T)1.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 = 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);
+        dX.set( (T)0, 1,1);
+        dX.set( (T)0, 2,1);
+        dX.set( (T)0, 3,1);
+        dX.set( (T)0, 4,1);
+        dX.set( (T)0, 5,1);    
+        
+        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;
+        execution = false;
+            
+    }
+    
+    ~Asserv()
+    {
+        
+    }
+    
+    void setGoal(T x, T y, T theta)
+    {
+        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;
+        
+    }
+    
+    bool isArrived() { return isarrived;}        
+    
+    void stop()
+    {
+        execution = false;
+    }
+    
+    void update(T deltat)
+    {
+        if(execution)
+        {            
+            dt = deltat;        
+            /*Asservissement*/        
+            
+            this->measurementCallback(&z);                        
+            instanceEKF.measurement_Callback( z, dX, true );
+             
+            instanceEKF.state_Callback();
+            //instance.setX(z);
+            X = instanceEKF.getX();
+            
+            //instance.computeCommand(dX, (double)dt, -2);                    
+            instanceEKF.computeCommand(dX, (T)dt, -2);                    
+            phi_r = instanceEKF.getCommand().get(1,1);
+            phi_l = instanceEKF.getCommand().get(2,1);                
+        }
+        else
+        {
+            phi_r = (T)0;
+            phi_l = (T)0;
+        }
+    }
+    
+    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 = 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); 
+        
+        //z->afficherM();
+    }            
+    
+    Mat<T> getX()
+    {
+        return X;
+    }
+    
+    T getPhiR()
+    {
+        return phi_r;
+    }
+    
+    T getPhiL()
+    {
+        return phi_l;
+    }
+    
+    T getPhiMax()
+    {
+        return phi_max;
+    }
+    
+    
+};    
+
+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);
+    //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));
+    T v = state.get(4,1);
+    T 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);
+    
+    T 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));   
+    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));
+    
+    
+    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 = sqrt(numeric_limits<T>::epsilon())*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;
+    */
+}
\ No newline at end of file