File content as of revision 17:cb8ad2fc76e5:

#include "EncoderMotor.hpp"
#include "Math.hpp"

const float tickTime = 1.f / 60;

EncoderMotor::EncoderMotor(MotorData nData, EncoderData encData, PIDData speedPIDData, PIDData turnPIDData) :
    Motor(nData),
    encoder(encData), speedSmoother(0.3f),
    mode(NoMode),
    speedPID(speedPIDData), setSpeed(0),
    turnPID(turnPIDData), setTurn(0)
{
    
}

void EncoderMotor::setup()
{
    ticker.attach(this, &EncoderMotor::tick, tickTime);
}

void EncoderMotor::drive(float speed)
{
    setSpeed = speed;
    mode = SpeedMode;
}

void EncoderMotor::rotate(float turn, float speedLimit)
{
    setTurn = encoder.getTurn() + turn;
    turnSpeedLimit = speedLimit;
    mode = TurnMode;
}

void EncoderMotor::tick()
{
    switch (mode)
    {
    case NoMode:
        break;
    
    case TurnMode:
    {
        float errorTurn = setTurn - encoder.getTurn();
        setSpeed = clampAmplitude(turnPID.step(errorTurn), turnSpeedLimit);
        // fallthrough
    }
    
    case SpeedMode:
    {
        float errorSpeed = setSpeed - speedSmoother.smooth(encoder.getTurnSpeed());
        Motor::drive(speedPID.step(errorSpeed));
        break;
    }
    }
}