Diff: Controls/Dynamics.cpp

Revision:

17:8a0e647cf551

Parent:

16:5b19be27f08a

Child:

18:0cfe72d8a006

--- a/Controls/Dynamics.cpp	Thu Dec 10 10:55:35 2015 +0000
+++ b/Controls/Dynamics.cpp	Fri Dec 11 00:44:45 2015 +0000
@@ -27,17 +27,17 @@
     float force;
     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);
+        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);
+        th2Des = obstacleAvoidance(z, p, th2Des);
         force = gains->getDesiredThetaP()*AHat*(K*(th2Des - th2) - D*dth2);
     }
 
-    return force;// + corrCentripCompHat + gravityCompHat;//gains->getDesiredThetaP()*
+    return force;// + corrCentripCompHat + gravityCompHat;
 }
 
 float obstacleAvoidance(volatile float z[4], float p[10], float theta){