Diff: actuators.cpp

Revision:

69:37f75a7d36d8

Parent:

68:21cb054f1399

Child:

74:75be98779124

--- a/actuators.cpp	Thu Oct 15 16:15:05 2015 +0200
+++ b/actuators.cpp	Thu Oct 15 18:52:09 2015 +0200
@@ -33,8 +33,8 @@
     double Ki1 = 0; 
     double Kd1 = 0;
 
-    double Kp2 = 0.0184; 
-    double Ki2 = 0.32; 
+    double Kp2 = 0.008; 
+    double Ki2 = 0.08; 
     double Kd2 = 0;
 
     double motor1PrevCounts = 0;
@@ -76,6 +76,9 @@
     motor1PID.SetSampleTime(motorCall);
     motor2PID.SetSampleTime(motorCall);
     
+    motor1PID.SetOutputLimits(0,1);
+    motor2PID.SetOutputLimits(0,1);
+    
     // Turn PID on
     motor1PID.SetMode(AUTOMATIC);
     motor2PID.SetMode(AUTOMATIC);
@@ -125,12 +128,12 @@
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     pidOut = motor2PID.Compute();
 // write new values to motor's
-    if (motor1PWM >= 0 ){ // CCW rotation 
+    if (motor1SetSpeed >= 0 ){ // CCW rotation 
         direction1 = false;
     }else{
         direction1 = true; // CW rotation
     }
-    if (motor2PWM >= 0 ){ // CCW rotation 
+    if (motor2SetSpeed >= 0 ){ // CCW rotation 
         direction2 = false;
     }else{
         direction2 = true; // CW rotation
@@ -138,8 +141,10 @@
     motor1Dir.write(direction1);
     motor2Dir.write(direction2);
     
+    double motor2FF = 0.0012*motor2SetSpeed+0.02;
+
     motor1.write(abs(motor1PWM));
-    motor2.write(abs(motor2PWM));
+    motor2.write(abs(motor2PWM)+motor2FF);
 }
 
 void servoControl(){