Diff: main.cpp

Revision:

31:e60cd1c984f4

Parent:

30:33e970ba1fe5

Child:

32:148147c0755e

diff -r 33e970ba1fe5 -r e60cd1c984f4 main.cpp
--- a/main.cpp	Sun Feb 01 19:29:14 2015 +0000
+++ b/main.cpp	Tue Feb 03 18:44:33 2015 +0000
@@ -75,7 +75,8 @@
     /*Odometry*/
     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);
+    Odometry odometry(&qei_left,&qei_right,63,63,280);
+    bool testOdo = false;
     /*----------------------------------------------------------------------------------------------*/
     
     
@@ -110,9 +111,9 @@
     
     /*desired State : (x y theta phiright phileft)*/
     Mat<double> dX((double)0, nbrstate, 1);
-    dX.set( (double)10, 1,1);
+    dX.set( (double)50, 1,1);
     dX.set( (double)0, 2,1);
-    dX.set( (double)PI/2, 3,1);
+    dX.set( (double)0, 3,1);
     dX.set( (double)0, 4,1);
     dX.set( (double)0, 5,1);    
     
@@ -167,8 +168,11 @@
                 
         //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();    
-        pcs.printf("State : \n\r");
-        (instance.getX()).afficherM();
+        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) );
         //pcs.printf("Sigma : \n\r");
         //(instance.getSigma()).afficher();
         pcs.printf("Kalman Gain : \n\r");
@@ -185,26 +189,29 @@
         else
             dir2.write(1);
             
-        if(abs(phi_r/phi_max) <= 1.0)
-        {
-            //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
-            setPWM(&pw1, (float)abs((double)phi_r/phi_max));
-        }
-        else
-        {
-            pcs.printf("P1...");
-            setPWM(&pw1, (float)abs((double)phi_max/phi_max));
-        }
-            
-        if(abs(phi_l/phi_max) <= 1.0)
-        {
-            //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
-            setPWM(&pw2,(float)abs((double)phi_l/phi_max));
-        }
-        else
-        {
-            pcs.printf("P2...");
-            setPWM(&pw2, (float)abs((double)phi_max/phi_max));
+        if(!testOdo)
+        {    
+            if(abs(phi_r/phi_max) <= 1.0)
+            {
+                //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
+                setPWM(&pw1, (float)abs((double)phi_r/phi_max));
+            }
+            else
+            {
+                pcs.printf("P1...");
+                setPWM(&pw1, (float)abs((double)phi_max/phi_max));
+            }
+                
+            if(abs(phi_l/phi_max) <= 1.0)
+            {
+                //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
+                setPWM(&pw2,(float)abs((double)phi_l/phi_max));
+            }
+            else
+            {
+                pcs.printf("P2...");
+                setPWM(&pw2, (float)abs((double)phi_max/phi_max));
+            }
         }
             
         //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
@@ -232,13 +239,15 @@
 {
     z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
     z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
-    z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);   
+    double theta = odometry->getTheta();
+    theta = atan21(sin(theta),cos(theta));
+    z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
     double vx = (double)odometry->getVx();
     double vy = (double)odometry->getVy();
     z->set( sqrt(vx*vx+vy*vy),4,1);
     z->set( (double)odometry->getW(),5,1); 
     
-    z->afficherM();
+    //z->afficherM();
 }
 
 Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)