bio robot
Dependencies: MPU6050-DMP QEI_hw mbed-rpc mbed
Fork of MPU6050_Example by
Diff: Controls/Controls.h
- Revision:
- 10:769cc457c3a4
- Parent:
- 9:1d9b24d7ac77
- Child:
- 11:711d3c207e8c
diff -r 1d9b24d7ac77 -r 769cc457c3a4 Controls/Controls.h --- a/Controls/Controls.h Sat Dec 05 00:40:42 2015 +0000 +++ b/Controls/Controls.h Sat Dec 05 09:04:23 2015 +0000 @@ -8,20 +8,24 @@ #include "Motor.h" #include "Dynamics.h" +#define controlTimerPeriod 0.001 + class Controls: public CommDelegate{ public: Controls():myMPU6050_1(p28, p27, p18), myMPU6050_2(p9, p10, p11){//I2C_SDA, I2C_SCL, int_pin + } + void setup(){ setInverted(false); float m1 = 0.55; float m2 = m1; float l1 = 0.30;//length of links float l2 = l1; - float I1 = 1/3*m1*pow(l1,2);//model as rod rotating around one end - float I2 = 1/3*m2*pow(l2,2); + float I1 = 1/3.0*m1*l1*l1;//model as rod rotating around one end + float I2 = 1/3.0*m2*l2*l2; float c1 = 0.5*l1;//location of center of mass along link float c2 = 0.5*l2; float g = 9.81; @@ -37,6 +41,8 @@ _parameters[8] = g; _parameters[9] = lattice_pitch; + _lastTorque = motor.getTorque(); + _manualTau = 0; } void setInverted(bool inverted){ @@ -66,11 +72,11 @@ void setSwingUpD(float d){ gains.setSwingUpD(d); }; - void setK2(float k2){ - gains.setK2(k2); + void setCurrentP(float p){ + gains.setCurrentP(p); }; - void setD2(float d2){ - gains.setD2(d2); + void setCurrentD(float d){ + gains.setCurrentD(d); }; float getSwingUpK(){ return gains.getSwingUpK(); @@ -78,11 +84,11 @@ float getSwingUpD(){ return gains.getSwingUpD(); }; - float getK2(){ - return gains.getK2(); + float getCurrentP(){ + return gains.getCurrentP(); }; - float getD2(){ - return gains.getD2(); + float getCurrentD(){ + return gains.getCurrentD(); }; Target target; @@ -95,21 +101,43 @@ Motor motor; void setTorque(float torque){ - motor.setTorque(torque); + _manualTau = torque; }; + + float _manualTau; //imu MyMPU6050 myMPU6050_1; MyMPU6050 myMPU6050_2; + + void updateIMUS(){ + getActiveIMU()->loop(); + } void loop(){ - if (_inverted) myMPU6050_2.loop(); - else myMPU6050_1.loop(); + +// getActiveIMU().disableInterrupt(); + updateThetas(); -// float tau = calcTau(_z, _parameters, &gains, _pc); - float tau = getTheta1(); + float tau = calcTau(_z, _parameters, &gains, _pc); +// float tau = getTheta1(); + motor.setTorque(tau); + +// getActiveIMU().enableInterrupt(); + } + + MyMPU6050* getActiveIMU(){ + if (_inverted) return &myMPU6050_2; + return &myMPU6050_1; + } + + float pdTorque(float desiredTorque, float deltaT){ + float torque = motor.getTorque(); + float newTorque = gains.getCurrentP()*(desiredTorque-torque) + gains.getCurrentD()*(torque-_lastTorque)/deltaT; + _lastTorque = torque;//update _lastTorque + return newTorque; } float getTheta1(){ @@ -125,10 +153,6 @@ return _z[3]; } - void printPWM(){ - motor.printPWM(); - } - private: @@ -136,8 +160,9 @@ Serial *_pc; float _parameters[10]; - - float _z[4];//theta1, theta2, dtheta2, dtheta2 + volatile float _z[4];//theta1, theta2, dtheta2, dtheta2 + + float _lastTorque; void updateThetas(){ _z[0] = _getTheta1(); @@ -147,12 +172,10 @@ } float _getTheta1(){ - if (_inverted) return myMPU6050_2.getTheta(); - return myMPU6050_1.getTheta(); + return getActiveIMU()->getTheta(); } float _getDTheta1(){ - if (_inverted) return myMPU6050_2.getDTheta(); - return myMPU6050_1.getDTheta(); + return getActiveIMU()->getDTheta(); } float _getTheta2(){ if (_inverted) return -motor.getTheta();