bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Kinmatics.cpp@11:711d3c207e8c, 2015-12-08 (annotated)
- Committer:
- amandaghassaei
- Date:
- Tue Dec 08 22:52:46 2015 +0000
- Revision:
- 11:711d3c207e8c
- Parent:
- 10:769cc457c3a4
- Child:
- 16:5b19be27f08a
negative dth;
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 | 11:711d3c207e8c | 14 | float t2 = c2*c2; |
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 | 11:711d3c207e8c | 19 | A[0][0] = I1+I2+t3+c1*c1*m1+l1*l1*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 | 11:711d3c207e8c | 25 | void getGripperPosition(float position[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 26 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 27 | float l2 = p[1]; |
amandaghassaei | 11:711d3c207e8c | 28 | float th1 = z[0]; |
amandaghassaei | 11:711d3c207e8c | 29 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 30 | float t2 = th1+th2; |
amandaghassaei | 11:711d3c207e8c | 31 | position[0] = l2*sin(t2)+l1*sin(th1); |
amandaghassaei | 11:711d3c207e8c | 32 | position[1] = -l2*cos(t2)-l1*cos(th1); |
amandaghassaei | 11:711d3c207e8c | 33 | } |
amandaghassaei | 11:711d3c207e8c | 34 | |
amandaghassaei | 11:711d3c207e8c | 35 | void getGripperVelocity(float velocity[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 36 | float dth1 = z[2]; |
amandaghassaei | 11:711d3c207e8c | 37 | float dth2 = z[3]; |
amandaghassaei | 11:711d3c207e8c | 38 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 39 | float l2 = p[1]; |
amandaghassaei | 11:711d3c207e8c | 40 | float th1 = z[0]; |
amandaghassaei | 11:711d3c207e8c | 41 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 42 | float t2 = th1+th2; |
amandaghassaei | 11:711d3c207e8c | 43 | float t3 = cos(t2); |
amandaghassaei | 11:711d3c207e8c | 44 | float t4 = sin(t2); |
amandaghassaei | 11:711d3c207e8c | 45 | velocity[0] = dth1*(l2*t3+l1*cos(th1))+dth2*l2*t3; |
amandaghassaei | 11:711d3c207e8c | 46 | velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4; |
amandaghassaei | 8:1a3a69fecedf | 47 | } |
amandaghassaei | 8:1a3a69fecedf | 48 | |
amandaghassaei | 10:769cc457c3a4 | 49 | float getEnergy(volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 50 | float I1 = p[6]; |
amandaghassaei | 8:1a3a69fecedf | 51 | float I2 = p[7]; |
amandaghassaei | 8:1a3a69fecedf | 52 | float c1 = p[2]; |
amandaghassaei | 8:1a3a69fecedf | 53 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 54 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 55 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 56 | float g = p[8]; |
amandaghassaei | 8:1a3a69fecedf | 57 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 58 | float m1 = p[4]; |
amandaghassaei | 8:1a3a69fecedf | 59 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 60 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 61 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 62 | float t2 = dth1*dth1; |
amandaghassaei | 11:711d3c207e8c | 63 | float t3 = c2*c2; |
amandaghassaei | 11:711d3c207e8c | 64 | float t4 = dth2*dth2; |
amandaghassaei | 8:1a3a69fecedf | 65 | float t5 = cos(th1); |
amandaghassaei | 8:1a3a69fecedf | 66 | float t6 = cos(th2); |
amandaghassaei | 11:711d3c207e8c | 67 | return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+c1*c1*m1*t2*(1.0/2.0)+l1*l1*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 | 68 | } |
amandaghassaei | 8:1a3a69fecedf | 69 | |
amandaghassaei | 11:711d3c207e8c | 70 | void getGravity(float output[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 71 | float c1 = p[2]; |
amandaghassaei | 8:1a3a69fecedf | 72 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 73 | float g = p[8]; |
amandaghassaei | 11:711d3c207e8c | 74 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 75 | float m1 = p[4]; |
amandaghassaei | 8:1a3a69fecedf | 76 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 77 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 78 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 79 | float t2 = sin(th1); |
amandaghassaei | 8:1a3a69fecedf | 80 | float t3 = th1+th2; |
amandaghassaei | 8:1a3a69fecedf | 81 | float t4 = sin(t3); |
amandaghassaei | 11:711d3c207e8c | 82 | output[0] = g*m2*(c2*t4+l1*t2)+c1*g*m1*t2; |
amandaghassaei | 11:711d3c207e8c | 83 | output[1] = c2*g*m2*t4; |
amandaghassaei | 8:1a3a69fecedf | 84 | } |
amandaghassaei | 8:1a3a69fecedf | 85 | |
amandaghassaei | 11:711d3c207e8c | 86 | void getCoriolisCentrip(float output[2], volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 87 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 88 | float dth1 = z[2]; |
amandaghassaei | 11:711d3c207e8c | 89 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 90 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 91 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 92 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 93 | float t2 = sin(th2); |
amandaghassaei | 11:711d3c207e8c | 94 | output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2); |
amandaghassaei | 11:711d3c207e8c | 95 | output[1] = c2*dth1*dth1*l1*m2*t2; |
amandaghassaei | 8:1a3a69fecedf | 96 | } |