Diff: Controls/Controls.h

Revision:

12:49813131dd15

Parent:

11:711d3c207e8c

Child:

13:64d337c5114e

--- a/Controls/Controls.h	Tue Dec 08 22:52:46 2015 +0000
+++ b/Controls/Controls.h	Wed Dec 09 03:08:30 2015 +0000
@@ -43,6 +43,8 @@
             
             _lastTorque = motor.getTorque();
             _manualTheta = 0;
+            
+            setTargetPosition(6);
         }
         
         void setInverted(bool inverted){
@@ -93,7 +95,7 @@
         
         Target target;
         void setTargetPosition(int position){
-            target.setPosition(position);
+            target.setPosition(position, _parameters);
         };
         int getTargetPosition(){
             return target.getPosition();
@@ -121,7 +123,7 @@
            // 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 = calcTau(_z, _parameters, &gains, &target, _pc);
 //            float tau = getTheta1();
             
             motor.setTorque(tau);