bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Dynamics.cpp
- Revision:
- 11:711d3c207e8c
- Parent:
- 10:769cc457c3a4
- Child:
- 12:49813131dd15
diff -r 769cc457c3a4 -r 711d3c207e8c Controls/Dynamics.cpp --- a/Controls/Dynamics.cpp Sat Dec 05 09:04:23 2015 +0000 +++ b/Controls/Dynamics.cpp Tue Dec 08 22:52:46 2015 +0000 @@ -3,9 +3,6 @@ # define M_PI 3.14159265358979323846 float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){ - - float A[2][2]; - getMassMatrix(A, z, p); float th1 = z[0]; float th2 = z[1]; @@ -14,39 +11,39 @@ float E = getEnergy(z, p); - float A_hat22 = A[1][1]-A[1][0]*A[0][1]/A[0][0]; + float A[2][2]; + getMassMatrix(A, z, p); + float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0]; + + float corrCentripComp[2]; + getCoriolisCentrip(corrCentripComp, z, p); + float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0]; + + float gravityComp[2]; + getGravity(gravityComp, z, p); + float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0]; + float K = gains->getSwingUpK(); float D = gains->getSwingUpD(); - float th2Des = thetaDesired(3*M_PI/6, th1, th2, dth1, dth2); + float th2Des = thetaDesired(2.2, 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); + float ddth2 = K*(th2Des - th2) - D*dth2; + float tau = gains->getCurrentP()*AHat*ddth2 + corrCentripCompHat + gravityCompHat; - //Compute virtual foce -//% J = gripper_jacobian; -//% lambda = A*J_inv; - - float gravityComp = 0;//getGravity(z, p); - - //todo coriolis/cetripedal compensation - float corCentripComp = 0;//getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2; - - return obstacleAvoidance + energyIncr + gravityComp + corCentripComp; + return tau; } 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; + 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; + return val > 0 ? floor(val) : ceil(val); } int signNonZero(float val){