Diff: main.cpp

Revision:

8:1150a13f6967

Parent:

4:4025c071b207

Child:

10:bca0274a007b

--- a/main.cpp	Fri Oct 03 13:43:08 2014 +0000
+++ b/main.cpp	Fri Oct 03 14:01:28 2014 +0000
@@ -33,7 +33,7 @@
     
     /*----------------------------------------------------------------------------------------------*/
     /*KalmanFilter*/
-    
+    double phi_max = 100;
     /*en millimetres*/
     bicycle.set((double)100, 1,1);  /*radius*/
     bicycle.set((double)100, 2,1);
@@ -43,7 +43,7 @@
     int nbrcontrol = 2;
     int nbrobs = 3;
     double dt = (double)0.05;
-    double stdnoise = (double)0.5;
+    double stdnoise = (double)0.05;
 
     Mat<double> initX((double)0, nbrstate, 1);  
     initX.set( (double)0, 3,1);
@@ -91,9 +91,16 @@
         measurementCallback(&z, &odometry);                
         instance.measurement_Callback( z, dX );
         instance.state_Callback();
+        double vd = instance.getCommand().get(1,1);
+        double wd = instance.getCommand().get(2,1);
+        double phi_r = bicycle.get(3,1)/bicycle.get(1,1)*(wd+vd/bicycle.get(3,1));
+        double phi_l = bicycle.get(3,1)/bicycle.get(2,1)*(wd+vd/bicycle.get(3,1));
+        
+        phi_r = (phi_r >= phi_max ? phi_r : phi_max);
+        phi_l = (phi_l >= phi_max ? phi_l : phi_max);
         
         instance.computeCommand(dX, (double)dt, -1);        
-        pc.printf("command : \n phi_r = %d \n phi_l = %d \n", instance.getCommand().get(1,1), instance.getCommand().get(2,1) );
+        pc.printf("command : \n phi_r = %d \n phi_l = %d \n", phi_r/phi_max*100, phi_l/phi_max*100);
          
         /*------------------------------------------------------------------------------------------*/