bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Kinmatics.cpp
- Revision:
- 10:769cc457c3a4
- Parent:
- 8:1a3a69fecedf
- Child:
- 11:711d3c207e8c
diff -r 1d9b24d7ac77 -r 769cc457c3a4 Controls/Kinmatics.cpp --- a/Controls/Kinmatics.cpp Sat Dec 05 00:40:42 2015 +0000 +++ b/Controls/Kinmatics.cpp Sat Dec 05 09:04:23 2015 +0000 @@ -2,18 +2,31 @@ #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; +void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){ + float I1 = p[6]; + float I2 = p[7]; + float c1 = p[2]; + float c2 = p[3]; + float l1 = p[0]; + float m1 = p[4]; + float m2 = p[5]; + float th2 = z[1]; + float t2 = pow(c2,2); + float t3 = m2*t2; + float t4 = cos(th2); + float t5 = c2*l1*m2*t4; + float t6 = t3+t5; + A[0][0] = I1+I2+t3+pow(c1,2)*m1+pow(l1,2)*m2+c2*l1*m2*t4*2.0; + A[0][1] = t6; + A[1][0] = t6; + A[1][1] = I2+t3; } float getPositionOfFoot(){ return 0; } -float getEnergy(float z[4], float p[10]){ +float getEnergy(volatile float z[4], float p[10]){ float I1 = p[6]; float I2 = p[7]; float c1 = p[2]; @@ -34,7 +47,7 @@ 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]){ +float getGravity(volatile float z[4], float p[10]){ // c1 = p(3,:); float c2 = p[3]; float g = p[8]; @@ -49,7 +62,7 @@ return c2*g*m2*t4; } -float getCoriolis(float z[4], float p[10]){ +float getCoriolis(volatile float z[4], float p[10]){ float c2 = p[3]; float dth1 = z[2]; // float dth2 = z[3];