Controls.h

00001 #ifndef Controls_h
00002 #define Controls_h
00003 
00004 #include <math.h>
00005 #include "Gains.h"
00006 #include "Target.h"
00007 #include "MyMPU6050.h" 
00008 #include "Motor.h"  
00009 #include "Dynamics.h"
00010 
00011 #define controlTimerPeriod 0.001
00012 
00013 class Controls: public CommDelegate{
00014     
00015     public:
00016     
00017         Controls():myMPU6050_1(p28, p27, p18), myMPU6050_2(p9, p10, p11){//I2C_SDA, I2C_SCL, int_pin
00018         }
00019         
00020         void setup(){
00021             setInverted(false);
00022             
00023             float m1 = 0.93159230;
00024             float m2 = 0.45433433;
00025             float l1 = 0.275;//length of links
00026             float l2 = l1;
00027             float I1 = 0.03736067;
00028             float I2 = 0.01778165;
00029             float c1 = l1-0.08567346;//location of center of mass along link
00030             float c2 = 0.17594269;
00031             float g = 9.81;
00032             float lattice_pitch  = 0.35;
00033             _parameters[0] = l1;
00034             _parameters[1] = l2;
00035             _parameters[2] = c1;
00036             _parameters[3] = c2;
00037             _parameters[4] = m1;
00038             _parameters[5] = m2;
00039             _parameters[6] = I1;
00040             _parameters[7] = I2;
00041             _parameters[8] =  g;
00042             _parameters[9] = lattice_pitch;
00043             
00044             _manualTheta = 0;
00045             
00046             setTargetPosition(6);//only 4 and 6 for now
00047         }
00048         
00049         void setInverted(bool inverted){
00050             _inverted = inverted;
00051             if (!_inverted) {
00052                 myMPU6050_2.disable();
00053                 myMPU6050_1.enable();
00054 //                motor.setGearRatio(10.164);
00055             } else {
00056                 myMPU6050_1.disable();
00057                 myMPU6050_2.enable();
00058 //                motor.setGearRatio(11.164);
00059             }
00060         }
00061         
00062         void setPC(Serial *pc){
00063             _pc = pc;
00064             motor.setPC(pc);
00065             gains.setPC(pc);
00066             target.setPC(pc);
00067             myMPU6050_1.setPC(pc);
00068             myMPU6050_2.setPC(pc);
00069         }
00070     
00071         Gains gains;
00072         void setSwingUpK(float k){
00073             gains.setSwingUpK(k);
00074         };
00075         void setSwingUpD(float d){
00076             gains.setSwingUpD(d);
00077         };
00078         void setDesiredThetaP(float p){
00079             gains.setDesiredThetaP(p);
00080         };
00081         void setSoftLimitsP(float p){
00082             gains.setSoftLimitsP(p);
00083         };
00084         float getSwingUpK(){
00085             return gains.getSwingUpK();
00086         };
00087         float getSwingUpD(){
00088             return gains.getSwingUpD();
00089         };
00090         float getDesiredThetaP(){
00091             return gains.getDesiredThetaP();
00092         };
00093         float getSoftLimitsP(){
00094             return gains.getSoftLimitsP();
00095         };
00096         
00097         Target target;
00098         void setTargetPosition(int position){
00099             target.setPosition(position, _parameters);
00100         };
00101         int getTargetPosition(){
00102             return target.getPosition();
00103         };
00104         
00105         void setTargetingK(float k){
00106             gains.setTargetingK(k);
00107         };
00108         void setTargetingD(float d){
00109             gains.setTargetingD(d);
00110         };
00111         float getTargetingK(){
00112             return gains.getTargetingK();
00113         };
00114         float getTargetingD(){
00115             return gains.getTargetingD();
00116         };
00117         
00118         Motor motor;
00119         void setTheta(float theta){
00120             _manualTheta = theta;
00121         };
00122         
00123         float _manualTheta;
00124     
00125         //imu
00126         MyMPU6050 myMPU6050_1;
00127         MyMPU6050 myMPU6050_2;
00128         
00129         void updateIMUS(){
00130             getActiveIMU()->loop();
00131         }
00132     
00133         void loop(){
00134 
00135             getActiveIMU()->disableInterrupt();
00136             updateThetas();
00137             float tau = calcTau(_z, _parameters, &gains, &target, _pc);
00138 
00139 //            float K = gains.getSwingUpK();
00140 //            float D = gains.getSwingUpD();
00141 //            
00142 //            float th1 = _z[0];
00143 //            float th2 = _z[1];
00144 //            float dth1 = _z[2];
00145 //            float dth2 = _z[3];
00146 //            float tau = (K*(_manualTheta - th2) - D*dth2);
00147 
00148             motor.setTorque(tau);
00149             
00150             getActiveIMU()->enableInterrupt();
00151         }
00152         
00153         MyMPU6050* getActiveIMU(){
00154             if (_inverted) return &myMPU6050_2;
00155             return &myMPU6050_1;
00156         }
00157         
00158         float getTheta1(){
00159             return _z[0];
00160         }
00161         float getDTheta1(){
00162             return _z[2];
00163         }
00164         float getTheta2(){
00165             return _z[1];
00166         }
00167         float getDTheta2(){
00168             return _z[3];
00169         }
00170         
00171         
00172     
00173     private:
00174     
00175         Serial *_pc;
00176     
00177         float _parameters[10];
00178         volatile float _z[4];//theta1, theta2, dtheta2, dtheta2
00179                 
00180         void updateThetas(){
00181             _z[0] = _getTheta1();
00182             _z[2] = _getDTheta1();
00183             _z[1] = _getTheta2();
00184             _z[3] = _getDTheta2();
00185             
00186         }
00187         float _getTheta1(){
00188             return getActiveIMU()->getTheta();
00189         }
00190         float _getDTheta1(){
00191             return getActiveIMU()->getDTheta();
00192         }
00193         float _getTheta2(){
00194             if (_inverted) return -motor.getTheta();
00195             return motor.getTheta();
00196         }
00197         float _getDTheta2(){
00198             if (_inverted) return -motor.getDTheta();
00199             return motor.getDTheta();
00200         }
00201         
00202         bool _inverted;
00203     
00204 };
00205 
00206 #endif