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@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?

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 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