bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Dynamics.cpp
- Committer:
- amandaghassaei
- Date:
- 2015-12-05
- Revision:
- 9:1d9b24d7ac77
- Parent:
- 8:1a3a69fecedf
- Child:
- 10:769cc457c3a4
File content as of revision 9:1d9b24d7ac77:
#include "Dynamics.h" #include <math.h> # define M_PI 3.14159265358979323846 float calcTau(float z[4], float p[10], Gains *gains, Serial *pc){ float A[2][2]; getMassMatrix(A); float th1 = z[0]; float th2 = z[1]; float dth1 = z[2]; float dth2 = z[3]; float E = getEnergy(z, p); float A_hat22 = A[1][1]-A[1][0]*A[0][1]/A[0][0]; float K = gains->getSwingUpK(); float D = gains->getSwingUpD(); float th2Des = thetaDesired(3*M_PI/6, th1, th2, dth1, dth2); float v = K*(th2Des - th2) - D*dth2;// + k3*u_hat; float energyIncr = A_hat22*v; float obstacleAvoidance = 0;//%obstacle_avoidance(z, p); //Compute virtual foce //% J = gripper_jacobian; //% lambda = A*J_inv; float gravityComp = getGravity(z, p); //todo coriolis/cetripedal compensation float corCentripComp = getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2; return obstacleAvoidance + energyIncr + gravityComp + corCentripComp; } float thetaDesired(float range, float th1, float th2, float dth1, float dth2){ int numTurns = fix(th1/(2*M_PI)); float th1Rel = th1-numTurns*2*M_PI; return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1) } int fix(float val){//round toward zero return val/1; } int signNonZero(float val){ if (val < 0) return -1; return 1; }