Diff: Controls/Dynamics.cpp

Revision:

10:769cc457c3a4

Parent:

9:1d9b24d7ac77

Child:

11:711d3c207e8c

--- a/Controls/Dynamics.cpp	Sat Dec 05 00:40:42 2015 +0000
+++ b/Controls/Dynamics.cpp	Sat Dec 05 09:04:23 2015 +0000
@@ -2,10 +2,10 @@
 #include <math.h>
 # define M_PI           3.14159265358979323846
 
-float calcTau(float z[4], float p[10], Gains *gains, Serial *pc){
+float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){
             
     float A[2][2];
-    getMassMatrix(A);
+    getMassMatrix(A, z, p);
     
     float th1 = z[0];
     float th2 = z[1];
@@ -29,18 +29,18 @@
 //%     J = gripper_jacobian;
 //%     lambda = A*J_inv;
 
-    float gravityComp = getGravity(z, p);
+    float gravityComp = 0;//getGravity(z, p);
 
     //todo coriolis/cetripedal compensation
-    float corCentripComp = getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2;
+    float corCentripComp = 0;//getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2;
 
     return obstacleAvoidance + energyIncr + gravityComp + corCentripComp;
 }
 
 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)
 }