Diff: Controls/Dynamics.cpp

Revision:

15:d88f10b3b5f8

Parent:

14:d620415259b1

Child:

16:5b19be27f08a

--- a/Controls/Dynamics.cpp	Wed Dec 09 03:40:38 2015 +0000
+++ b/Controls/Dynamics.cpp	Thu Dec 10 09:46:56 2015 +0000
@@ -24,15 +24,44 @@
     float K = gains->getSwingUpK();
     float D = gains->getSwingUpD();
     
-    float th2Des;
-    if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z);
-    else th2Des = thetaDesiredForSwingUp(-1.5, 1.5, z);
-    
-    float ddth2 = K*(th2Des - th2) - D*dth2;
+    float force;
+    if (getEnergy(z, p) > target->getTargetEnergy()) {
+        float th2Des = target->getTheta2ForTarget(z);
+        force = K*(th2Des - th2) - D*dth2;
+    } 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;
+    }
 
-    return gains->getDesiredThetaP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat;
+    return force;// + corrCentripCompHat + gravityCompHat;//gains->getDesiredThetaP()*AHat*
 }
 
+float obstacleAvoidance(volatile float z[4], float p[10], float theta){
+    
+    float armLength = p[0];
+    float latticePitch = p[9];
+    
+    float safeRad = 0.07;
+    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 th1 = z[0];
+    float th2 = z[1];
+    
+    float direction = 1;
+    if (th2<0) direction = -1;
+    
+    float th2MinPhase = direction*th2MinMin;
+    float th2Min = th2MinMin+th2MinAmp*cos(4.0*(th1+th2MinPhase));
+    
+    if (direction*theta < th2Min) return direction*th2Min;
+    return theta;
+}
 
 float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){