bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
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