bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Dynamics.cpp
00001 #include "Dynamics.h" 00002 #include <math.h> 00003 # define M_PI 3.14159265358979323846 00004 00005 float calcTau(volatile float z[4], float p[10], Gains *gains, Target *target, Serial *pc){ 00006 00007 float th1 = z[0]; 00008 float th2 = z[1]; 00009 float dth1 = z[2]; 00010 float dth2 = z[3]; 00011 00012 float A[2][2]; 00013 getMassMatrix(A, z, p); 00014 float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0]; 00015 00016 float corrCentripComp[2]; 00017 getCoriolisCentrip(corrCentripComp, z, p); 00018 float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0]; 00019 00020 float gravityComp[2]; 00021 getGravity(gravityComp, z, p); 00022 float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0]; 00023 00024 float K = gains->getSwingUpK(); 00025 float D = gains->getSwingUpD(); 00026 00027 float force = 0; 00028 // if (target->getTargetingStarted() || target->shouldSwitchToTargetingController(z, p)) { 00029 // target->setTargetingStarted(true); 00030 // float th2Des = target->getFinalTh2(z); 00031 // force = K*(th2Des - th2) - D*dth2; 00032 //// force = target->calcTargetingForce(z, p, K, D); 00033 // } else { 00034 float softLimit = 2.35;//2.5;//143 degrees 00035 float th2Des = thetaDesiredForSwingUp(-softLimit, softLimit, z); 00036 th2Des = obstacleAvoidance(z, p, th2Des); 00037 float P = overallGainForSwingUp(z, th2Des, gains); 00038 force = P*(K*(th2Des - th2) - D*dth2);//AHat* 00039 // } 00040 00041 return force + corrCentripCompHat + gravityCompHat; 00042 } 00043 00044 float obstacleAvoidance(volatile float z[4], float p[10], float theta){ 00045 00046 float armLength = p[0]; 00047 float latticePitch = p[9]; 00048 00049 float safeRad = 0.02; 00050 float th2MinMin = M_PI-2.0*asin((latticePitch-safeRad)/(2.0*armLength)); 00051 float th2MinMax = M_PI-2.0*asin((latticePitch*sqrt(2.0)-safeRad)/(2.0*armLength)); 00052 00053 float th2MinAvg = (th2MinMin+th2MinMax)/2.0; 00054 float th2MinAmp = (th2MinMin-th2MinAvg); 00055 00056 float th1 = z[0]; 00057 float th2 = z[1]; 00058 00059 float direction = 1; 00060 if (th2<0) direction = -1; 00061 00062 float th2MinPhase = direction*th2MinMin; 00063 float th2Min = th2MinAvg+th2MinAmp*cos(4.0*(th1+th2MinPhase)); 00064 00065 if (direction*theta < th2Min) return direction*th2Min; 00066 return theta; 00067 } 00068 00069 float thetaDesiredForSwingUp(float rangeMin, float rangeMax, volatile float z[4]){ 00070 00071 float th1 = z[0]; 00072 float dth1 = z[2]; 00073 float th1Rel = boundTheta(th1); 00074 00075 if (dth1<0) return rangeMin*abs(cos(th1Rel/2.0));//-abs(th1Rel));//*cos(th1) 00076 return rangeMax*abs(cos(th1Rel/2.0)); 00077 } 00078 00079 00080 float overallGainForSwingUp(volatile float z[4], float th2Des, Gains *gains){ 00081 float th2 = z[1]; 00082 float dth1 = z[2]; 00083 if ((dth1<0 && th2<th2Des) || (dth1>0 && th2>th2Des)) return gains->getSoftLimitsP(); 00084 return gains->getDesiredThetaP(); 00085 }
Generated on Sun Jul 17 2022 01:31:45 by 1.7.2