bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Dynamics.cpp
- Revision:
- 12:49813131dd15
- Parent:
- 11:711d3c207e8c
- Child:
- 13:64d337c5114e
--- a/Controls/Dynamics.cpp Tue Dec 08 22:52:46 2015 +0000 +++ b/Controls/Dynamics.cpp Wed Dec 09 03:08:30 2015 +0000 @@ -2,15 +2,13 @@ #include <math.h> # define M_PI 3.14159265358979323846 -float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){ +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 E = getEnergy(z, p); - float A[2][2]; getMassMatrix(A, z, p); float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0]; @@ -26,20 +24,25 @@ float K = gains->getSwingUpK(); float D = gains->getSwingUpD(); - float th2Des = thetaDesired(2.2, th1, th2, dth1, dth2); + float th2Des; + if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z); + else th2Des = thetaDesired(2.5, z); float ddth2 = K*(th2Des - th2) - D*dth2; - float tau = gains->getCurrentP()*AHat*ddth2 + corrCentripCompHat + gravityCompHat; - return tau; + return gains->getCurrentP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat; } -float thetaDesired(float range, float th1, float th2, float dth1, float dth2){ + +float thetaDesired(float range, volatile float z[4]){ + + float th1 = z[0]; + float dth1 = z[2]; int numTurns = fix(th1/(2*M_PI)); float th1Rel = th1-numTurns*2*M_PI; - return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1) + return signNonZero(dth1)*(range*abs(cos(th1Rel/2.0)));//-abs(th1Rel));//*cos(th1) } int fix(float val){//round toward zero