Diff: main.cpp

Revision:

34:95b9e61c7dae

Parent:

32:148147c0755e

Child:

35:54b13e154801

--- a/main.cpp	Tue Feb 03 18:47:02 2015 +0000
+++ b/main.cpp	Sat Feb 07 07:26:52 2015 +0000
@@ -76,7 +76,7 @@
     QEI qei_left(D2,D3,NC,1024*4,QEI::X4_ENCODING);
     QEI qei_right(D4,D5,NC,1024*4,QEI::X4_ENCODING);
     Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
-    bool testOdo = false;
+    bool testOdo = true;
     /*----------------------------------------------------------------------------------------------*/
     
     
@@ -97,6 +97,7 @@
 
     Mat<double> initX((double)0, nbrstate, 1);  
     initX.set( (double)0, 3,1);
+    Mat<double> X(initX);
     
     bool extended = true;
     bool filterOn = true;
@@ -111,7 +112,7 @@
     
     /*desired State : (x y theta phiright phileft)*/
     Mat<double> dX((double)0, nbrstate, 1);
-    dX.set( (double)50, 1,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);
@@ -144,7 +145,7 @@
     /*----------------------------------------------------------------------------------------------*/
     
     
-    while(abs(instance.getX().get(1,1)-50) > 10)
+    while(1)
     {       
         t.start(); 
         //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                
@@ -158,6 +159,7 @@
          
         instance.state_Callback();
         //instance.setX(z);
+        X = instance.getX();
         
         //instance.computeCommand(dX, (double)dt, -2);                    
         instance.computeCommand(dX, (double)t.read(), -2);                    
@@ -168,15 +170,15 @@
                 
         //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();
-        //blue.printf(" x : %f ;\t y : %f ;\t theta : %f \n\r",z.get(1,1),z.get(2,1),z.get(3,1) );
+        //blue.printf("State : \n\r");
+        //(instance.getX()).afficherMblue();
+        //blue.printf("Odometry : \n\r");
+        //z.afficherMblue();
+        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();
-        pcs.printf("Kalman Gain : \n\r");
-        (instance.getKGain()).afficherM();
+        //pcs.printf("Kalman Gain : \n\r");
+        //(instance.getKGain()).afficherM();
         
         
         if(phi_r >= 0)