Diff: Asservissement/Asserv.h

Revision:

107:a6e498b5706c

Parent:

97:70bb90e8fe90

--- a/Asservissement/Asserv.h	Tue May 05 05:05:07 2015 +0000
+++ b/Asservissement/Asserv.h	Tue May 05 12:08:20 2015 +0000
@@ -207,7 +207,7 @@
 {
     private :  
     
-    Odometry* odometry;      
+    Odometry2* odometry;      
     EKF<T>* instanceEKF;
     int nbrstate;
     int nbrcontrol;
@@ -242,8 +242,9 @@
     bool isarrivedalpha;
     bool isarrivedrho;  
     int behaviour;
+    bool rhoOnly;
     
-    Asserv(Odometry* odometry)
+    Asserv(Odometry2* odometry)
     {        
         /*Odometry*/
         this->odometry = odometry;
@@ -310,6 +311,7 @@
         isarrivedrho = true;
         execution = false;
         behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
+        rhoOnly = false;
             
     }
     
@@ -352,10 +354,51 @@
         
         dXrho = dX;
         dXrho.set( alpha, 3,1);
+        rhoOnly = false;
         
     }
     
-    bool isArrived() { return isarrived;}  
+    
+    void setGoal(T x, T y)
+    {
+        instanceEKF->resetPID();
+        dX.set(x,1,1);
+        dX.set(y,2,1);
+        //delayed to alpha so that it won't even move : dX.set(alpha,3,1);
+        dX.set((T)0,4,1);
+        dX.set((T)0,5,1);
+        execution = true;
+        isarrived = false;
+        isarrivedalpha = false;
+        isarrivedrho = false;
+        behaviour = 0;
+        
+        T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));                        
+        alpha = alpha - X.get(3,1);//atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
+        
+        while(alpha > (T)PI)
+        {
+             alpha -= 2.0f*(T)PI;                 
+        }
+        while(alpha <= -(T)PI)
+        {
+            alpha += 2.0f*(T)PI;                
+        }
+        
+        dXalpha.set( X.get(1,1), 1,1);
+        dXalpha.set( X.get(2,1), 2,1);
+        dXalpha.set( alpha, 3,1);
+        dXalpha.set((T)0, 4,1);
+        dXalpha.set((T)0, 5,1);
+        
+        dX.set( alpha, 3,1);
+        dXrho = dX;
+        dXrho.set( alpha, 3,1);
+        rhoOnly = true;
+        
+    }
+    
+    bool isArrived() { return (rhoOnly ? isarrivedrho : isarrived);}  
     bool isArrivedRho() {   return isarrivedrho;}      
     
     void stop()