bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Dynamics.cpp
- Committer:
- amandaghassaei
- Date:
- 2015-12-09
- Revision:
- 14:d620415259b1
- Parent:
- 13:64d337c5114e
- Child:
- 15:d88f10b3b5f8
File content as of revision 14:d620415259b1:
#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 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; return gains->getDesiredThetaP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat; } float thetaDesiredForSwingUp(float rangeMin, float rangeMax, 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; 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); }