bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Kinmatics.cpp@16:5b19be27f08a, 2015-12-10 (annotated)
- Committer:
- amandaghassaei
- Date:
- Thu Dec 10 10:55:35 2015 +0000
- Revision:
- 16:5b19be27f08a
- Parent:
- 11:711d3c207e8c
targeting code added;
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 | 16:5b19be27f08a | 3 | #define M_PI 3.14159265358979323846 |
amandaghassaei | 8:1a3a69fecedf | 4 | |
amandaghassaei | 8:1a3a69fecedf | 5 | |
amandaghassaei | 10:769cc457c3a4 | 6 | void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){ |
amandaghassaei | 10:769cc457c3a4 | 7 | float I1 = p[6]; |
amandaghassaei | 10:769cc457c3a4 | 8 | float I2 = p[7]; |
amandaghassaei | 10:769cc457c3a4 | 9 | float c1 = p[2]; |
amandaghassaei | 10:769cc457c3a4 | 10 | float c2 = p[3]; |
amandaghassaei | 10:769cc457c3a4 | 11 | float l1 = p[0]; |
amandaghassaei | 10:769cc457c3a4 | 12 | float m1 = p[4]; |
amandaghassaei | 10:769cc457c3a4 | 13 | float m2 = p[5]; |
amandaghassaei | 10:769cc457c3a4 | 14 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 15 | float t2 = c2*c2; |
amandaghassaei | 10:769cc457c3a4 | 16 | float t3 = m2*t2; |
amandaghassaei | 10:769cc457c3a4 | 17 | float t4 = cos(th2); |
amandaghassaei | 10:769cc457c3a4 | 18 | float t5 = c2*l1*m2*t4; |
amandaghassaei | 10:769cc457c3a4 | 19 | float t6 = t3+t5; |
amandaghassaei | 11:711d3c207e8c | 20 | A[0][0] = I1+I2+t3+c1*c1*m1+l1*l1*m2+c2*l1*m2*t4*2.0; |
amandaghassaei | 10:769cc457c3a4 | 21 | A[0][1] = t6; |
amandaghassaei | 10:769cc457c3a4 | 22 | A[1][0] = t6; |
amandaghassaei | 10:769cc457c3a4 | 23 | A[1][1] = I2+t3; |
amandaghassaei | 8:1a3a69fecedf | 24 | } |
amandaghassaei | 8:1a3a69fecedf | 25 | |
amandaghassaei | 11:711d3c207e8c | 26 | void getGripperPosition(float position[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 27 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 28 | float l2 = p[1]; |
amandaghassaei | 11:711d3c207e8c | 29 | float th1 = z[0]; |
amandaghassaei | 11:711d3c207e8c | 30 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 31 | float t2 = th1+th2; |
amandaghassaei | 11:711d3c207e8c | 32 | position[0] = l2*sin(t2)+l1*sin(th1); |
amandaghassaei | 11:711d3c207e8c | 33 | position[1] = -l2*cos(t2)-l1*cos(th1); |
amandaghassaei | 11:711d3c207e8c | 34 | } |
amandaghassaei | 11:711d3c207e8c | 35 | |
amandaghassaei | 11:711d3c207e8c | 36 | void getGripperVelocity(float velocity[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 37 | float dth1 = z[2]; |
amandaghassaei | 11:711d3c207e8c | 38 | float dth2 = z[3]; |
amandaghassaei | 11:711d3c207e8c | 39 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 40 | float l2 = p[1]; |
amandaghassaei | 11:711d3c207e8c | 41 | float th1 = z[0]; |
amandaghassaei | 11:711d3c207e8c | 42 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 43 | float t2 = th1+th2; |
amandaghassaei | 11:711d3c207e8c | 44 | float t3 = cos(t2); |
amandaghassaei | 11:711d3c207e8c | 45 | float t4 = sin(t2); |
amandaghassaei | 11:711d3c207e8c | 46 | velocity[0] = dth1*(l2*t3+l1*cos(th1))+dth2*l2*t3; |
amandaghassaei | 11:711d3c207e8c | 47 | velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4; |
amandaghassaei | 8:1a3a69fecedf | 48 | } |
amandaghassaei | 8:1a3a69fecedf | 49 | |
amandaghassaei | 16:5b19be27f08a | 50 | void getGripperJacobianTranspose(float Jtrans[2][2], volatile float z[4], float p[10]){ |
amandaghassaei | 16:5b19be27f08a | 51 | float l1 = p[0]; |
amandaghassaei | 16:5b19be27f08a | 52 | float l2 = p[1]; |
amandaghassaei | 16:5b19be27f08a | 53 | float th1 = z[0]; |
amandaghassaei | 16:5b19be27f08a | 54 | float th2 = z[1]; |
amandaghassaei | 16:5b19be27f08a | 55 | float t2 = th1+th2; |
amandaghassaei | 16:5b19be27f08a | 56 | float t3 = cos(t2); |
amandaghassaei | 16:5b19be27f08a | 57 | float t4 = l2*t3; |
amandaghassaei | 16:5b19be27f08a | 58 | float t5 = sin(t2); |
amandaghassaei | 16:5b19be27f08a | 59 | float t6 = l2*t5; |
amandaghassaei | 16:5b19be27f08a | 60 | Jtrans[0][0] = t4+l1*cos(th1); |
amandaghassaei | 16:5b19be27f08a | 61 | Jtrans[0][1] = t4; |
amandaghassaei | 16:5b19be27f08a | 62 | Jtrans[1][0] = t6+l1*sin(th1); |
amandaghassaei | 16:5b19be27f08a | 63 | Jtrans[1][1] = t6; |
amandaghassaei | 16:5b19be27f08a | 64 | } |
amandaghassaei | 16:5b19be27f08a | 65 | |
amandaghassaei | 10:769cc457c3a4 | 66 | float getEnergy(volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 67 | float I1 = p[6]; |
amandaghassaei | 8:1a3a69fecedf | 68 | float I2 = p[7]; |
amandaghassaei | 8:1a3a69fecedf | 69 | float c1 = p[2]; |
amandaghassaei | 8:1a3a69fecedf | 70 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 71 | float dth1 = z[2]; |
amandaghassaei | 8:1a3a69fecedf | 72 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 73 | float g = p[8]; |
amandaghassaei | 8:1a3a69fecedf | 74 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 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 = dth1*dth1; |
amandaghassaei | 11:711d3c207e8c | 80 | float t3 = c2*c2; |
amandaghassaei | 11:711d3c207e8c | 81 | float t4 = dth2*dth2; |
amandaghassaei | 8:1a3a69fecedf | 82 | float t5 = cos(th1); |
amandaghassaei | 8:1a3a69fecedf | 83 | float t6 = cos(th2); |
amandaghassaei | 11:711d3c207e8c | 84 | 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 | 85 | } |
amandaghassaei | 8:1a3a69fecedf | 86 | |
amandaghassaei | 11:711d3c207e8c | 87 | void getGravity(float output[2], volatile float z[4], float p[10]){ |
amandaghassaei | 11:711d3c207e8c | 88 | float c1 = p[2]; |
amandaghassaei | 8:1a3a69fecedf | 89 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 90 | float g = p[8]; |
amandaghassaei | 11:711d3c207e8c | 91 | float l1 = p[0]; |
amandaghassaei | 11:711d3c207e8c | 92 | float m1 = p[4]; |
amandaghassaei | 8:1a3a69fecedf | 93 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 94 | float th1 = z[0]; |
amandaghassaei | 8:1a3a69fecedf | 95 | float th2 = z[1]; |
amandaghassaei | 11:711d3c207e8c | 96 | float t2 = sin(th1); |
amandaghassaei | 8:1a3a69fecedf | 97 | float t3 = th1+th2; |
amandaghassaei | 8:1a3a69fecedf | 98 | float t4 = sin(t3); |
amandaghassaei | 11:711d3c207e8c | 99 | output[0] = g*m2*(c2*t4+l1*t2)+c1*g*m1*t2; |
amandaghassaei | 11:711d3c207e8c | 100 | output[1] = c2*g*m2*t4; |
amandaghassaei | 8:1a3a69fecedf | 101 | } |
amandaghassaei | 8:1a3a69fecedf | 102 | |
amandaghassaei | 11:711d3c207e8c | 103 | void getCoriolisCentrip(float output[2], volatile float z[4], float p[10]){ |
amandaghassaei | 8:1a3a69fecedf | 104 | float c2 = p[3]; |
amandaghassaei | 8:1a3a69fecedf | 105 | float dth1 = z[2]; |
amandaghassaei | 11:711d3c207e8c | 106 | float dth2 = z[3]; |
amandaghassaei | 8:1a3a69fecedf | 107 | float l1 = p[0]; |
amandaghassaei | 8:1a3a69fecedf | 108 | float m2 = p[5]; |
amandaghassaei | 8:1a3a69fecedf | 109 | float th2 = z[1]; |
amandaghassaei | 8:1a3a69fecedf | 110 | float t2 = sin(th2); |
amandaghassaei | 11:711d3c207e8c | 111 | output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2); |
amandaghassaei | 11:711d3c207e8c | 112 | output[1] = c2*dth1*dth1*l1*m2*t2; |
amandaghassaei | 16:5b19be27f08a | 113 | } |
amandaghassaei | 16:5b19be27f08a | 114 | |
amandaghassaei | 16:5b19be27f08a | 115 | float boundTheta(float theta){ |
amandaghassaei | 16:5b19be27f08a | 116 | int numTurns = fix(theta/(2*M_PI)); |
amandaghassaei | 16:5b19be27f08a | 117 | return theta-numTurns*2*M_PI; |
amandaghassaei | 16:5b19be27f08a | 118 | } |
amandaghassaei | 16:5b19be27f08a | 119 | |
amandaghassaei | 16:5b19be27f08a | 120 | int fix(float val){//round toward zero |
amandaghassaei | 16:5b19be27f08a | 121 | return val > 0 ? floor(val) : ceil(val); |
amandaghassaei | 8:1a3a69fecedf | 122 | } |