File content as of revision 47:4f418a4b0051:

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

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

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

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

SpeedEncoder& EncoderMotor::getEncoder()
{
    return encoder;
}

float EncoderMotor::getSetTurn() const
{
    return setTurn;
}

float EncoderMotor::getSetSpeed() const
{
    return setSpeed;
}

void EncoderMotor::tick()
{
    switch (mode)
    {
    case NoMode:
        break;
    
    case TurnMode:
    {
        errorTurn = setTurn - encoder.getTurn();
        setSpeed = clampAmplitude(turnPID.step(errorTurn), turnSpeedLimit);
        // fallthrough
    }
    
    case SpeedMode:
    {
        float actualSpeed = speedSmoother.smooth(encoder.getTurnSpeed());
        float factor = actualSpeed / setSpeed;
        
        float desiredSpeed;
        if (sync != NULL)
        {
            float minFactor = sync->update(this, factor);
            desiredSpeed = minFactor * setSpeed;
        }
        else
        {
            desiredSpeed = setSpeed;
        }
        
        float errorSpeed = desiredSpeed - actualSpeed;
        float out = speedPID.step(errorSpeed);
        Motor::drive(out);
        break;
    }
    }
}