Diff: actuators.cpp

Revision:

5:73bfad06b775

Parent:

4:80e2280058ed

Child:

6:b957d8809e7c

--- a/actuators.cpp	Mon Oct 05 16:15:25 2015 +0200
+++ b/actuators.cpp	Mon Oct 05 17:35:02 2015 +0200
@@ -25,8 +25,8 @@
 }
 
 void initPID(){
-        // create PID instances for motors
-    // PID pidname(input, output, setpoint, kp, ki, kd, direction)
+    // create PID instances for motors
+        // PID pidname(input, output, setpoint, kp, ki, kd, direction)
     PID PIDmotor1(&motorSpeed1, &motorPWM1, &motorSetSpeed1, Kp1, Ki1, Kd1, DIRECT);
     PID PIDmotor2(&motorSpeed2, &motorPWM2, &motorSetSpeed2, Kp2, Ki2, Kd2, DIRECT);
 
@@ -52,15 +52,24 @@
         motorSpeed1 = encoder1.getSpeed();
         motorSpeed2 = encoder2.getSpeed();
 
-        prevTime = time.read();
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
         PIDmotor1.compute();
         PIDmotor2.compute();
     // translate to motor rotation speed
     // write new values to motor's
-        motor1.write(motorPWM1);
-        motor2.write(motorPWM2);
+        if (motorPWM1 > 0 ){ // CCW rotation (unitcircle convetion)
+            direction1 = false;
+        }else{
+            direction1 = true; // CW rotation
+        }
+        if (motorPWM2 > 0 ){ // CCW rotation (unitcircle convetion)
+            direction2 = false;
+        }else{
+            direction2 = true; // CW rotation
+        }
+        motor1.write(abs(motorPWM1));
+        motor2.write(abs(motorPWM2));
 
     }else{
         // write 0 to motors