Diff: Controls/Dynamics.cpp

Revision:

18:0cfe72d8a006

Parent:

17:8a0e647cf551

Child:

19:270735e44c98

--- a/Controls/Dynamics.cpp	Fri Dec 11 00:44:45 2015 +0000
+++ b/Controls/Dynamics.cpp	Fri Dec 11 06:16:33 2015 +0000
@@ -24,20 +24,21 @@
     float K = gains->getSwingUpK();
     float D = gains->getSwingUpD();
     
-    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);
-    } else {
-        float softLimit = 1.5;//2.5;//143 degrees
-        float th2Des = thetaDesiredForSwingUp(-softLimit, softLimit, z);
-        th2Des = obstacleAvoidance(z, p, th2Des);
-        force = gains->getDesiredThetaP()*AHat*(K*(th2Des - th2) - D*dth2);
-    }
+    float force = 0;
+   // 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);
+//        float P = overallGainForSwingUp(z, th2Des, gains);
+//        force = P*(K*(th2Des - th2) - D*dth2);//AHat*
+//    }
 
-    return force;// + corrCentripCompHat + gravityCompHat;
+    return force + gravityCompHat;//corrCentripCompHat
 }
 
 float obstacleAvoidance(volatile float z[4], float p[10], float theta){
@@ -73,4 +74,12 @@
     
     if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1)
     return rangeMax*abs(cos(th1Rel/2.0));
+}
+
+
+float overallGainForSwingUp(volatile float z[4], float th2Des, Gains *gains){
+    float th2 = z[1];
+    float dth1 = z[2];
+    if ((dth1<0 && th2<th2Des) || (dth1>0 && th2>th2Des)) return 1.0;
+    return gains->getDesiredThetaP();
 }
\ No newline at end of file