Diff: Controls/Controls.h

Revision:

11:711d3c207e8c

Parent:

10:769cc457c3a4

Child:

12:49813131dd15

--- a/Controls/Controls.h	Sat Dec 05 09:04:23 2015 +0000
+++ b/Controls/Controls.h	Tue Dec 08 22:52:46 2015 +0000
@@ -20,14 +20,14 @@
         void setup(){
             setInverted(false);
             
-            float m1 = 0.55;
-            float m2 = m1;
-            float l1 = 0.30;//length of links
+            float m1 = 0.93159230;
+            float m2 = 0.45433433;
+            float l1 = 0.275;//length of links
             float l2 = l1;
-            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 I1 = 0.03736067;
+            float I2 = 0.01778165;
+            float c1 = l1-0.08567346;//location of center of mass along link
+            float c2 = 0.17594269;
             float g = 9.81;
             float lattice_pitch  = 0.35;
             _parameters[0] = l1;
@@ -42,7 +42,7 @@
             _parameters[9] = lattice_pitch;
             
             _lastTorque = motor.getTorque();
-            _manualTau = 0;
+            _manualTheta = 0;
         }
         
         void setInverted(bool inverted){
@@ -100,11 +100,11 @@
         };
         
         Motor motor;
-        void setTorque(float torque){
-            _manualTau = torque;
+        void setTheta(float theta){
+            _manualTheta = theta;
         };
         
-        float _manualTau;
+        float _manualTheta;
     
         //imu
         MyMPU6050 myMPU6050_1;
@@ -112,13 +112,14 @@
         
         void updateIMUS(){
             getActiveIMU()->loop();
+            updateThetas();
         }
     
         void loop(){
 
 //            getActiveIMU().disableInterrupt();
-            
-            updateThetas();
+           // float z[4] = {1,2,3,4};
+//            _pc->printf("%f\n", calcTau(z, _parameters, &gains, _pc));
             
             float tau = calcTau(_z, _parameters, &gains, _pc);
 //            float tau = getTheta1();