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@8:1a3a69fecedf, 2015-12-03 (annotated)

Committer:
amandaghassaei
Date:
Thu Dec 03 23:55:44 2015 +0000
Revision:
8:1a3a69fecedf
Child:
10:769cc457c3a4
added in matlab controls;

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 8:1a3a69fecedf 5 void getMassMatrix(float A[2][2]){
amandaghassaei 8:1a3a69fecedf 6 A[0][0] = 0;
amandaghassaei 8:1a3a69fecedf 7 A[0][1] = 0;
amandaghassaei 8:1a3a69fecedf 8 A[1][0] = 0;
amandaghassaei 8:1a3a69fecedf 9 A[1][1] = 0;
amandaghassaei 8:1a3a69fecedf 10 }
amandaghassaei 8:1a3a69fecedf 11
amandaghassaei 8:1a3a69fecedf 12 float getPositionOfFoot(){
amandaghassaei 8:1a3a69fecedf 13 return 0;
amandaghassaei 8:1a3a69fecedf 14 }
amandaghassaei 8:1a3a69fecedf 15
amandaghassaei 8:1a3a69fecedf 16 float getEnergy(float z[4], float p[10]){
amandaghassaei 8:1a3a69fecedf 17 float I1 = p[6];
amandaghassaei 8:1a3a69fecedf 18 float I2 = p[7];
amandaghassaei 8:1a3a69fecedf 19 float c1 = p[2];
amandaghassaei 8:1a3a69fecedf 20 float c2 = p[3];
amandaghassaei 8:1a3a69fecedf 21 float dth1 = z[2];
amandaghassaei 8:1a3a69fecedf 22 float dth2 = z[3];
amandaghassaei 8:1a3a69fecedf 23 float g = p[8];
amandaghassaei 8:1a3a69fecedf 24 float l1 = p[0];
amandaghassaei 8:1a3a69fecedf 25 float m1 = p[4];
amandaghassaei 8:1a3a69fecedf 26 float m2 = p[5];
amandaghassaei 8:1a3a69fecedf 27 float th1 = z[0];
amandaghassaei 8:1a3a69fecedf 28 float th2 = z[1];
amandaghassaei 8:1a3a69fecedf 29 float t2 = pow(dth1,2);
amandaghassaei 8:1a3a69fecedf 30 float t3 = pow(c2,2);
amandaghassaei 8:1a3a69fecedf 31 float t4 = pow(dth2,2);
amandaghassaei 8:1a3a69fecedf 32 float t5 = cos(th1);
amandaghassaei 8:1a3a69fecedf 33 float t6 = cos(th2);
amandaghassaei 8:1a3a69fecedf 34 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 35 }
amandaghassaei 8:1a3a69fecedf 36
amandaghassaei 8:1a3a69fecedf 37 float getGravity(float z[4], float p[10]){
amandaghassaei 8:1a3a69fecedf 38 // c1 = p(3,:);
amandaghassaei 8:1a3a69fecedf 39 float c2 = p[3];
amandaghassaei 8:1a3a69fecedf 40 float g = p[8];
amandaghassaei 8:1a3a69fecedf 41 // l1 = p(1,:);
amandaghassaei 8:1a3a69fecedf 42 // m1 = p(5,:);
amandaghassaei 8:1a3a69fecedf 43 float m2 = p[5];
amandaghassaei 8:1a3a69fecedf 44 float th1 = z[0];
amandaghassaei 8:1a3a69fecedf 45 float th2 = z[1];
amandaghassaei 8:1a3a69fecedf 46 // t2 = sin(th1);
amandaghassaei 8:1a3a69fecedf 47 float t3 = th1+th2;
amandaghassaei 8:1a3a69fecedf 48 float t4 = sin(t3);
amandaghassaei 8:1a3a69fecedf 49 return c2*g*m2*t4;
amandaghassaei 8:1a3a69fecedf 50 }
amandaghassaei 8:1a3a69fecedf 51
amandaghassaei 8:1a3a69fecedf 52 float getCoriolis(float z[4], float p[10]){
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 l1 = p[0];
amandaghassaei 8:1a3a69fecedf 57 float m2 = p[5];
amandaghassaei 8:1a3a69fecedf 58 float th2 = z[1];
amandaghassaei 8:1a3a69fecedf 59 float t2 = sin(th2);
amandaghassaei 8:1a3a69fecedf 60 return c2*pow(dth1,2)*l1*m2*t2;
amandaghassaei 8:1a3a69fecedf 61 }