Diff: Controls/Dynamics.cpp

Revision:

12:49813131dd15

Parent:

11:711d3c207e8c

Child:

13:64d337c5114e

--- a/Controls/Dynamics.cpp	Tue Dec 08 22:52:46 2015 +0000
+++ b/Controls/Dynamics.cpp	Wed Dec 09 03:08:30 2015 +0000
@@ -2,15 +2,13 @@
 #include <math.h>
 # define M_PI           3.14159265358979323846
 
-float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){
+float calcTau(volatile float z[4], float p[10], Gains *gains, Target *target, Serial *pc){
     
     float th1 = z[0];
     float th2 = z[1];
     float dth1 = z[2];
     float dth2 = z[3];
     
-    float E = getEnergy(z, p);
-    
     float A[2][2];
     getMassMatrix(A, z, p);
     float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0];
@@ -26,20 +24,25 @@
     float K = gains->getSwingUpK();
     float D = gains->getSwingUpD();
     
-    float th2Des = thetaDesired(2.2, th1, th2, dth1, dth2);
+    float th2Des;
+    if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z);
+    else th2Des = thetaDesired(2.5, z);
     
     float ddth2 = K*(th2Des - th2) - D*dth2;
-    float tau = gains->getCurrentP()*AHat*ddth2 + corrCentripCompHat + gravityCompHat;
 
-    return tau;
+    return gains->getCurrentP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat;
 }
 
-float thetaDesired(float range, float th1, float th2, float dth1, float dth2){
+
+float thetaDesired(float range, volatile float z[4]){
+    
+    float th1 = z[0];
+    float dth1 = z[2];
     
     int numTurns = fix(th1/(2*M_PI));
     float th1Rel = th1-numTurns*2*M_PI;
     
-    return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1)
+    return signNonZero(dth1)*(range*abs(cos(th1Rel/2.0)));//-abs(th1Rel));//*cos(th1)
 }
 
 int fix(float val){//round toward zero