bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Controls/Controls.h@19:270735e44c98, 2015-12-11 (annotated)
- Committer:
- amandaghassaei
- Date:
- Fri Dec 11 08:53:08 2015 +0000
- Revision:
- 19:270735e44c98
- Parent:
- 18:0cfe72d8a006
obstacle debugging
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 | 10:769cc457c3a4 | 11 | #define controlTimerPeriod 0.001 |
amandaghassaei | 10:769cc457c3a4 | 12 | |
amandaghassaei | 8:1a3a69fecedf | 13 | class Controls: public CommDelegate{ |
amandaghassaei | 7:7efcd3bf3302 | 14 | |
amandaghassaei | 7:7efcd3bf3302 | 15 | public: |
amandaghassaei | 7:7efcd3bf3302 | 16 | |
amandaghassaei | 9:1d9b24d7ac77 | 17 | Controls():myMPU6050_1(p28, p27, p18), myMPU6050_2(p9, p10, p11){//I2C_SDA, I2C_SCL, int_pin |
amandaghassaei | 10:769cc457c3a4 | 18 | } |
amandaghassaei | 8:1a3a69fecedf | 19 | |
amandaghassaei | 10:769cc457c3a4 | 20 | void setup(){ |
amandaghassaei | 9:1d9b24d7ac77 | 21 | setInverted(false); |
amandaghassaei | 8:1a3a69fecedf | 22 | |
amandaghassaei | 11:711d3c207e8c | 23 | float m1 = 0.93159230; |
amandaghassaei | 11:711d3c207e8c | 24 | float m2 = 0.45433433; |
amandaghassaei | 11:711d3c207e8c | 25 | float l1 = 0.275;//length of links |
amandaghassaei | 8:1a3a69fecedf | 26 | float l2 = l1; |
amandaghassaei | 11:711d3c207e8c | 27 | float I1 = 0.03736067; |
amandaghassaei | 11:711d3c207e8c | 28 | float I2 = 0.01778165; |
amandaghassaei | 11:711d3c207e8c | 29 | float c1 = l1-0.08567346;//location of center of mass along link |
amandaghassaei | 11:711d3c207e8c | 30 | float c2 = 0.17594269; |
amandaghassaei | 8:1a3a69fecedf | 31 | float g = 9.81; |
amandaghassaei | 8:1a3a69fecedf | 32 | float lattice_pitch = 0.35; |
amandaghassaei | 8:1a3a69fecedf | 33 | _parameters[0] = l1; |
amandaghassaei | 8:1a3a69fecedf | 34 | _parameters[1] = l2; |
amandaghassaei | 8:1a3a69fecedf | 35 | _parameters[2] = c1; |
amandaghassaei | 8:1a3a69fecedf | 36 | _parameters[3] = c2; |
amandaghassaei | 8:1a3a69fecedf | 37 | _parameters[4] = m1; |
amandaghassaei | 8:1a3a69fecedf | 38 | _parameters[5] = m2; |
amandaghassaei | 8:1a3a69fecedf | 39 | _parameters[6] = I1; |
amandaghassaei | 8:1a3a69fecedf | 40 | _parameters[7] = I2; |
amandaghassaei | 8:1a3a69fecedf | 41 | _parameters[8] = g; |
amandaghassaei | 8:1a3a69fecedf | 42 | _parameters[9] = lattice_pitch; |
amandaghassaei | 8:1a3a69fecedf | 43 | |
amandaghassaei | 11:711d3c207e8c | 44 | _manualTheta = 0; |
amandaghassaei | 12:49813131dd15 | 45 | |
amandaghassaei | 15:d88f10b3b5f8 | 46 | setTargetPosition(6);//only 4 and 6 for now |
amandaghassaei | 7:7efcd3bf3302 | 47 | } |
amandaghassaei | 9:1d9b24d7ac77 | 48 | |
amandaghassaei | 9:1d9b24d7ac77 | 49 | void setInverted(bool inverted){ |
amandaghassaei | 9:1d9b24d7ac77 | 50 | _inverted = inverted; |
amandaghassaei | 9:1d9b24d7ac77 | 51 | if (!_inverted) { |
amandaghassaei | 9:1d9b24d7ac77 | 52 | myMPU6050_2.disable(); |
amandaghassaei | 9:1d9b24d7ac77 | 53 | myMPU6050_1.enable(); |
amandaghassaei | 18:0cfe72d8a006 | 54 | // motor.setGearRatio(10.164); |
amandaghassaei | 9:1d9b24d7ac77 | 55 | } else { |
amandaghassaei | 9:1d9b24d7ac77 | 56 | myMPU6050_1.disable(); |
amandaghassaei | 9:1d9b24d7ac77 | 57 | myMPU6050_2.enable(); |
amandaghassaei | 18:0cfe72d8a006 | 58 | // motor.setGearRatio(11.164); |
amandaghassaei | 9:1d9b24d7ac77 | 59 | } |
amandaghassaei | 9:1d9b24d7ac77 | 60 | } |
amandaghassaei | 9:1d9b24d7ac77 | 61 | |
amandaghassaei | 9:1d9b24d7ac77 | 62 | void setPC(Serial *pc){ |
amandaghassaei | 9:1d9b24d7ac77 | 63 | _pc = pc; |
amandaghassaei | 9:1d9b24d7ac77 | 64 | motor.setPC(pc); |
amandaghassaei | 9:1d9b24d7ac77 | 65 | gains.setPC(pc); |
amandaghassaei | 9:1d9b24d7ac77 | 66 | target.setPC(pc); |
amandaghassaei | 9:1d9b24d7ac77 | 67 | myMPU6050_1.setPC(pc); |
amandaghassaei | 9:1d9b24d7ac77 | 68 | myMPU6050_2.setPC(pc); |
amandaghassaei | 9:1d9b24d7ac77 | 69 | } |
amandaghassaei | 7:7efcd3bf3302 | 70 | |
amandaghassaei | 7:7efcd3bf3302 | 71 | Gains gains; |
amandaghassaei | 9:1d9b24d7ac77 | 72 | void setSwingUpK(float k){ |
amandaghassaei | 9:1d9b24d7ac77 | 73 | gains.setSwingUpK(k); |
amandaghassaei | 9:1d9b24d7ac77 | 74 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 75 | void setSwingUpD(float d){ |
amandaghassaei | 9:1d9b24d7ac77 | 76 | gains.setSwingUpD(d); |
amandaghassaei | 9:1d9b24d7ac77 | 77 | }; |
amandaghassaei | 14:d620415259b1 | 78 | void setDesiredThetaP(float p){ |
amandaghassaei | 14:d620415259b1 | 79 | gains.setDesiredThetaP(p); |
amandaghassaei | 9:1d9b24d7ac77 | 80 | }; |
amandaghassaei | 19:270735e44c98 | 81 | void setSoftLimitsP(float p){ |
amandaghassaei | 19:270735e44c98 | 82 | gains.setSoftLimitsP(p); |
amandaghassaei | 19:270735e44c98 | 83 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 84 | float getSwingUpK(){ |
amandaghassaei | 9:1d9b24d7ac77 | 85 | return gains.getSwingUpK(); |
amandaghassaei | 9:1d9b24d7ac77 | 86 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 87 | float getSwingUpD(){ |
amandaghassaei | 9:1d9b24d7ac77 | 88 | return gains.getSwingUpD(); |
amandaghassaei | 9:1d9b24d7ac77 | 89 | }; |
amandaghassaei | 14:d620415259b1 | 90 | float getDesiredThetaP(){ |
amandaghassaei | 14:d620415259b1 | 91 | return gains.getDesiredThetaP(); |
amandaghassaei | 9:1d9b24d7ac77 | 92 | }; |
amandaghassaei | 19:270735e44c98 | 93 | float getSoftLimitsP(){ |
amandaghassaei | 19:270735e44c98 | 94 | return gains.getSoftLimitsP(); |
amandaghassaei | 19:270735e44c98 | 95 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 96 | |
amandaghassaei | 7:7efcd3bf3302 | 97 | Target target; |
amandaghassaei | 9:1d9b24d7ac77 | 98 | void setTargetPosition(int position){ |
amandaghassaei | 12:49813131dd15 | 99 | target.setPosition(position, _parameters); |
amandaghassaei | 9:1d9b24d7ac77 | 100 | }; |
amandaghassaei | 9:1d9b24d7ac77 | 101 | int getTargetPosition(){ |
amandaghassaei | 9:1d9b24d7ac77 | 102 | return target.getPosition(); |
amandaghassaei | 9:1d9b24d7ac77 | 103 | }; |
amandaghassaei | 8:1a3a69fecedf | 104 | |
amandaghassaei | 17:8a0e647cf551 | 105 | void setTargetingK(float k){ |
amandaghassaei | 17:8a0e647cf551 | 106 | gains.setTargetingK(k); |
amandaghassaei | 17:8a0e647cf551 | 107 | }; |
amandaghassaei | 17:8a0e647cf551 | 108 | void setTargetingD(float d){ |
amandaghassaei | 17:8a0e647cf551 | 109 | gains.setTargetingD(d); |
amandaghassaei | 17:8a0e647cf551 | 110 | }; |
amandaghassaei | 17:8a0e647cf551 | 111 | float getTargetingK(){ |
amandaghassaei | 17:8a0e647cf551 | 112 | return gains.getTargetingK(); |
amandaghassaei | 17:8a0e647cf551 | 113 | }; |
amandaghassaei | 17:8a0e647cf551 | 114 | float getTargetingD(){ |
amandaghassaei | 17:8a0e647cf551 | 115 | return gains.getTargetingD(); |
amandaghassaei | 17:8a0e647cf551 | 116 | }; |
amandaghassaei | 17:8a0e647cf551 | 117 | |
amandaghassaei | 8:1a3a69fecedf | 118 | Motor motor; |
amandaghassaei | 11:711d3c207e8c | 119 | void setTheta(float theta){ |
amandaghassaei | 11:711d3c207e8c | 120 | _manualTheta = theta; |
amandaghassaei | 9:1d9b24d7ac77 | 121 | }; |
amandaghassaei | 10:769cc457c3a4 | 122 | |
amandaghassaei | 11:711d3c207e8c | 123 | float _manualTheta; |
amandaghassaei | 7:7efcd3bf3302 | 124 | |
amandaghassaei | 8:1a3a69fecedf | 125 | //imu |
amandaghassaei | 8:1a3a69fecedf | 126 | MyMPU6050 myMPU6050_1; |
amandaghassaei | 8:1a3a69fecedf | 127 | MyMPU6050 myMPU6050_2; |
amandaghassaei | 10:769cc457c3a4 | 128 | |
amandaghassaei | 10:769cc457c3a4 | 129 | void updateIMUS(){ |
amandaghassaei | 10:769cc457c3a4 | 130 | getActiveIMU()->loop(); |
amandaghassaei | 10:769cc457c3a4 | 131 | } |
amandaghassaei | 7:7efcd3bf3302 | 132 | |
amandaghassaei | 7:7efcd3bf3302 | 133 | void loop(){ |
amandaghassaei | 10:769cc457c3a4 | 134 | |
amandaghassaei | 13:64d337c5114e | 135 | getActiveIMU()->disableInterrupt(); |
amandaghassaei | 18:0cfe72d8a006 | 136 | updateThetas(); |
amandaghassaei | 18:0cfe72d8a006 | 137 | float tau = calcTau(_z, _parameters, &gains, &target, _pc); |
amandaghassaei | 13:64d337c5114e | 138 | |
amandaghassaei | 18:0cfe72d8a006 | 139 | // float K = gains.getSwingUpK(); |
amandaghassaei | 18:0cfe72d8a006 | 140 | // float D = gains.getSwingUpD(); |
amandaghassaei | 18:0cfe72d8a006 | 141 | // |
amandaghassaei | 18:0cfe72d8a006 | 142 | // float th1 = _z[0]; |
amandaghassaei | 18:0cfe72d8a006 | 143 | // float th2 = _z[1]; |
amandaghassaei | 18:0cfe72d8a006 | 144 | // float dth1 = _z[2]; |
amandaghassaei | 18:0cfe72d8a006 | 145 | // float dth2 = _z[3]; |
amandaghassaei | 18:0cfe72d8a006 | 146 | // float tau = (K*(_manualTheta - th2) - D*dth2); |
amandaghassaei | 18:0cfe72d8a006 | 147 | |
amandaghassaei | 9:1d9b24d7ac77 | 148 | motor.setTorque(tau); |
amandaghassaei | 10:769cc457c3a4 | 149 | |
amandaghassaei | 13:64d337c5114e | 150 | getActiveIMU()->enableInterrupt(); |
amandaghassaei | 10:769cc457c3a4 | 151 | } |
amandaghassaei | 10:769cc457c3a4 | 152 | |
amandaghassaei | 10:769cc457c3a4 | 153 | MyMPU6050* getActiveIMU(){ |
amandaghassaei | 10:769cc457c3a4 | 154 | if (_inverted) return &myMPU6050_2; |
amandaghassaei | 10:769cc457c3a4 | 155 | return &myMPU6050_1; |
amandaghassaei | 10:769cc457c3a4 | 156 | } |
amandaghassaei | 10:769cc457c3a4 | 157 | |
amandaghassaei | 8:1a3a69fecedf | 158 | float getTheta1(){ |
amandaghassaei | 8:1a3a69fecedf | 159 | return _z[0]; |
amandaghassaei | 7:7efcd3bf3302 | 160 | } |
amandaghassaei | 8:1a3a69fecedf | 161 | float getDTheta1(){ |
amandaghassaei | 8:1a3a69fecedf | 162 | return _z[2]; |
amandaghassaei | 8:1a3a69fecedf | 163 | } |
amandaghassaei | 8:1a3a69fecedf | 164 | float getTheta2(){ |
amandaghassaei | 8:1a3a69fecedf | 165 | return _z[1]; |
amandaghassaei | 8:1a3a69fecedf | 166 | } |
amandaghassaei | 8:1a3a69fecedf | 167 | float getDTheta2(){ |
amandaghassaei | 8:1a3a69fecedf | 168 | return _z[3]; |
amandaghassaei | 8:1a3a69fecedf | 169 | } |
amandaghassaei | 8:1a3a69fecedf | 170 | |
amandaghassaei | 8:1a3a69fecedf | 171 | |
amandaghassaei | 7:7efcd3bf3302 | 172 | |
amandaghassaei | 7:7efcd3bf3302 | 173 | private: |
amandaghassaei | 7:7efcd3bf3302 | 174 | |
amandaghassaei | 9:1d9b24d7ac77 | 175 | Serial *_pc; |
amandaghassaei | 9:1d9b24d7ac77 | 176 | |
amandaghassaei | 8:1a3a69fecedf | 177 | float _parameters[10]; |
amandaghassaei | 10:769cc457c3a4 | 178 | volatile float _z[4];//theta1, theta2, dtheta2, dtheta2 |
amandaghassaei | 14:d620415259b1 | 179 | |
amandaghassaei | 8:1a3a69fecedf | 180 | void updateThetas(){ |
amandaghassaei | 8:1a3a69fecedf | 181 | _z[0] = _getTheta1(); |
amandaghassaei | 8:1a3a69fecedf | 182 | _z[2] = _getDTheta1(); |
amandaghassaei | 8:1a3a69fecedf | 183 | _z[1] = _getTheta2(); |
amandaghassaei | 8:1a3a69fecedf | 184 | _z[3] = _getDTheta2(); |
amandaghassaei | 8:1a3a69fecedf | 185 | |
amandaghassaei | 8:1a3a69fecedf | 186 | } |
amandaghassaei | 8:1a3a69fecedf | 187 | float _getTheta1(){ |
amandaghassaei | 10:769cc457c3a4 | 188 | return getActiveIMU()->getTheta(); |
amandaghassaei | 8:1a3a69fecedf | 189 | } |
amandaghassaei | 8:1a3a69fecedf | 190 | float _getDTheta1(){ |
amandaghassaei | 10:769cc457c3a4 | 191 | return getActiveIMU()->getDTheta(); |
amandaghassaei | 8:1a3a69fecedf | 192 | } |
amandaghassaei | 8:1a3a69fecedf | 193 | float _getTheta2(){ |
amandaghassaei | 8:1a3a69fecedf | 194 | if (_inverted) return -motor.getTheta(); |
amandaghassaei | 8:1a3a69fecedf | 195 | return motor.getTheta(); |
amandaghassaei | 8:1a3a69fecedf | 196 | } |
amandaghassaei | 8:1a3a69fecedf | 197 | float _getDTheta2(){ |
amandaghassaei | 8:1a3a69fecedf | 198 | if (_inverted) return -motor.getDTheta(); |
amandaghassaei | 8:1a3a69fecedf | 199 | return motor.getDTheta(); |
amandaghassaei | 8:1a3a69fecedf | 200 | } |
amandaghassaei | 8:1a3a69fecedf | 201 | |
amandaghassaei | 8:1a3a69fecedf | 202 | bool _inverted; |
amandaghassaei | 7:7efcd3bf3302 | 203 | |
amandaghassaei | 7:7efcd3bf3302 | 204 | }; |
amandaghassaei | 7:7efcd3bf3302 | 205 | |
amandaghassaei | 7:7efcd3bf3302 | 206 | #endif |