bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Dynamics.cpp
- Revision:
- 13:64d337c5114e
- Parent:
- 12:49813131dd15
- Child:
- 14:d620415259b1
--- a/Controls/Dynamics.cpp Wed Dec 09 03:08:30 2015 +0000 +++ b/Controls/Dynamics.cpp Wed Dec 09 03:25:22 2015 +0000 @@ -26,7 +26,7 @@ float th2Des; if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z); - else th2Des = thetaDesired(2.5, z); + else th2Des = thetaDesiredForSwingUp(-1.5, 1.5, z); float ddth2 = K*(th2Des - th2) - D*dth2; @@ -34,7 +34,7 @@ } -float thetaDesired(float range, volatile float z[4]){ +float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){ float th1 = z[0]; float dth1 = z[2]; @@ -42,14 +42,10 @@ int numTurns = fix(th1/(2*M_PI)); float th1Rel = th1-numTurns*2*M_PI; - return signNonZero(dth1)*(range*abs(cos(th1Rel/2.0)));//-abs(th1Rel));//*cos(th1) + if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1) + return rangeMax*abs(cos(th1Rel/2.0)); } int fix(float val){//round toward zero return val > 0 ? floor(val) : ceil(val); -} - -int signNonZero(float val){ - if (val < 0) return -1; - return 1; } \ No newline at end of file