Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Controls/Dynamics.cpp@20:f13b949b623b, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
19:270735e44c98 
publish

Who changed what in which revision?

UserRevisionLine numberNew 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 18:0cfe72d8a006 27 float force = 0;
amandaghassaei 18:0cfe72d8a006 28 // if (target->getTargetingStarted() || target->shouldSwitchToTargetingController(z, p)) {
amandaghassaei 18:0cfe72d8a006 29 // target->setTargetingStarted(true);
amandaghassaei 18:0cfe72d8a006 30 // float th2Des = target->getFinalTh2(z);
amandaghassaei 18:0cfe72d8a006 31 // force = K*(th2Des - th2) - D*dth2;
amandaghassaei 18:0cfe72d8a006 32 //// force = target->calcTargetingForce(z, p, K, D);
amandaghassaei 18:0cfe72d8a006 33 // } else {
amandaghassaei 19:270735e44c98 34 float softLimit = 2.35;//2.5;//143 degrees
amandaghassaei 19:270735e44c98 35 float th2Des = thetaDesiredForSwingUp(-softLimit, softLimit, z);
amandaghassaei 19:270735e44c98 36 th2Des = obstacleAvoidance(z, p, th2Des);
amandaghassaei 19:270735e44c98 37 float P = overallGainForSwingUp(z, th2Des, gains);
amandaghassaei 19:270735e44c98 38 force = P*(K*(th2Des - th2) - D*dth2);//AHat*
amandaghassaei 18:0cfe72d8a006 39 // }
amandaghassaei 8:1a3a69fecedf 40
amandaghassaei 19:270735e44c98 41 return force + corrCentripCompHat + gravityCompHat;
amandaghassaei 8:1a3a69fecedf 42 }
amandaghassaei 8:1a3a69fecedf 43
amandaghassaei 15:d88f10b3b5f8 44 float obstacleAvoidance(volatile float z[4], float p[10], float theta){
amandaghassaei 15:d88f10b3b5f8 45
amandaghassaei 15:d88f10b3b5f8 46 float armLength = p[0];
amandaghassaei 15:d88f10b3b5f8 47 float latticePitch = p[9];
amandaghassaei 15:d88f10b3b5f8 48
amandaghassaei 19:270735e44c98 49 float safeRad = 0.02;
amandaghassaei 15:d88f10b3b5f8 50 float th2MinMin = M_PI-2.0*asin((latticePitch-safeRad)/(2.0*armLength));
amandaghassaei 15:d88f10b3b5f8 51 float th2MinMax = M_PI-2.0*asin((latticePitch*sqrt(2.0)-safeRad)/(2.0*armLength));
amandaghassaei 15:d88f10b3b5f8 52
amandaghassaei 15:d88f10b3b5f8 53 float th2MinAvg = (th2MinMin+th2MinMax)/2.0;
amandaghassaei 19:270735e44c98 54 float th2MinAmp = (th2MinMin-th2MinAvg);
amandaghassaei 15:d88f10b3b5f8 55
amandaghassaei 15:d88f10b3b5f8 56 float th1 = z[0];
amandaghassaei 15:d88f10b3b5f8 57 float th2 = z[1];
amandaghassaei 15:d88f10b3b5f8 58
amandaghassaei 15:d88f10b3b5f8 59 float direction = 1;
amandaghassaei 15:d88f10b3b5f8 60 if (th2<0) direction = -1;
amandaghassaei 15:d88f10b3b5f8 61
amandaghassaei 15:d88f10b3b5f8 62 float th2MinPhase = direction*th2MinMin;
amandaghassaei 19:270735e44c98 63 float th2Min = th2MinAvg+th2MinAmp*cos(4.0*(th1+th2MinPhase));
amandaghassaei 15:d88f10b3b5f8 64
amandaghassaei 15:d88f10b3b5f8 65 if (direction*theta < th2Min) return direction*th2Min;
amandaghassaei 15:d88f10b3b5f8 66 return theta;
amandaghassaei 15:d88f10b3b5f8 67 }
amandaghassaei 12:49813131dd15 68
amandaghassaei 13:64d337c5114e 69 float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){
amandaghassaei 12:49813131dd15 70
amandaghassaei 12:49813131dd15 71 float th1 = z[0];
amandaghassaei 12:49813131dd15 72 float dth1 = z[2];
amandaghassaei 16:5b19be27f08a 73 float th1Rel = boundTheta(th1);
amandaghassaei 8:1a3a69fecedf 74
amandaghassaei 13:64d337c5114e 75 if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1)
amandaghassaei 13:64d337c5114e 76 return rangeMax*abs(cos(th1Rel/2.0));
amandaghassaei 18:0cfe72d8a006 77 }
amandaghassaei 18:0cfe72d8a006 78
amandaghassaei 18:0cfe72d8a006 79
amandaghassaei 18:0cfe72d8a006 80 float overallGainForSwingUp(volatile float z[4], float th2Des, Gains *gains){
amandaghassaei 18:0cfe72d8a006 81 float th2 = z[1];
amandaghassaei 18:0cfe72d8a006 82 float dth1 = z[2];
amandaghassaei 19:270735e44c98 83 if ((dth1<0 && th2<th2Des) || (dth1>0 && th2>th2Des)) return gains->getSoftLimitsP();
amandaghassaei 18:0cfe72d8a006 84 return gains->getDesiredThetaP();
amandaghassaei 8:1a3a69fecedf 85 }