Diff: Controls/Dynamics.cpp

Revision:

19:270735e44c98

Parent:

18:0cfe72d8a006

--- a/Controls/Dynamics.cpp	Fri Dec 11 06:16:33 2015 +0000
+++ b/Controls/Dynamics.cpp	Fri Dec 11 08:53:08 2015 +0000
@@ -31,14 +31,14 @@
 //        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*
+        float softLimit = 2.35;//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 + gravityCompHat;//corrCentripCompHat
+    return force + corrCentripCompHat + gravityCompHat;
 }
 
 float obstacleAvoidance(volatile float z[4], float p[10], float theta){
@@ -46,12 +46,12 @@
     float armLength = p[0];
     float latticePitch = p[9];
     
-    float safeRad = 0.07;
+    float safeRad = 0.02;
     float th2MinMin = M_PI-2.0*asin((latticePitch-safeRad)/(2.0*armLength));
     float th2MinMax = M_PI-2.0*asin((latticePitch*sqrt(2.0)-safeRad)/(2.0*armLength));
     
     float th2MinAvg = (th2MinMin+th2MinMax)/2.0;
-    float th2MinAmp = (th2MinMin-th2MinAvg)/2.0;
+    float th2MinAmp = (th2MinMin-th2MinAvg);
     
     float th1 = z[0];
     float th2 = z[1];
@@ -60,7 +60,7 @@
     if (th2<0) direction = -1;
     
     float th2MinPhase = direction*th2MinMin;
-    float th2Min = th2MinMin+th2MinAmp*cos(4.0*(th1+th2MinPhase));
+    float th2Min = th2MinAvg+th2MinAmp*cos(4.0*(th1+th2MinPhase));
     
     if (direction*theta < th2Min) return direction*th2Min;
     return theta;
@@ -80,6 +80,6 @@
 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;
+    if ((dth1<0 && th2<th2Des) || (dth1>0 && th2>th2Des)) return gains->getSoftLimitsP();
     return gains->getDesiredThetaP();
 }
\ No newline at end of file