Diff: main.cpp

Revision:

35:54b13e154801

Parent:

34:95b9e61c7dae

Child:

36:54f86bc6fd80

--- a/main.cpp	Sat Feb 07 07:26:52 2015 +0000
+++ b/main.cpp	Sun Mar 08 15:15:59 2015 +0000
@@ -1,25 +1,8 @@
 #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;
-/*---------------------------------------------------------------------------------------------------------*/
-/*---------------------------------------------------------------------------------------------------------*/
-
+#include "Asserv.h"
 /*----------------------------------------------------------------------------------------------*/
     /*Serial*/    
     Serial pcs(USBTX, USBRX); // tx, rx
@@ -77,103 +60,25 @@
     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);
     
     /*----------------------------------------------------------------------------------------------*/
-    /*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);
-    Mat<double> X(initX);
-    
-    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)300, 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(1)
     {       
         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);
-        X = instance.getX();
-        
-        //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("%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();
@@ -235,164 +140,4 @@
         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;
 }
\ No newline at end of file