Diff: Asservissement/Asserv.h

Revision:

100:a827a645d6c2

Parent:

94:5c37bcf73d14

Parent:

91:c0b436c52ede

--- a/Asservissement/Asserv.h	Thu Apr 30 16:14:39 2015 +0000
+++ b/Asservissement/Asserv.h	Thu Apr 30 16:16:29 2015 +0000
@@ -53,14 +53,30 @@
     r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
     
     T angle = (r.get(3,1) + dt*w);
+<<<<<<< local
     
     while(angle > (T)PI)
+=======
+    if( angle < -(T)PI)
+>>>>>>> other
     {
+<<<<<<< local
          angle -= 2.0f*(T)PI;                 
+=======
+        angle = angle - (T)PI*ceil(angle/PI);
+>>>>>>> other
     }
+<<<<<<< local
     while(angle <= -(T)PI)
+=======
+    else if( angle > (T)PI)
+>>>>>>> other
     {
+<<<<<<< local
         angle += 2.0f*(T)PI;                
+=======
+        angle = angle - (T)PI*floor(angle/PI);
+>>>>>>> other
     }
     
     /*
@@ -333,7 +349,11 @@
         behaviour = 0;
         
         T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));                        
+<<<<<<< local
         alpha = alpha - X.get(3,1);//atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
+=======
+        alpha = alpha - atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
+>>>>>>> other
         
         while(alpha > (T)PI)
         {
@@ -452,15 +472,31 @@
             //instanceEKF->state_Callback();
             instanceEKF->setX(z);
             X = instanceEKF->getX();
+<<<<<<< local
+=======
             
 #ifdef debugAsserv            
             logger.printf("STATE CALLBACK : DONE. \r\n");
             transpose(X).afficherMblue();
             transpose(z).afficherMblue();
 #endif            
+>>>>>>> other
             
+<<<<<<< local
+#ifdef debugAsserv            
+            logger.printf("STATE CALLBACK : DONE. \r\n");
+            transpose(X).afficherMblue();
+            transpose(z).afficherMblue();
+#endif            
+=======
+            instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
+>>>>>>> other
+            
+<<<<<<< local
             //instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
             instanceEKF->computeCommand(dXbehaviour, (T)dt, 1);                    
+=======
+>>>>>>> other
 #ifdef debugAsserv            
             logger.printf("COMMAND CALLBACK : DONE. \r\n");
 #endif