Robot's source code

Dependencies:   mbed

Revision:
17:f360e21d3307
Parent:
16:6bd245b26423
Child:
20:2840a749fb55
--- a/main.cpp	Fri Oct 03 14:53:35 2014 +0000
+++ b/main.cpp	Fri Dec 12 12:44:59 2014 +0000
@@ -1,30 +1,72 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "Odometry.h"
-
+#include <iostream>
 
 /*---------------------------------------------------------------------------------------------------------*/
 /*---------------------------------------------------------------------------------------------------------*/
 /*KalmanFilter*/
 #include "EKF.h"
-Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
-Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
-Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
-Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
+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> 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
+/*----------------------------------------------------------------------------------------------*/
 
 int main()
 {
+
+    
+    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);
+    */
+    pw1.period_us(10);
+    pw2.period_us(10);
+    
+    
+    dir1.write(0);
+    dir2.write(0);    
+    pw1.write(1.0);
+    pw2.write(0.8);
+    //setPWM(&pw1,0.9);
+    pcs.printf("mise à jour des pwm.\n");
+    //while(1);
     /*----------------------------------------------------------------------------------------------*/
     /*Odometry*/
-    QEI qei_left(p15,p16,NC,1024,QEI::X4_ENCODING);
-    QEI qei_right(p17,p18,NC,1024,QEI::X4_ENCODING);
+    QEI qei_left(p15,p16,NC,1024*reduc,QEI::X4_ENCODING);
+    //QEI qei_left(P0_2,P0_7,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
+    //QEI qei_left(PA_3,PA_2,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
+    
+    QEI qei_right(p17,p18,NC,1024*reduc,QEI::X4_ENCODING);
+    //QEI qei_right(P0_8,P0_20,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
+    //QEI qei_right(PA_10,PB_3,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
     
     Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
     /*----------------------------------------------------------------------------------------------*/
@@ -41,7 +83,7 @@
     
     int nbrstate = 5;
     int nbrcontrol = 2;
-    int nbrobs = 3;
+    int nbrobs = 5;
     double dt = (double)0.05;
     double stdnoise = (double)0.05;
 
@@ -49,57 +91,94 @@
     initX.set( (double)0, 3,1);
     
     bool extended = true;
-    EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended);
+    bool filterOn = false;
+    EKF<double> instance(&pcs, nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
     
-    instance.initMotion(motion_bicycle2);
-    instance.initSensor(sensor_bicycle2);
-    instance.initJMotion(jmotion_bicycle2);
-    instance.initJSensor(jsensor_bicycle2);
+    instance.initMotion(motion_bicycle3);
+    instance.initSensor(sensor_bicycle3);
+    instance.initJMotion(jmotion_bicycle3);
+    instance.initJSensor(jsensor_bicycle3);
     
     /*desired State : (x y theta phiright phileft)*/
     Mat<double> dX((double)0, nbrstate, 1);
-    dX.set( (double)0, 1,1);
-    dX.set( (double)100, 2,1);
+    dX.set( (double)100, 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*/
-    Mat<double> z(3,1);
+    /*il nous faut 5 observation :*/
+    Mat<double> z((double)0,5,1);
     measurementCallback(&z, &odometry);    
     
     /*----------------------------------------------------------------------------------------------*/
     
     
-    
-    
-    /*----------------------------------------------------------------------------------------------*/
-    /*Serial*/    
-    Serial pc(USBTX, USBRX); // tx, rx
-    /*----------------------------------------------------------------------------------------------*/
-    
     while(1)
     {
-        wait(1);
-        pc.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
-        
+        //wait(1);
+        pcs.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);                
+                
         
         /*------------------------------------------------------------------------------------------*/
-        /*Asservissement*/
-        measurementCallback(&z, &odometry);                
-        instance.measurement_Callback( z, dX );
+        /*Asservissement*/        
+        
+        //measurementCallback(&z, &odometry);                        
+        instance.measurement_Callback( instance.getX(), dX, true );
+         
         instance.state_Callback();
+                
         double phi_r = instance.getCommand().get(1,1);
         double phi_l = instance.getCommand().get(2,1);
         
+        double phi_max = 100;                    
         
-        instance.computeCommand(dX, (double)dt, -1);        
-        pc.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
-        instance.getX().afficher();
-         
-        /*------------------------------------------------------------------------------------------*/
+        instance.computeCommand(dX, (double)dt, -2);        
+        pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
+        //instance.getX().afficher();
+        
+        
+        if(phi_r <= 0)
+            dir1.write(0);
+        else
+            dir1.write(1);
+            
+        if(phi_l <= 0)
+            dir2.write(0);
+        else
+            dir2.write(1);
+            
+        if(abs(phi_r/phi_max) < 1.0)
+            setPWM(&pw1, (float)abs(phi_r/phi_max));
+        else
+            cout << "P1..." << endl;
+            
+        if(abs(phi_l/phi_max) < 1.0)
+            setPWM(&pw2,(float)abs(phi_l/phi_max));
+        else
+            pcs.printf("P2...");
+            
+        pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n\n");
         
     }
 }
@@ -111,16 +190,16 @@
     z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);    
 }
 
-Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+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 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);
     
-    double angle = (r.get(3,1) + dt/bicycle.get(3,1)*(r.get(4,1)-r.get(5,1)));
+    double angle = (r.get(3,1) + dt*w);
     if( angle < -PI)
     {
         angle = angle - PI*ceil(angle/PI);
@@ -130,52 +209,79 @@
         angle = angle - PI*floor(angle/PI);
     }
     
-    r.set( angle, 3,1);
+    r.set( atan21(sin(angle), cos(angle)), 3,1);
     
-    r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1)), 4,1);
-    r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1)), 5,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));
+    
+    
+    r.set( r1, 4,1);
+    r.set( r2, 5,1);    
+    
+    
+    /*----------------------------------------*/
+    /*----------------------------------------*/    
     
     return r;
 }
 
 
-Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
-{
-    return extract(state-d_state, 1,1, 3,1);
+Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+{    
+    return state;
 }
 
-Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt)
+
+Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
 {
     double h = numeric_limits<double>::epsilon()*10e2;
     Mat<double> var( (double)0, state.getLine(), state.getColumn());
     var.set( h, 1,1);
-    Mat<double> G(motion_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt));
+    Mat<double> G(motion_bicycle3(state, 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_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt) );
+        G = operatorL(G, motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt) );
     }       
     
     
     return (1.0/h)*G;
 }
 
-Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
+Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
 {
     double h = numeric_limits<double>::epsilon()*10e2;
     Mat<double> var((double)0, state.getLine(), state.getColumn());
     var.set( h, 1,1);
-    Mat<double> H(sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt));
+    Mat<double> H(sensor_bicycle3(state, 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_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt) );
+        Mat<double> temp(sensor_bicycle3(state, command, d_state, dt) - sensor_bicycle3(state+var, command, d_state, dt));
+        
+        H = operatorL(H, temp );        
+        pcs.printf("sensor bicycle  %d...\n",i);
     }       
     
     
     return (1.0/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