Diff: EncoderMotor.cpp

Revision:

6:9f9047ca4a89

Parent:

5:7f800f61cb13

Child:

7:8dcdb39efc0e

--- a/EncoderMotor.cpp	Tue Mar 29 14:59:13 2016 +0000
+++ b/EncoderMotor.cpp	Tue Mar 29 15:40:15 2016 +0000
@@ -2,7 +2,7 @@
 
 const float tickTime = 1.f / 60;
 
-EncoderMotor::EncoderMotor(PinName pwmPin, PinName dir1Pin, PinName dir2Pin, Encoder &nEncoder, ErrorController &nEc, ErrorController &nEcRot) : Motor(pwmPin, dir1Pin, dir2Pin), encoder(nEncoder), ec(nEc), setPower(0), prevCount(0), prevSpeed(0), ecRot(nEcRot), setRot(0)
+EncoderMotor::EncoderMotor(PinName pwmPin, PinName dir1Pin, PinName dir2Pin, SpeedEncoder &nEncoder, ErrorController &nEc, ErrorController &nEcRot) : Motor(pwmPin, dir1Pin, dir2Pin), encoder(nEncoder), ec(nEc), setPower(0), prevSpeed(0), ecRot(nEcRot), setRot(0)
 {
     
 }
@@ -23,24 +23,17 @@
     setRot += rot;
 }
 
-long EncoderMotor::getSpeed()
-{
-    return dCount;
-}
-
 void EncoderMotor::tick()
 {
     long count = encoder.getCount();
-    dCount = count - prevCount;
     
     float errorRot = setRot - count / 600.f;
     drive(ecRot.step(errorRot));
     
-    speed = 0.7f * prevSpeed + 0.3f * dCount / tickTime / 600;
+    speed = 0.7f * prevSpeed + 0.3f * encoder.getSpeed();
     float error = setPower - speed;
     out = ec.step(error);
     Motor::drive(out);
     
-    prevCount = count;
     prevSpeed = speed;
 }
\ No newline at end of file