bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Kinmatics.cpp@10:769cc457c3a4, 2015-12-05 (annotated)
- Committer:
- amandaghassaei
- Date:
- Sat Dec 05 09:04:23 2015 +0000
- Revision:
- 10:769cc457c3a4
- Parent:
- 8:1a3a69fecedf
- Child:
- 11:711d3c207e8c
swinging looking good;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
amandaghassaei | 8:1a3a69fecedf | 1 | #include "Kinematics.h" |
amandaghassaei | 8:1a3a69fecedf | 2 | #include <math.h> |
amandaghassaei | 8:1a3a69fecedf | 3 | |
amandaghassaei | 8:1a3a69fecedf | 4 | |
amandaghassaei | 10:769cc457c3a4 | 5 | void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){ |
amandaghassaei | 10:769cc457c3a4 | 6 | float I1 = p[6]; |
amandaghassaei | 10:769cc457c3a4 | 7 | float I2 = p[7]; |
amandaghassaei | 10:769cc457c3a4 | 8 | float c1 = p[2]; |
amandaghassaei | 10:769cc457c3a4 | 9 | float c2 = p[3]; |
amandaghassaei | 10:769cc457c3a4 | 10 | float l1 = p[0]; |
amandaghassaei | 10:769cc457c3a4 | 11 | float m1 = p[4]; |
amandaghassaei | 10:769cc457c3a4 | 12 | float m2 = p[5]; |
amandaghassaei | 10:769cc457c3a4 | 13 | float th2 = z[1]; |
amandaghassaei | 10:769cc457c3a4 | 14 | float t2 = pow(c2,2); |
amandaghassaei | 10:769cc457c3a4 | 15 | float t3 = m2*t2; |
amandaghassaei | 10:769cc457c3a4 | 16 | float t4 = cos(th2); |
amandaghassaei | 10:769cc457c3a4 | 17 | float t5 = c2*l1*m2*t4; |
amandaghassaei | 10:769cc457c3a4 | 18 | float t6 = t3+t5; |
amandaghassaei | 10:769cc457c3a4 | 19 | A[0][0] = I1+I2+t3+pow(c1,2)*m1+pow(l1,2)*m2+c2*l1*m2*t4*2.0; |
amandaghassaei | 10:769cc457c3a4 | 20 | A[0][1] = t6; |
amandaghassaei | 10:769cc457c3a4 | 21 | A[1][0] = t6; |
amandaghassaei | 10:769cc457c3a4 | 22 | A[1][1] = I2+t3; |
amandaghassaei | 8:1a3a69fecedf | 23 | } |
amandaghassaei | 8:1a3a69fecedf | 24 | |
amandaghassaei | 8:1a3a69fecedf | 25 | float getPositionOfFoot(){ |
amandaghassaei | 8:1a3a69fecedf | 26 | return 0; |
amandaghassaei | 8:1a3a69fecedf | 27 | } |
amandaghassaei | 8:1a3a69fecedf | 28 | |
amandaghassaei | 10:769cc457c3a4 | 29 | float getEnergy(volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 30 | float I1 = p[6]; |
amandaghassaei | 8:1a3a69fecedf | 31 | float I2 = p[7]; |
amandaghassaei | 8:1a3a69fecedf | 32 | float c1 = p[2]; |
amandaghassaei | 8:1a3a69fecedf | 33 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 34 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 35 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 36 | float g = p[8]; |
amandaghassaei | 8:1a3a69fecedf | 37 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 38 | float m1 = p[4]; |
amandaghassaei | 8:1a3a69fecedf | 39 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 40 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 41 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 42 | float t2 = pow(dth1,2); |
amandaghassaei | 8:1a3a69fecedf | 43 | float t3 = pow(c2,2); |
amandaghassaei | 8:1a3a69fecedf | 44 | float t4 = pow(dth2,2); |
amandaghassaei | 8:1a3a69fecedf | 45 | float t5 = cos(th1); |
amandaghassaei | 8:1a3a69fecedf | 46 | float t6 = cos(th2); |
amandaghassaei | 8:1a3a69fecedf | 47 | 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; |
amandaghassaei | 8:1a3a69fecedf | 48 | } |
amandaghassaei | 8:1a3a69fecedf | 49 | |
amandaghassaei | 10:769cc457c3a4 | 50 | float getGravity(volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 51 | // c1 = p(3,:); |
amandaghassaei | 8:1a3a69fecedf | 52 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 53 | float g = p[8]; |
amandaghassaei | 8:1a3a69fecedf | 54 | // l1 = p(1,:); |
amandaghassaei | 8:1a3a69fecedf | 55 | // m1 = p(5,:); |
amandaghassaei | 8:1a3a69fecedf | 56 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 57 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 58 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 59 | // t2 = sin(th1); |
amandaghassaei | 8:1a3a69fecedf | 60 | float t3 = th1+th2; |
amandaghassaei | 8:1a3a69fecedf | 61 | float t4 = sin(t3); |
amandaghassaei | 8:1a3a69fecedf | 62 | return c2*g*m2*t4; |
amandaghassaei | 8:1a3a69fecedf | 63 | } |
amandaghassaei | 8:1a3a69fecedf | 64 | |
amandaghassaei | 10:769cc457c3a4 | 65 | float getCoriolis(volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 66 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 67 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 68 | // float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 69 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 70 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 71 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 72 | float t2 = sin(th2); |
amandaghassaei | 8:1a3a69fecedf | 73 | return c2*pow(dth1,2)*l1*m2*t2; |
amandaghassaei | 8:1a3a69fecedf | 74 | } |