File content as of revision 23:d844cc906b66:

#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),
    mode(NoMode),
    speedPID(speedPIDData), setSpeed(0),
    turnPID(turnPIDData), setTurn(0),
    sync(nSync)
{
    
}

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;
}

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

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

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

void EncoderMotor::tick()
{
    /*pc.printf("%f\n", encoder.getTurn());
    return;*/
    
    switch (mode)
    {
    case NoMode:
        break;
    
    case TurnMode:
    {
        float errorTurn = setTurn - encoder.getTurn();
        setSpeed = clampAmplitude(turnPID.step(errorTurn), turnSpeedLimit);
        // fallthrough
    }
    
    case SpeedMode:
    {
        float actualSpeed = speedSmoother.smooth(encoder.getTurnSpeed());
        //float actualSpeed = 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);
        /*if ((++x %= 10) == 0)
            pc.printf("%f %f %f %f %f\n", encoder.getTurn(), actualSpeed, desiredSpeed, errorSpeed, out);*/
        Motor::drive(out);
        break;
    }
    }
}