Amanda Ghassaei / Mbed 2 deprecated Bracky-MPU6050-DMP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050-DMP QEI_hw mbed-rpc mbed

Fork of MPU6050_Example by Shundo Kishi

Controls/Dynamics.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:
12:49813131dd15
negative dth;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 8:1a3a69fecedf 1 #include "Dynamics.h"
amandaghassaei 8:1a3a69fecedf 2 #include <math.h>
amandaghassaei 8:1a3a69fecedf 3 # define M_PI 3.14159265358979323846
amandaghassaei 8:1a3a69fecedf 4
amandaghassaei 10:769cc457c3a4 5 float calcTau(volatile float z[4], float p[10], Gains *gains, Serial *pc){
amandaghassaei 8:1a3a69fecedf 6
amandaghassaei 8:1a3a69fecedf 7 float th1 = z[0];
amandaghassaei 8:1a3a69fecedf 8 float th2 = z[1];
amandaghassaei 8:1a3a69fecedf 9 float dth1 = z[2];
amandaghassaei 8:1a3a69fecedf 10 float dth2 = z[3];
amandaghassaei 8:1a3a69fecedf 11
amandaghassaei 8:1a3a69fecedf 12 float E = getEnergy(z, p);
amandaghassaei 8:1a3a69fecedf 13
amandaghassaei 11:711d3c207e8c 14 float A[2][2];
amandaghassaei 11:711d3c207e8c 15 getMassMatrix(A, z, p);
amandaghassaei 11:711d3c207e8c 16 float AHat = A[1][1]-A[1][0]*A[0][1]/A[0][0];
amandaghassaei 11:711d3c207e8c 17
amandaghassaei 11:711d3c207e8c 18 float corrCentripComp[2];
amandaghassaei 11:711d3c207e8c 19 getCoriolisCentrip(corrCentripComp, z, p);
amandaghassaei 11:711d3c207e8c 20 float corrCentripCompHat = corrCentripComp[1]-A[1][0]*corrCentripComp[0]/A[0][0];
amandaghassaei 11:711d3c207e8c 21
amandaghassaei 11:711d3c207e8c 22 float gravityComp[2];
amandaghassaei 11:711d3c207e8c 23 getGravity(gravityComp, z, p);
amandaghassaei 11:711d3c207e8c 24 float gravityCompHat = gravityComp[1]-A[1][0]*gravityComp[0]/A[0][0];
amandaghassaei 11:711d3c207e8c 25
amandaghassaei 9:1d9b24d7ac77 26 float K = gains->getSwingUpK();
amandaghassaei 9:1d9b24d7ac77 27 float D = gains->getSwingUpD();
amandaghassaei 8:1a3a69fecedf 28
amandaghassaei 11:711d3c207e8c 29 float th2Des = thetaDesired(2.2, th1, th2, dth1, dth2);
amandaghassaei 8:1a3a69fecedf 30
amandaghassaei 11:711d3c207e8c 31 float ddth2 = K*(th2Des - th2) - D*dth2;
amandaghassaei 11:711d3c207e8c 32 float tau = gains->getCurrentP()*AHat*ddth2 + corrCentripCompHat + gravityCompHat;
amandaghassaei 8:1a3a69fecedf 33
amandaghassaei 11:711d3c207e8c 34 return tau;
amandaghassaei 8:1a3a69fecedf 35 }
amandaghassaei 8:1a3a69fecedf 36
amandaghassaei 8:1a3a69fecedf 37 float thetaDesired(float range, float th1, float th2, float dth1, float dth2){
amandaghassaei 8:1a3a69fecedf 38
amandaghassaei 11:711d3c207e8c 39 int numTurns = fix(th1/(2*M_PI));
amandaghassaei 11:711d3c207e8c 40 float th1Rel = th1-numTurns*2*M_PI;
amandaghassaei 8:1a3a69fecedf 41
amandaghassaei 8:1a3a69fecedf 42 return signNonZero(dth1)*(range-abs(th1Rel));//*cos(th1)
amandaghassaei 8:1a3a69fecedf 43 }
amandaghassaei 8:1a3a69fecedf 44
amandaghassaei 8:1a3a69fecedf 45 int fix(float val){//round toward zero
amandaghassaei 11:711d3c207e8c 46 return val > 0 ? floor(val) : ceil(val);
amandaghassaei 8:1a3a69fecedf 47 }
amandaghassaei 8:1a3a69fecedf 48
amandaghassaei 8:1a3a69fecedf 49 int signNonZero(float val){
amandaghassaei 8:1a3a69fecedf 50 if (val < 0) return -1;
amandaghassaei 8:1a3a69fecedf 51 return 1;
amandaghassaei 8:1a3a69fecedf 52 }