bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Dynamics.cpp Source File

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 }