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

Committer:
amandaghassaei
Date:
Thu Dec 03 23:55:44 2015 +0000
Revision:
8:1a3a69fecedf
Parent:
7:7efcd3bf3302
Child:
9:1d9b24d7ac77
added in matlab controls;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amandaghassaei 7:7efcd3bf3302 1 #ifndef Controls_h
amandaghassaei 7:7efcd3bf3302 2 #define Controls_h
amandaghassaei 7:7efcd3bf3302 3
amandaghassaei 8:1a3a69fecedf 4 #include <math.h>
amandaghassaei 7:7efcd3bf3302 5 #include "Gains.h"
amandaghassaei 7:7efcd3bf3302 6 #include "Target.h"
amandaghassaei 8:1a3a69fecedf 7 #include "MyMPU6050.h"
amandaghassaei 8:1a3a69fecedf 8 #include "Motor.h"
amandaghassaei 8:1a3a69fecedf 9 #include "Dynamics.h"
amandaghassaei 7:7efcd3bf3302 10
amandaghassaei 8:1a3a69fecedf 11 class Controls: public CommDelegate{
amandaghassaei 7:7efcd3bf3302 12
amandaghassaei 7:7efcd3bf3302 13 public:
amandaghassaei 7:7efcd3bf3302 14
amandaghassaei 8:1a3a69fecedf 15 Controls():myMPU6050_1(p9, p10, p11), myMPU6050_2(p28, p27, p18){//I2C_SDA, I2C_SCL, int_pin
amandaghassaei 8:1a3a69fecedf 16
amandaghassaei 8:1a3a69fecedf 17 _inverted = false;
amandaghassaei 8:1a3a69fecedf 18
amandaghassaei 8:1a3a69fecedf 19 float m1 = 0.55;
amandaghassaei 8:1a3a69fecedf 20 float m2 = m1;
amandaghassaei 8:1a3a69fecedf 21 float l1 = 0.30;//length of links
amandaghassaei 8:1a3a69fecedf 22 float l2 = l1;
amandaghassaei 8:1a3a69fecedf 23 float I1 = 1/3*m1*pow(l1,2);//model as rod rotating around one end
amandaghassaei 8:1a3a69fecedf 24 float I2 = 1/3*m2*pow(l2,2);
amandaghassaei 8:1a3a69fecedf 25 float c1 = 0.5*l1;//location of center of mass along link
amandaghassaei 8:1a3a69fecedf 26 float c2 = 0.5*l2;
amandaghassaei 8:1a3a69fecedf 27 float g = 9.81;
amandaghassaei 8:1a3a69fecedf 28 float lattice_pitch = 0.35;
amandaghassaei 8:1a3a69fecedf 29 _parameters[0] = l1;
amandaghassaei 8:1a3a69fecedf 30 _parameters[1] = l2;
amandaghassaei 8:1a3a69fecedf 31 _parameters[2] = c1;
amandaghassaei 8:1a3a69fecedf 32 _parameters[3] = c2;
amandaghassaei 8:1a3a69fecedf 33 _parameters[4] = m1;
amandaghassaei 8:1a3a69fecedf 34 _parameters[5] = m2;
amandaghassaei 8:1a3a69fecedf 35 _parameters[6] = I1;
amandaghassaei 8:1a3a69fecedf 36 _parameters[7] = I2;
amandaghassaei 8:1a3a69fecedf 37 _parameters[8] = g;
amandaghassaei 8:1a3a69fecedf 38 _parameters[9] = lattice_pitch;
amandaghassaei 8:1a3a69fecedf 39
amandaghassaei 7:7efcd3bf3302 40 }
amandaghassaei 7:7efcd3bf3302 41
amandaghassaei 7:7efcd3bf3302 42 Gains gains;
amandaghassaei 7:7efcd3bf3302 43 Target target;
amandaghassaei 8:1a3a69fecedf 44
amandaghassaei 8:1a3a69fecedf 45 Motor motor;
amandaghassaei 7:7efcd3bf3302 46
amandaghassaei 8:1a3a69fecedf 47 //imu
amandaghassaei 8:1a3a69fecedf 48 MyMPU6050 myMPU6050_1;
amandaghassaei 8:1a3a69fecedf 49 MyMPU6050 myMPU6050_2;
amandaghassaei 7:7efcd3bf3302 50
amandaghassaei 7:7efcd3bf3302 51 void loop(){
amandaghassaei 8:1a3a69fecedf 52 if (_inverted) myMPU6050_2.loop();
amandaghassaei 8:1a3a69fecedf 53 else myMPU6050_1.loop();
amandaghassaei 8:1a3a69fecedf 54 updateThetas();
amandaghassaei 8:1a3a69fecedf 55
amandaghassaei 8:1a3a69fecedf 56 float tau = calcTau(_z, _parameters);
amandaghassaei 8:1a3a69fecedf 57 }
amandaghassaei 8:1a3a69fecedf 58
amandaghassaei 8:1a3a69fecedf 59 float getTheta1(){
amandaghassaei 8:1a3a69fecedf 60 return _z[0];
amandaghassaei 7:7efcd3bf3302 61 }
amandaghassaei 8:1a3a69fecedf 62 float getDTheta1(){
amandaghassaei 8:1a3a69fecedf 63 return _z[2];
amandaghassaei 8:1a3a69fecedf 64 }
amandaghassaei 8:1a3a69fecedf 65 float getTheta2(){
amandaghassaei 8:1a3a69fecedf 66 return _z[1];
amandaghassaei 8:1a3a69fecedf 67 }
amandaghassaei 8:1a3a69fecedf 68 float getDTheta2(){
amandaghassaei 8:1a3a69fecedf 69 return _z[3];
amandaghassaei 8:1a3a69fecedf 70 }
amandaghassaei 8:1a3a69fecedf 71
amandaghassaei 8:1a3a69fecedf 72
amandaghassaei 7:7efcd3bf3302 73
amandaghassaei 7:7efcd3bf3302 74 private:
amandaghassaei 7:7efcd3bf3302 75
amandaghassaei 8:1a3a69fecedf 76 float _parameters[10];
amandaghassaei 7:7efcd3bf3302 77
amandaghassaei 8:1a3a69fecedf 78 float _z[4];//theta1, theta2, dtheta2, dtheta2
amandaghassaei 8:1a3a69fecedf 79
amandaghassaei 8:1a3a69fecedf 80 void updateThetas(){
amandaghassaei 8:1a3a69fecedf 81 _z[0] = _getTheta1();
amandaghassaei 8:1a3a69fecedf 82 _z[2] = _getDTheta1();
amandaghassaei 8:1a3a69fecedf 83 _z[1] = _getTheta2();
amandaghassaei 8:1a3a69fecedf 84 _z[3] = _getDTheta2();
amandaghassaei 8:1a3a69fecedf 85
amandaghassaei 8:1a3a69fecedf 86 }
amandaghassaei 8:1a3a69fecedf 87 float _getTheta1(){
amandaghassaei 8:1a3a69fecedf 88 if (_inverted) return myMPU6050_2.getTheta();
amandaghassaei 8:1a3a69fecedf 89 return myMPU6050_1.getTheta();
amandaghassaei 8:1a3a69fecedf 90 }
amandaghassaei 8:1a3a69fecedf 91 float _getDTheta1(){
amandaghassaei 8:1a3a69fecedf 92 if (_inverted) return myMPU6050_2.getDTheta();
amandaghassaei 8:1a3a69fecedf 93 return myMPU6050_1.getDTheta();
amandaghassaei 8:1a3a69fecedf 94 }
amandaghassaei 8:1a3a69fecedf 95 float _getTheta2(){
amandaghassaei 8:1a3a69fecedf 96 if (_inverted) return -motor.getTheta();
amandaghassaei 8:1a3a69fecedf 97 return motor.getTheta();
amandaghassaei 8:1a3a69fecedf 98 }
amandaghassaei 8:1a3a69fecedf 99 float _getDTheta2(){
amandaghassaei 8:1a3a69fecedf 100 if (_inverted) return -motor.getDTheta();
amandaghassaei 8:1a3a69fecedf 101 return motor.getDTheta();
amandaghassaei 8:1a3a69fecedf 102 }
amandaghassaei 8:1a3a69fecedf 103
amandaghassaei 8:1a3a69fecedf 104 bool _inverted;
amandaghassaei 7:7efcd3bf3302 105
amandaghassaei 7:7efcd3bf3302 106 };
amandaghassaei 7:7efcd3bf3302 107
amandaghassaei 7:7efcd3bf3302 108 #endif