Diff: main.cpp

Revision:

28:0a659a910771

Parent:

27:5f441ecda140

Child:

30:33e970ba1fe5

--- a/main.cpp	Tue Jan 20 14:42:00 2015 +0000
+++ b/main.cpp	Mon Jan 26 22:28:21 2015 +0000
@@ -73,8 +73,8 @@
     
     /*----------------------------------------------------------------------------------------------*/
     /*Odometry*/
-    QEI qei_left(p15,p16,NC,1024*reduc,QEI::X4_ENCODING);
-    QEI qei_right(p17,p18,NC,1024*reduc,QEI::X4_ENCODING);
+    QEI qei_left(D2,D3,NC,1024,QEI::X4_ENCODING);
+    QEI qei_right(D4,D5,NC,1024,QEI::X4_ENCODING);
     Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
     /*----------------------------------------------------------------------------------------------*/
     
@@ -136,9 +136,9 @@
     Mat<double> u(transpose( instance.getCommand()) );
     
     /*Observations*/
-    /*il nous faut 5 observation :*/
+    /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
     Mat<double> z((double)0,5,1);
-    //measurementCallback(&z, &odometry);    
+    measurementCallback(&z, &odometry);    
     
     /*----------------------------------------------------------------------------------------------*/
     
@@ -152,8 +152,8 @@
         /*------------------------------------------------------------------------------------------*/
         /*Asservissement*/        
         
-        //measurementCallback(&z, &odometry);                        
-        instance.measurement_Callback( instance.getX(), dX, true );
+        measurementCallback(&z, &odometry);                        
+        instance.measurement_Callback( z, dX, true );
          
         instance.state_Callback();
                 
@@ -164,8 +164,9 @@
         
         instance.computeCommand(dX, (double)dt, -2);            
         //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();    
-        //(instance.getX()).afficher();
+        (instance.getCommand()).afficher();    
+        (instance.getX()).afficher();
+        (instance.getSigma()).afficher();
         
         
         if(phi_r >= 0)
@@ -267,6 +268,17 @@
 
 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;
 }