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@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?

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