Diff: EncoderMotor.cpp

Revision:

25:a8bb69e99d6b

Parent:

23:d844cc906b66

Child:

27:24a9ac72fe92

--- a/EncoderMotor.cpp	Tue Apr 19 18:26:44 2016 +0000
+++ b/EncoderMotor.cpp	Tue Apr 26 12:22:05 2016 +0000
@@ -1,14 +1,11 @@
 #include "EncoderMotor.hpp"
 #include "Math.hpp"
 
-extern Serial pc;
-long x = 0;
-
 const float tickTime = 1.f / 60;
 
 EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData, SyncGroup *nSync) :
     Motor(nData),
-    encoder(encData), speedSmoother(0.1f),
+    encoder(encData), speedSmoother(0.3f),
     mode(NoMode),
     speedPID(speedPIDData), setSpeed(0),
     turnPID(turnPIDData), setTurn(0),
@@ -19,7 +16,7 @@
 
 void EncoderMotor::setup()
 {
-    //ticker.attach(this, &EncoderMotor::tick, tickTime);
+    ticker.attach(this, &EncoderMotor::tick, tickTime);
 }
 
 void EncoderMotor::drive(float speed)
@@ -52,9 +49,6 @@
 
 void EncoderMotor::tick()
 {
-    /*pc.printf("%f\n", encoder.getTurn());
-    return;*/
-    
     switch (mode)
     {
     case NoMode:
@@ -70,7 +64,6 @@
     case SpeedMode:
     {
         float actualSpeed = speedSmoother.smooth(encoder.getTurnSpeed());
-        //float actualSpeed = encoder.getTurnSpeed();
         float factor = actualSpeed / setSpeed;
         
         float desiredSpeed;
@@ -86,8 +79,6 @@
         
         float errorSpeed = desiredSpeed - actualSpeed;
         float out = speedPID.step(errorSpeed);
-        /*if ((++x %= 10) == 0)
-            pc.printf("%f %f %f %f %f\n", encoder.getTurn(), actualSpeed, desiredSpeed, errorSpeed, out);*/
         Motor::drive(out);
         break;
     }