Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

bio robot

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Controls/Kinmatics.cpp@20:f13b949b623b, 2015-12-11 (annotated)

Committer:
amandaghassaei
Date:
Fri Dec 11 08:54:32 2015 +0000
Revision:
20:f13b949b623b
Parent:
16:5b19be27f08a 
publish

Who changed what in which revision?

UserRevisionLine numberNew 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 }