Diff: actuators.cpp

Revision:

2:95ba9f6f0128

Parent:

1:80f098c05d4b

Child:

3:47c76be6d402

--- a/actuators.cpp	Thu Oct 01 15:09:24 2015 +0200
+++ b/actuators.cpp	Thu Oct 01 15:36:55 2015 +0200
@@ -13,6 +13,7 @@
     PIDmotor1.SetMode(AUTOMATIC);
     PIDmotor2.SetMode(AUTOMATIC);
 
+    // set limits for PID output to avoid integrator build up.
     PIDmotor1.SetOutputLimits(-1f, 1f);
     PIDmotor2.SetOutputLimits(-1f, 1f);
     }
@@ -24,15 +25,26 @@
     // get encoder positions
         // check if motor's are within rotational boundarys
     // calculate  encoder speeds
+        motorSpeed1=(motorPos1-prevMotorPos1)/(time.read()-prevTime);
+        motorSpeed2=(motorPos2-prevMotorPos2)/(time.read()-prevTime);
+
+        // store current positions and time
+        prevMotorPos1 = motorPos1;
+        prevMotorPos2 = motorPos2;
+        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);
+
     }else{
         // write 0 to motors
+        motor1.write(0);
+        motor2.write(0);
     }
 }