Diff: Controls/Dynamics.cpp

Revision:

13:64d337c5114e

Parent:

12:49813131dd15

Child:

14:d620415259b1

--- a/Controls/Dynamics.cpp	Wed Dec 09 03:08:30 2015 +0000
+++ b/Controls/Dynamics.cpp	Wed Dec 09 03:25:22 2015 +0000
@@ -26,7 +26,7 @@
     
     float th2Des;
     if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z);
-    else th2Des = thetaDesired(2.5, z);
+    else th2Des = thetaDesiredForSwingUp(-1.5, 1.5, z);
     
     float ddth2 = K*(th2Des - th2) - D*dth2;
 
@@ -34,7 +34,7 @@
 }
 
 
-float thetaDesired(float range, volatile float z[4]){
+float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){
     
     float th1 = z[0];
     float dth1 = z[2];
@@ -42,14 +42,10 @@
     int numTurns = fix(th1/(2*M_PI));
     float th1Rel = th1-numTurns*2*M_PI;
     
-    return signNonZero(dth1)*(range*abs(cos(th1Rel/2.0)));//-abs(th1Rel));//*cos(th1)
+    if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1)
+    return rangeMax*abs(cos(th1Rel/2.0));
 }
 
 int fix(float val){//round toward zero
     return val > 0 ? floor(val) : ceil(val);
-}
-
-int signNonZero(float val){
-    if (val < 0) return -1;
-    return 1;
 }
\ No newline at end of file