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

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