bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
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?
User | Revision | Line number | New 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 |