Diff: Controls/Dynamics.cpp

Revision:

11:711d3c207e8c

Parent:

10:769cc457c3a4

Child:

12:49813131dd15

--- a/Controls/Dynamics.cpp	Sat Dec 05 09:04:23 2015 +0000
+++ b/Controls/Dynamics.cpp	Tue Dec 08 22:52:46 2015 +0000
@@ -3,9 +3,6 @@
 # define M_PI           3.14159265358979323846
 
 float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){
-            
-    float A[2][2];
-    getMassMatrix(A, z, p);
     
     float th1 = z[0];
     float th2 = z[1];
@@ -14,39 +11,39 @@
     
     float E = getEnergy(z, p);
     
-    float A_hat22 = A[1][1]-A[1][0]*A[0][1]/A[0][0];
+    float A[2][2];
+    getMassMatrix(A, z, p);
+    float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0];
+    
+    float corrCentripComp[2];
+    getCoriolisCentrip(corrCentripComp, z, p);
+    float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0];
+    
+    float gravityComp[2];
+    getGravity(gravityComp, z, p);
+    float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0];
+    
     float K = gains->getSwingUpK();
     float D = gains->getSwingUpD();
     
-    float th2Des = thetaDesired(3*M_PI/6, th1, th2, dth1, dth2);
+    float th2Des = thetaDesired(2.2, th1, th2, dth1, dth2);
     
-    float v = K*(th2Des - th2) - D*dth2;// + k3*u_hat;
-    float energyIncr = A_hat22*v;
-
-    float obstacleAvoidance = 0;//%obstacle_avoidance(z, p);
+    float ddth2 = K*(th2Des - th2) - D*dth2;
+    float tau = gains->getCurrentP()*AHat*ddth2 + corrCentripCompHat + gravityCompHat;
 
-    //Compute virtual foce
-//%     J = gripper_jacobian;
-//%     lambda = A*J_inv;
-
-    float gravityComp = 0;//getGravity(z, p);
-
-    //todo coriolis/cetripedal compensation
-    float corCentripComp = 0;//getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2;
-
-    return obstacleAvoidance + energyIncr + gravityComp + corCentripComp;
+    return tau;
 }
 
 float thetaDesired(float range, float th1, float th2, float dth1, float dth2){
     
-//    int numTurns = fix(th1/(2*M_PI));
-    float th1Rel = th1;//-numTurns*2*M_PI;
+    int numTurns = fix(th1/(2*M_PI));
+    float th1Rel = th1-numTurns*2*M_PI;
     
     return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1)
 }
 
 int fix(float val){//round toward zero
-    return val/1;
+    return val > 0 ? floor(val) : ceil(val);
 }
 
 int signNonZero(float val){