Diff: Controls/Dynamics.cpp

Revision:

16:5b19be27f08a

Parent:

15:d88f10b3b5f8

Child:

17:8a0e647cf551

--- a/Controls/Dynamics.cpp	Thu Dec 10 09:46:56 2015 +0000
+++ b/Controls/Dynamics.cpp	Thu Dec 10 10:55:35 2015 +0000
@@ -25,17 +25,19 @@
     float D = gains->getSwingUpD();
     
     float force;
-    if (getEnergy(z, p) > target->getTargetEnergy()) {
-        float th2Des = target->getTheta2ForTarget(z);
-        force = K*(th2Des - th2) - D*dth2;
+    if (target->getTargetingStarted() || target->shouldSwitchToTargetingController(z, p)) {
+        target->setTargetingStarted(true);
+//        float th2Des = target->getFinalTh2(z);
+//        force = K*(th2Des - th2) - D*dth2;
+        force = target->calcTargetingForce(z, p, K, D);
     } else {
         float softLimit = 1.5;//2.5;//143 degrees
         float th2Des = thetaDesiredForSwingUp(-softLimit, softLimit, z);
-        th2Des = obstacleAvoidance(z, p, th2Des);
-        force = K*(th2Des - th2) - D*dth2;
+//        th2Des = obstacleAvoidance(z, p, th2Des);
+        force = gains->getDesiredThetaP()*AHat*(K*(th2Des - th2) - D*dth2);
     }
 
-    return force;// + corrCentripCompHat + gravityCompHat;//gains->getDesiredThetaP()*AHat*
+    return force;// + corrCentripCompHat + gravityCompHat;//gains->getDesiredThetaP()*
 }
 
 float obstacleAvoidance(volatile float z[4], float p[10], float theta){
@@ -67,14 +69,8 @@
     
     float th1 = z[0];
     float dth1 = z[2];
-    
-    int numTurns = fix(th1/(2*M_PI));
-    float th1Rel = th1-numTurns*2*M_PI;
+    float th1Rel = boundTheta(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);
 }
\ No newline at end of file