Diff: Controls/Controls.h

Revision:

18:0cfe72d8a006

Parent:

17:8a0e647cf551

Child:

19:270735e44c98

--- a/Controls/Controls.h	Fri Dec 11 00:44:45 2015 +0000
+++ b/Controls/Controls.h	Fri Dec 11 06:16:33 2015 +0000
@@ -51,9 +51,11 @@
             if (!_inverted) {
                 myMPU6050_2.disable();
                 myMPU6050_1.enable();
+//                motor.setGearRatio(10.164);
             } else {
                 myMPU6050_1.disable();
                 myMPU6050_2.enable();
+//                motor.setGearRatio(11.164);
             }
         }
         
@@ -120,14 +122,23 @@
         
         void updateIMUS(){
             getActiveIMU()->loop();
-            updateThetas();
         }
     
         void loop(){
 
             getActiveIMU()->disableInterrupt();
+            updateThetas();
+            float tau = calcTau(_z, _parameters, &gains, &target, _pc);
 
-            float tau = calcTau(_z, _parameters, &gains, &target, _pc);
+//            float K = gains.getSwingUpK();
+//            float D = gains.getSwingUpD();
+//            
+//            float th1 = _z[0];
+//            float th2 = _z[1];
+//            float dth1 = _z[2];
+//            float dth2 = _z[3];
+//            float tau = (K*(_manualTheta - th2) - D*dth2);
+
             motor.setTorque(tau);
             
             getActiveIMU()->enableInterrupt();