Diff: Asservissement/Asserv.h

Revision:

47:4909e97570f6

Parent:

44:d5f95af61243

Child:

50:16c033eea17d

--- a/Asservissement/Asserv.h	Tue Mar 24 09:17:11 2015 +0000
+++ b/Asservissement/Asserv.h	Thu Mar 26 18:04:23 2015 +0000
@@ -35,11 +35,16 @@
     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));
+    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);
@@ -61,9 +66,15 @@
     /*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 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);
@@ -306,16 +317,18 @@
     {
         if(execution)
         {            
-            dt = deltat;        
+            dt = deltat;    
+                
             /*Asservissement*/                    
-            this->measurementCallback(&z);                                 
+            this->measurementCallback(&z);                       
+            transpose(z).afficherMblue();          
             instanceEKF->measurement_Callback( z, dX, true );            
             instanceEKF->state_Callback();
-            //instance.setX(z);
+            //instanceEKF->setX(z);
             X = instanceEKF->getX();            
             
-            //instance.computeCommand(dX, (double)dt, -2);                    
-            instanceEKF->computeCommand(dX, (T)dt, -2);                    
+            instanceEKF->computeCommand(dX, (T)dt, -1);                    
+            //instanceEKF->computeCommand(dX, (T)dt, -2);                    
             phi_l = instanceEKF->getCommand().get(1,1);
             phi_r = instanceEKF->getCommand().get(2,1);                
         }
@@ -326,7 +339,7 @@
         }
     }
     
-    void measurementCallback( Mat<T>* z)
+    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);
@@ -339,6 +352,21 @@
         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 = atan21(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()