Diff: actuators.cpp

Revision:

48:3cf1eaf34926

Parent:

46:a00cc8a751f5

Child:

50:b0cf07ca53cf

--- a/actuators.cpp	Thu Oct 08 15:24:12 2015 +0200
+++ b/actuators.cpp	Thu Oct 08 15:29:48 2015 +0200
@@ -34,7 +34,7 @@
     float Ki2 = 0; 
     float Kd2 = 0;
 
-    float PIDinterval = 0.01;
+    float PIDinterval = 0.1;
 
     float prevMotor2Pos = 0;
     float prevMotor1Pos = 0;
@@ -92,7 +92,6 @@
 
 
 void motorControl(){
-    if(motorsEnable){  // only run motors if switch is enabled
     // get encoder positions in degrees
         // 131.25:1 gear ratio
         // getPosition uses X2 configuration, so 32 counts per revolution
@@ -111,9 +110,10 @@
         
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
+        
+    if(motorsEnable){  // only run motors if switch is enabled
     // compute new PID parameters using setpoint angle speeds and encoder speed
         writeMotors();
-
     }else{
         // write 0 to motors
         motor1.write(0);