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@10:769cc457c3a4, 2015-12-05 (annotated)

Committer:
amandaghassaei
Date:
Sat Dec 05 09:04:23 2015 +0000
Revision:
10:769cc457c3a4
Parent:
9:1d9b24d7ac77
Child:
11:711d3c207e8c
swinging looking good;

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 10:769cc457c3a4 5 float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){
amandaghassaei 8:1a3a69fecedf 6
amandaghassaei 8:1a3a69fecedf 7 float A[2][2];
amandaghassaei 10:769cc457c3a4 8 getMassMatrix(A, z, p);
amandaghassaei 8:1a3a69fecedf 9
amandaghassaei 8:1a3a69fecedf 10 float th1 = z[0];
amandaghassaei 8:1a3a69fecedf 11 float th2 = z[1];
amandaghassaei 8:1a3a69fecedf 12 float dth1 = z[2];
amandaghassaei 8:1a3a69fecedf 13 float dth2 = z[3];
amandaghassaei 8:1a3a69fecedf 14
amandaghassaei 8:1a3a69fecedf 15 float E = getEnergy(z, p);
amandaghassaei 8:1a3a69fecedf 16
amandaghassaei 8:1a3a69fecedf 17 float A_hat22 = A[1][1]-A[1][0]*A[0][1]/A[0][0];
amandaghassaei 9:1d9b24d7ac77 18 float K = gains->getSwingUpK();
amandaghassaei 9:1d9b24d7ac77 19 float D = gains->getSwingUpD();
amandaghassaei 8:1a3a69fecedf 20
amandaghassaei 8:1a3a69fecedf 21 float th2Des = thetaDesired(3*M_PI/6, th1, th2, dth1, dth2);
amandaghassaei 8:1a3a69fecedf 22
amandaghassaei 8:1a3a69fecedf 23 float v = K*(th2Des - th2) - D*dth2;// + k3*u_hat;
amandaghassaei 8:1a3a69fecedf 24 float energyIncr = A_hat22*v;
amandaghassaei 8:1a3a69fecedf 25
amandaghassaei 8:1a3a69fecedf 26 float obstacleAvoidance = 0;//%obstacle_avoidance(z, p);
amandaghassaei 8:1a3a69fecedf 27
amandaghassaei 8:1a3a69fecedf 28 //Compute virtual foce
amandaghassaei 8:1a3a69fecedf 29 //% J = gripper_jacobian;
amandaghassaei 8:1a3a69fecedf 30 //% lambda = A*J_inv;
amandaghassaei 8:1a3a69fecedf 31
amandaghassaei 10:769cc457c3a4 32 float gravityComp = 0;//getGravity(z, p);
amandaghassaei 8:1a3a69fecedf 33
amandaghassaei 8:1a3a69fecedf 34 //todo coriolis/cetripedal compensation
amandaghassaei 10:769cc457c3a4 35 float corCentripComp = 0;//getCoriolis(z, p);// - A_hat22*gripper_jacobian(z,p)*dth2;
amandaghassaei 8:1a3a69fecedf 36
amandaghassaei 8:1a3a69fecedf 37 return obstacleAvoidance + energyIncr + gravityComp + corCentripComp;
amandaghassaei 8:1a3a69fecedf 38 }
amandaghassaei 8:1a3a69fecedf 39
amandaghassaei 8:1a3a69fecedf 40 float thetaDesired(float range, float th1, float th2, float dth1, float dth2){
amandaghassaei 8:1a3a69fecedf 41
amandaghassaei 10:769cc457c3a4 42 // int numTurns = fix(th1/(2*M_PI));
amandaghassaei 10:769cc457c3a4 43 float th1Rel = th1;//-numTurns*2*M_PI;
amandaghassaei 8:1a3a69fecedf 44
amandaghassaei 8:1a3a69fecedf 45 return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1)
amandaghassaei 8:1a3a69fecedf 46 }
amandaghassaei 8:1a3a69fecedf 47
amandaghassaei 8:1a3a69fecedf 48 int fix(float val){//round toward zero
amandaghassaei 8:1a3a69fecedf 49 return val/1;
amandaghassaei 8:1a3a69fecedf 50 }
amandaghassaei 8:1a3a69fecedf 51
amandaghassaei 8:1a3a69fecedf 52 int signNonZero(float val){
amandaghassaei 8:1a3a69fecedf 53 if (val < 0) return -1;
amandaghassaei 8:1a3a69fecedf 54 return 1;
amandaghassaei 8:1a3a69fecedf 55 }