EncoderStateCalculator.cpp

Go to the documentation of this file.

#include "robodyn_utilities/EncoderStateCalculator.h"

EncoderStateCalculator::EncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
        const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection,
        const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart)
    : encoderVelocityLowFrequency(0.0),
      encoderVelocityLowFrequencyPrevious(0.0),
      encoderVelocityHighFrequency(0.0),
      encoderVelocityBlended(0.0),
      encoderPosition(0.0),
      encoderAccumulationPrevious(0),
      gammaBlend(0.0),
      jointGearRatio(jointGearRatio),
      pwmFrequency(pwmFrequency),
      pwmDeadTime(pwmDeadTime),
      busVoltage(busVoltage),
      motorCurrentLimit(motorCurrentLimit),
      phaseResistance(phaseResistance),
      backEmfConstant(backEmfConstant),
      jointKinematicDirection(jointKinematicDirection),
      encoderMountingDirection(encoderMountingDirection),
      encoderGlitchScalar(encoderGlitchScalar),
      encoderVelocityBlendStop(encoderVelocityBlendStop),
      encoderVelocityBlendStart(encoderVelocityBlendStart),
      encoderTickTimeout(8388607),   
      clockFrequency(50000000.0),    
      backEmfConstantMinimum(0.0001) 
{
    busVoltageRatio = 1.0 - (2.0 * pwmFrequency * pwmDeadTime);

    if (backEmfConstant >= backEmfConstantMinimum)
    {
        maxEncoderVelocity = ((busVoltageRatio * busVoltage) - (motorCurrentLimit * phaseResistance)) / backEmfConstant;
    }
    else
    {
        throw std::runtime_error("EncoderStateCalculator() - backEmfConstant must be greater than 0.0001.");
    }


    gammaBlendDenominator = encoderVelocityBlendStop - encoderVelocityBlendStart;

    if (gammaBlendDenominator < 0)
    {
        gammaBlendDenominator = -gammaBlendDenominator;
    }

}

EncoderStateCalculator::~EncoderStateCalculator()
{
}

double EncoderStateCalculator::getPosition(const double& encoderPositionReference, const int32_t& incrementalEncoderReference, const int32_t& incrementalEncoderNow)
{
    encoderPosition = encoderPositionReference + ((incrementalEncoderNow - incrementalEncoderReference) * encoderCountsToJointRadians * encoderMountingDirection * jointKinematicDirection);

    return encoderPosition;
}

double EncoderStateCalculator::getVelocity(int32_t timer, const int32_t& timeout, const int32_t& encoderAccumulation, const double& measuredControlRateHz)
{
    if (timer >= 0)
    {
        if (timeout > timer)
        {
            timer = timeout;
        }
    }
    else
    {
        if (-timeout < timer)
        {
            timer = -timeout;
        }
    }

    if (timer == 0)
    {
        timer = 1;
    }

    if (timeout >= encoderTickTimeout)
    {
        encoderVelocityLowFrequency = 0.0;
    }
    else
    {
        encoderVelocityLowFrequency = (clockFrequency / timer) * (encoderRadiansPerCount * encoderMountingDirection);
    }

    if (std::abs(encoderVelocityLowFrequency) > (encoderGlitchScalar * maxEncoderVelocity))
    {
        encoderVelocityLowFrequency = encoderVelocityLowFrequencyPrevious;
    }

    encoderVelocityLowFrequencyPrevious = encoderVelocityLowFrequency;

    encoderVelocityHighFrequency = (measuredControlRateHz * (encoderAccumulation - encoderAccumulationPrevious)) * (encoderRadiansPerCount * encoderMountingDirection);
    encoderAccumulationPrevious = encoderAccumulation;

    gammaBlend = 0.0;

    if (gammaBlendDenominator > 1.0)
    {
        gammaBlend = (std::abs(encoderVelocityBlended) - encoderVelocityBlendStart) / gammaBlendDenominator;
        gammaBlend = applyLimits(gammaBlend, 0.0, 1.0);
    }
    else
    {
        gammaBlend = 0;
    }

    encoderVelocityBlended  = (encoderVelocityLowFrequency * (1.0 - gammaBlend))           + (encoderVelocityHighFrequency * gammaBlend);

    return encoderVelocityBlended * jointKinematicDirection;
}

double EncoderStateCalculator::applyLimits(const double& value, const double& lowerLimit, const double& upperLimit)
{
    if (value > upperLimit)
    {
        return upperLimit;
    }
    else if (value < lowerLimit)
    {
        return lowerLimit;
    }
    else
    {
        return value;
    }
}

MotorEncoderStateCalculator::MotorEncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
        const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection, const double& encoderCountsPerRevolution,
        const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart)
    : EncoderStateCalculator(jointGearRatio, pwmFrequency, pwmDeadTime, busVoltage, motorCurrentLimit, phaseResistance,
                             backEmfConstant, jointKinematicDirection, encoderMountingDirection,
                             encoderGlitchScalar, encoderVelocityBlendStop, encoderVelocityBlendStart),
      encoderCountsPerRevolution(encoderCountsPerRevolution)
{
    encoderCountsPerMotorRevolution = 4.0 * encoderCountsPerRevolution;
    encoderRadiansPerCount          = 2.0 * boost::math::constants::pi<double>() / encoderCountsPerMotorRevolution;
    encoderCountsToJointRadians     = encoderRadiansPerCount / jointGearRatio;
}

MotorEncoderStateCalculator::~MotorEncoderStateCalculator()
{
}

HallsEncoderStateCalculator::HallsEncoderStateCalculator(const double& jointGearRatio, const double& pwmFrequency, const double& pwmDeadTime, const double& busVoltage, const double& motorCurrentLimit, const double& phaseResistance,
        const double& backEmfConstant, const double& jointKinematicDirection, const double& encoderMountingDirection, const double& motorPoleCount,
        const double& encoderGlitchScalar, const double& encoderVelocityBlendStop, const double& encoderVelocityBlendStart)
    : EncoderStateCalculator(jointGearRatio, pwmFrequency, pwmDeadTime, busVoltage, motorCurrentLimit, phaseResistance,
                             backEmfConstant, jointKinematicDirection, encoderMountingDirection,
                             encoderGlitchScalar, encoderVelocityBlendStop, encoderVelocityBlendStart),
      motorPoleCount(motorPoleCount)
{
    encoderCountsPerMotorRevolution = 3.0 * motorPoleCount;
    encoderRadiansPerCount          = 2.0 * boost::math::constants::pi<double>() / encoderCountsPerMotorRevolution;
    encoderCountsToJointRadians     = encoderRadiansPerCount / jointGearRatio;
}

HallsEncoderStateCalculator::~HallsEncoderStateCalculator()
{
}