Diff: actuators.cpp

Revision:

29:e4f3455aaa0b

Parent:

28:40a931dfe840

Child:

30:a20f16bf8dda

diff -r 40a931dfe840 -r e4f3455aaa0b actuators.cpp
--- a/actuators.cpp	Tue Oct 06 16:55:31 2015 +0200
+++ b/actuators.cpp	Tue Oct 06 17:55:11 2015 +0000
@@ -34,6 +34,10 @@
 
 float PIDinterval = 0.2;
 
+float prevMotor2Pos = 0;
+float prevMotor1Pos = 0;
+float prevTime = 0;
+float curTime = 0;
 
 // Create object instances
 // Initialze motors
@@ -41,8 +45,8 @@
 PwmOut motor2(motor2PWMPin);
 
 // Initialize encoders
-Encoder encoder1(enc1A, enc1B, true);
-Encoder encoder2(enc2A, enc2B, true);
+Encoder encoder1(enc1A, enc1B);
+Encoder encoder2(enc2A, enc2B);
 
 // Set direction pins     
 DigitalOut motor1Dir(motor1DirPin);
@@ -51,7 +55,7 @@
 // create PID instances
 PID PIDmotor1(Kp1, Ki1, Kd1, PIDinterval);
 PID PIDmotor2(Kp2, Ki2, Kd2, PIDinterval);
-    
+Timer t;  
     
 void motorInit(){
     
@@ -74,7 +78,7 @@
     // set limits for PID output to avoid integrator build up.
     PIDmotor1.setOutputLimits(-1.0, 1.0);
     PIDmotor2.setOutputLimits(-1.0, 1.0);
-
+    t.start();
 }
 
 
@@ -87,9 +91,14 @@
 
         // check if motor's are within rotational boundarys
     // get  encoder speeds
-        motorSpeed1 = encoder1.getSpeed();
-        motorSpeed2 = encoder2.getSpeed();
-
+        curTime = t.read()
+        motorSpeed1 = (motor1Pos - prevMotor1Pos)/(curTime-prevTime);
+        motorSpeed2 = (motor2Pos - prevMotor2Pos)/(curTime-prevTime);
+        prevTime = curTime;
+        prevMotor1Pos = motor1Pos;
+        prevMotor2Pos = motor2Pos;
+        
+        
     // translate to x/y speed
     // compute new PID parameters using setpoint speeds and x/y speeds
         motorPWM1 = PIDmotor1.compute();