bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Dynamics.cpp@14:d620415259b1, 2015-12-09 (annotated)
- Committer:
- amandaghassaei
- Date:
- Wed Dec 09 03:40:38 2015 +0000
- Revision:
- 14:d620415259b1
- Parent:
- 13:64d337c5114e
- Child:
- 15:d88f10b3b5f8
renaming gains variable
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
amandaghassaei | 8:1a3a69fecedf | 1 | #include "Dynamics.h" |
amandaghassaei | 8:1a3a69fecedf | 2 | #include <math.h> |
amandaghassaei | 8:1a3a69fecedf | 3 | # define M_PI 3.14159265358979323846 |
amandaghassaei | 8:1a3a69fecedf | 4 | |
amandaghassaei | 12:49813131dd15 | 5 | float calcTau(volatile float z[4], float p[10], Gains *gains, Target *target, Serial *pc){ |
amandaghassaei | 8:1a3a69fecedf | 6 | |
amandaghassaei | 8:1a3a69fecedf | 7 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 8 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 9 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 10 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 11 | |
amandaghassaei | 11:711d3c207e8c | 12 | float A[2][2]; |
amandaghassaei | 11:711d3c207e8c | 13 | getMassMatrix(A, z, p); |
amandaghassaei | 11:711d3c207e8c | 14 | float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0]; |
amandaghassaei | 11:711d3c207e8c | 15 | |
amandaghassaei | 11:711d3c207e8c | 16 | float corrCentripComp[2]; |
amandaghassaei | 11:711d3c207e8c | 17 | getCoriolisCentrip(corrCentripComp, z, p); |
amandaghassaei | 11:711d3c207e8c | 18 | float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0]; |
amandaghassaei | 11:711d3c207e8c | 19 | |
amandaghassaei | 11:711d3c207e8c | 20 | float gravityComp[2]; |
amandaghassaei | 11:711d3c207e8c | 21 | getGravity(gravityComp, z, p); |
amandaghassaei | 11:711d3c207e8c | 22 | float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0]; |
amandaghassaei | 11:711d3c207e8c | 23 | |
amandaghassaei | 9:1d9b24d7ac77 | 24 | float K = gains->getSwingUpK(); |
amandaghassaei | 9:1d9b24d7ac77 | 25 | float D = gains->getSwingUpD(); |
amandaghassaei | 8:1a3a69fecedf | 26 | |
amandaghassaei | 12:49813131dd15 | 27 | float th2Des; |
amandaghassaei | 12:49813131dd15 | 28 | if (getEnergy(z, p) > target->getTargetEnergy()) th2Des = target->getTheta2ForTarget(z); |
amandaghassaei | 13:64d337c5114e | 29 | else th2Des = thetaDesiredForSwingUp(-1.5, 1.5, z); |
amandaghassaei | 8:1a3a69fecedf | 30 | |
amandaghassaei | 11:711d3c207e8c | 31 | float ddth2 = K*(th2Des - th2) - D*dth2; |
amandaghassaei | 8:1a3a69fecedf | 32 | |
amandaghassaei | 14:d620415259b1 | 33 | return gains->getDesiredThetaP()*AHat*ddth2;// + corrCentripCompHat + gravityCompHat; |
amandaghassaei | 8:1a3a69fecedf | 34 | } |
amandaghassaei | 8:1a3a69fecedf | 35 | |
amandaghassaei | 12:49813131dd15 | 36 | |
amandaghassaei | 13:64d337c5114e | 37 | float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){ |
amandaghassaei | 12:49813131dd15 | 38 | |
amandaghassaei | 12:49813131dd15 | 39 | float th1 = z[0]; |
amandaghassaei | 12:49813131dd15 | 40 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 41 | |
amandaghassaei | 11:711d3c207e8c | 42 | int numTurns = fix(th1/(2*M_PI)); |
amandaghassaei | 11:711d3c207e8c | 43 | float th1Rel = th1-numTurns*2*M_PI; |
amandaghassaei | 8:1a3a69fecedf | 44 | |
amandaghassaei | 13:64d337c5114e | 45 | if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1) |
amandaghassaei | 13:64d337c5114e | 46 | return rangeMax*abs(cos(th1Rel/2.0)); |
amandaghassaei | 8:1a3a69fecedf | 47 | } |
amandaghassaei | 8:1a3a69fecedf | 48 | |
amandaghassaei | 8:1a3a69fecedf | 49 | int fix(float val){//round toward zero |
amandaghassaei | 11:711d3c207e8c | 50 | return val > 0 ? floor(val) : ceil(val); |
amandaghassaei | 8:1a3a69fecedf | 51 | } |