Diff: Controls/Controls.h

Revision:

9:1d9b24d7ac77

Parent:

8:1a3a69fecedf

Child:

10:769cc457c3a4

--- a/Controls/Controls.h	Thu Dec 03 23:55:44 2015 +0000
+++ b/Controls/Controls.h	Sat Dec 05 00:40:42 2015 +0000
@@ -12,9 +12,9 @@
     
     public:
     
-        Controls():myMPU6050_1(p9, p10, p11), myMPU6050_2(p28, p27, p18){//I2C_SDA, I2C_SCL, int_pin
+        Controls():myMPU6050_1(p28, p27, p18), myMPU6050_2(p9, p10, p11){//I2C_SDA, I2C_SCL, int_pin
         
-            _inverted = false;
+            setInverted(false);
             
             float m1 = 0.55;
             float m2 = m1;
@@ -38,11 +38,65 @@
             _parameters[9] = lattice_pitch;
             
         }
+        
+        void setInverted(bool inverted){
+            _inverted = inverted;
+            if (!_inverted) {
+                myMPU6050_2.disable();
+                myMPU6050_1.enable();
+            } else {
+                myMPU6050_1.disable();
+                myMPU6050_2.enable();
+            }
+        }
+        
+        void setPC(Serial *pc){
+            _pc = pc;
+            motor.setPC(pc);
+            gains.setPC(pc);
+            target.setPC(pc);
+            myMPU6050_1.setPC(pc);
+            myMPU6050_2.setPC(pc);
+        }
     
         Gains gains;
+        void setSwingUpK(float k){
+            gains.setSwingUpK(k);
+        };
+        void setSwingUpD(float d){
+            gains.setSwingUpD(d);
+        };
+        void setK2(float k2){
+            gains.setK2(k2);
+        };
+        void setD2(float d2){
+            gains.setD2(d2);
+        };
+        float getSwingUpK(){
+            return gains.getSwingUpK();
+        };
+        float getSwingUpD(){
+            return gains.getSwingUpD();
+        };
+        float getK2(){
+            return gains.getK2();
+        };
+        float getD2(){
+            return gains.getD2();
+        };
+        
         Target target;
+        void setTargetPosition(int position){
+            target.setPosition(position);
+        };
+        int getTargetPosition(){
+            return target.getPosition();
+        };
         
         Motor motor;
+        void setTorque(float torque){
+            motor.setTorque(torque);
+        };
     
         //imu
         MyMPU6050 myMPU6050_1;
@@ -53,7 +107,9 @@
             else myMPU6050_1.loop();
             updateThetas();
             
-            float tau = calcTau(_z, _parameters);
+//            float tau = calcTau(_z, _parameters, &gains, _pc);
+            float tau = getTheta1();
+            motor.setTorque(tau);
         }
         
         float getTheta1(){
@@ -69,10 +125,16 @@
             return _z[3];
         }
         
+        void printPWM(){
+            motor.printPWM();
+        }
+        
         
     
     private:
     
+        Serial *_pc;
+    
         float _parameters[10];
     
         float _z[4];//theta1, theta2, dtheta2, dtheta2