![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Dynamics.cpp
- Committer:
- amandaghassaei
- Date:
- 2015-12-11
- Revision:
- 20:f13b949b623b
- Parent:
- 19:270735e44c98
File content as of revision 20:f13b949b623b:
#include "Dynamics.h" #include <math.h> # define M_PI 3.14159265358979323846 float calcTau(volatile float z[4], float p[10], Gains *gains, Target *target, Serial *pc){ float th1 = z[0]; float th2 = z[1]; float dth1 = z[2]; float dth2 = z[3]; float A[2][2]; getMassMatrix(A, z, p); float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0]; float corrCentripComp[2]; getCoriolisCentrip(corrCentripComp, z, p); float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0]; float gravityComp[2]; getGravity(gravityComp, z, p); float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0]; float K = gains->getSwingUpK(); float D = gains->getSwingUpD(); 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 = 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 + corrCentripCompHat + gravityCompHat; } float obstacleAvoidance(volatile float z[4], float p[10], float theta){ float armLength = p[0]; float latticePitch = p[9]; 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); float th1 = z[0]; float th2 = z[1]; float direction = 1; if (th2<0) direction = -1; float th2MinPhase = direction*th2MinMin; float th2Min = th2MinAvg+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]){ float th1 = z[0]; float dth1 = z[2]; float th1Rel = boundTheta(th1); 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 gains->getSoftLimitsP(); return gains->getDesiredThetaP(); }