Diff: Controls/Controls.h

Revision:

10:769cc457c3a4

Parent:

9:1d9b24d7ac77

Child:

11:711d3c207e8c

--- 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();