MotorControl.h

Go to the documentation of this file.

#pragma once

#include "mbed.h"

#include "atoms/control/pid.h"

#include "Encoder.h"
#include "Motor.h"

namespace nxpcup {

class MotorControl {
public:
    struct Config {
        float coefficientP = 0.007; 
        float coefficientI = 0.008; 
        float antiWindup = 0.5; 
        float nullSpeedThreshold = 0.05; 
        float nullSpeedPower = 0.2; 
    };

    MotorControl(Motor& motor, Encoder& encoder, Config config)
        : m_motor(motor)
        , m_encoder(encoder)
        , m_config(config)
    {
        m_motorMaxPower = m_motor.maxPower();
    }

    void setSpeed(float desiredSpeed) { m_desiredSpeed = desiredSpeed; }

    float desiredSpeed() const
    {
        return m_desiredSpeed;
    }

    float actualSpeed() const
    {
        return m_actualSpeed;
    }

    void regulate(uint16_t timeSinceLastCallUs)
    {
        m_actualSpeed = m_encoder.speed();

        float error = m_desiredSpeed - m_actualSpeed;
        m_errorSum += m_config.coefficientI * error;
        if (m_errorSum > m_config.antiWindup) {
            m_errorSum = m_config.antiWindup;
        }
        if (m_errorSum < 0) {
            m_errorSum = 0;
        }

        float output = m_config.coefficientP * error + m_errorSum; // normalized output <0.0 - 1.0>
        if (output > 1) { // clamp the output
            output = 1;
        } else if (output < 0 || m_desiredSpeed < m_config.nullSpeedThreshold) { // turn off regulation around zero/low desire speed
            output = 0;
            m_errorSum = 0;
        } else if ( // solve slower start of robot
            m_actualSpeed < m_config.nullSpeedThreshold && output > m_config.nullSpeedPower) {
            output = m_config.nullSpeedPower;
            m_errorSum = 0;
        }
        output = output * m_motorMaxPower; // output <-1000; 1000>
        m_motor.power(int32_t(output));
    }

    Config config() const
    {
        return m_config;
    }

    void setConfig(Config& config)
    {
        m_config = config;
        reset();
    }

    void reset()
    {
        m_motor.power(0);
        m_errorSum = 0;
    }

private:
    Motor& m_motor;
    Encoder& m_encoder;
    Config m_config;

    float m_desiredSpeed = 0; //[m/s]
    float m_actualSpeed = 0; //[m/s]
    uint16_t m_motorMaxPower = 0;
    float m_errorSum = 0;
};

} // namespace nxpcup