bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Kinmatics.cpp
- Revision:
- 8:1a3a69fecedf
- Child:
- 10:769cc457c3a4
diff -r 7efcd3bf3302 -r 1a3a69fecedf Controls/Kinmatics.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controls/Kinmatics.cpp Thu Dec 03 23:55:44 2015 +0000 @@ -0,0 +1,61 @@ +#include "Kinematics.h" +#include <math.h> + + +void getMassMatrix(float A[2][2]){ + A[0][0] = 0; + A[0][1] = 0; + A[1][0] = 0; + A[1][1] = 0; +} + +float getPositionOfFoot(){ + return 0; +} + +float getEnergy(float z[4], float p[10]){ + float I1 = p[6]; + float I2 = p[7]; + float c1 = p[2]; + float c2 = p[3]; + float dth1 = z[2]; + float dth2 = z[3]; + float g = p[8]; + float l1 = p[0]; + float m1 = p[4]; + float m2 = p[5]; + float th1 = z[0]; + float th2 = z[1]; + float t2 = pow(dth1,2); + float t3 = pow(c2,2); + float t4 = pow(dth2,2); + float t5 = cos(th1); + float t6 = cos(th2); + return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+pow(c1,2)*m1*t2*(1.0/2.0)+pow(l1,2)*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6; +} + +float getGravity(float z[4], float p[10]){ +// c1 = p(3,:); + float c2 = p[3]; + float g = p[8]; +// l1 = p(1,:); +// m1 = p(5,:); + float m2 = p[5]; + float th1 = z[0]; + float th2 = z[1]; +// t2 = sin(th1); + float t3 = th1+th2; + float t4 = sin(t3); + return c2*g*m2*t4; +} + +float getCoriolis(float z[4], float p[10]){ + float c2 = p[3]; + float dth1 = z[2]; +// float dth2 = z[3]; + float l1 = p[0]; + float m2 = p[5]; + float th2 = z[1]; + float t2 = sin(th2); + return c2*pow(dth1,2)*l1*m2*t2; +} \ No newline at end of file