Diff: actuators.cpp

Revision:

123:815ace09b753

Parent:

122:1a5df0765790

--- a/actuators.cpp	Thu Oct 29 12:26:50 2015 +0000
+++ b/actuators.cpp	Thu Oct 29 14:41:54 2015 +0000
@@ -38,8 +38,8 @@
 double motor2PWM = 0;
 
 // Set PID values
-double Kp1 = 1; 
-double Ki1 = 0; 
+double Kp1 = 0.008; 
+double Ki1 = 0.08; 
 double Kd1 = 0;
 
 double Kp2 = 0.008; 
@@ -140,16 +140,13 @@
     // calculate motor setpoint speed in deg/sec from setpoint x/y speed
 
     // exclude kinematics when still calibrating
-//    if (calReady){
-//      kinematics();
-//    }
+    if (calReady){
+      kinematics();
+    }
 
     if(motorsEnable){  // only run motors if switch is enabled
     // compute new PID parameters using setpoint angle speeds and encoder speed
-        motor1Dir.write(true);
-        motor2Dir.write(true);
-        motor1.write(0);
-        motor2.write(0.05f);
+        writeMotors();
         servoControl();
     }else{
         // write 0 to motors
@@ -162,6 +159,9 @@
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
 // write new values to motor's
+    if (motor1SetSpeed ==0 ){
+      motor1PID.SetOutputLimits(0,0);
+    }
     if (motor1SetSpeed > 0 ){ // CCW rotation 
         direction1 = true;
         motor1PID.SetOutputLimits(0,1); // change pid output direction
@@ -169,6 +169,9 @@
         direction1 = false; // CW rotation
         motor1PID.SetOutputLimits(-1,0);
     }
+    if (motor2SetSpeed ==0 ){
+      motor2PID.SetOutputLimits(0,0);
+    }
     if (motor2SetSpeed > 0 ){ // CCW rotation 
         direction2 = true;
         motor2PID.SetOutputLimits(0,1);