Cubic.controller.h

Go to the documentation of this file.

 
#pragma once
#include <Arduino.h>
#include "PID.h"
#include "cubic_arduino.h"
 
constexpr double degToRad(double deg)
{
    return deg * DEG_TO_RAD;
}
 
constexpr double radToDeg(double rad)
{
    return rad * RAD_TO_DEG;
}
 
namespace Cubic_controller
{
    constexpr uint16_t AMT22_CPR = 16384;
 
    constexpr double LOOP_THRESHOLD = 5.0 * PI / 6.0;
 
    constexpr double ALLOWED_ROTATION_RANGE = 2.0 * PI;
 
    enum class encoderType
    {
        inc,
        abs
    };
 
    constexpr double limitAngle(double angle, const double min = -PI)
    {
        while (angle < min)
        {
            angle += TWO_PI;
        }
        while (angle >= min + TWO_PI)
        {
            angle -= TWO_PI;
        }
        return angle;
    }
 
    constexpr double encoderToAngle(const int32_t encoder, const uint16_t CPR, const double offset = -PI, const bool limit = true)
    {
        if (limit)
        {
            return limitAngle(offset + encoder * (TWO_PI / (double)CPR));
        }
        else
        {
            return offset + encoder * (TWO_PI / (double)CPR);
        }
    }
 
    class Controller
    {
    private:
        PID::PID &pid;
 
        double capableDutyCycle;
        double dutyCycle;
 
    protected:
        const uint8_t motorNo;
        const enum class encoderType encoderType;
        const uint8_t encoderNo;
        const uint16_t CPR;
        const bool direction;
        const bool logging;
 
        double compute_PID(double current);
 
    public:
        Controller(uint8_t motorNo, uint8_t encoderNo, enum class encoderType encoderType, uint16_t CPR, double Kp, double Ki, double Kd, double target, bool direction, double capableDutyCycle, double current, bool logging = false);
 
        virtual double compute() = 0;
        virtual void setTarget(double target);
        void setGains(double Kp, double Ki, double Kd);
        void setKp(double Kp);
        void setKi(double Ki);
        void setKd(double Kd);
        int32_t readEncoder() const;
        double getTarget() const;
        double getDutyCycle() const;
        double getDt() const;
        double getCurrent() const;
 
        virtual void reset();
 
        virtual void reset(double target);
 
        virtual void reset(double Kp, double Ki, double Kd);
 
        virtual void reset(double Kp, double Ki, double Kd, double target);
 
        virtual double encoderToAngle(int32_t encoder) = 0;
    };
 
    class Velocity_PID : public Controller
    {
    private:
        double p;
        double vLPF = 0.0;
 
    public:
        Velocity_PID(uint8_t motorNo, uint8_t encoderNo, enum class encoderType encoderType, uint16_t CPR, double Kp, double Ki, double Kd, double target, bool direction, double capableDutyCycle = 1.0, double p = 1.0, bool logging = false);
        void setLPF(double p);
        double encoderToAngle(int32_t encoder) override;
        double compute() override;
        void reset() override;
        void reset(double target) override;
        void reset(double Kp, double Ki, double Kd) override;
        void reset(double Kp, double Ki, double Kd, double target) override;
    };
 
    class Position_PID : public Controller
    {
    private:
        int8_t loopCount = 0;
 
    public:
        Position_PID(uint8_t motorNo, uint8_t encoderNo, enum class encoderType encoderType, uint16_t CPR, double Kp, double Ki, double Kd, double target, bool direction, double capableDutyCycle = 1.0, bool logging = false);
 
        void setTarget(double target) override;
        double encoderToAngle(int32_t encoder) override;
        double compute() override;
    };
 
    // Definition
 
    inline double Controller::compute_PID(const double current)
    {
        return dutyCycle = this->pid.compute_PID(current, logging);
    }
    inline void Controller::setTarget(const double target)
    {
        this->pid.setTarget(target);
    }
    inline void Controller::setGains(const double Kp, const double Ki, const double Kd)
    {
        this->pid.setGains(abs(Kp), abs(Ki), abs(Kd));
    }
    inline void Controller::setKp(const double Kp)
    {
        this->pid.setKp(abs(Kp));
    }
    inline void Controller::setKi(const double Ki)
    {
        this->pid.setKi(abs(Ki));
    }
    inline void Controller::setKd(const double Kd)
    {
        this->pid.setKd(abs(Kd));
    }
    inline double Controller::getTarget() const
    {
        return this->pid.getTarget();
    }
    inline double Controller::getCurrent() const
    {
        return this->pid.getCurrent();
    }
    inline double Controller::getDutyCycle() const
    {
        return this->dutyCycle;
    }
    inline double Controller::getDt() const
    {
        return this->pid.getDt();
    }
    inline double Velocity_PID::encoderToAngle(const int32_t encoder)
    {
        return Cubic_controller::encoderToAngle(encoder, this->CPR, 0, false);
    }
    inline double Position_PID::encoderToAngle(const int32_t encoder)
    {
        double angle = Cubic_controller::encoderToAngle(encoder, this->CPR, -PI, true);
        static double prevAngle = angle;
        double actualAngle = angle;
 
        if (actualAngle < -LOOP_THRESHOLD && prevAngle > LOOP_THRESHOLD)
        {
            this->loopCount++;
        }
        else if (actualAngle > LOOP_THRESHOLD && prevAngle < -LOOP_THRESHOLD)
        {
            this->loopCount--;
        }
        prevAngle = actualAngle;
 
        return angle + TWO_PI * this->loopCount;
    }
    inline int32_t Controller::readEncoder() const
    {
        int32_t value = encoderType == encoderType::inc ? Inc_enc::get_diff(encoderNo) : Abs_enc::get(encoderNo);
        if (logging)
        {
            Serial.print("encoder:");
            Serial.print(value);
            Serial.print(",");
        }
        return value;
    }
    inline void Velocity_PID::setLPF(const double p)
    {
        this->p = p;
    }
    inline void Controller::reset()
    {
        this->pid.reset();
    }
    inline void Controller::reset(const double target)
    {
        this->pid.reset(target);
    }
    inline void Controller::reset(const double Kp, const double Ki, const double Kd)
    {
        this->pid.reset(Kp, Ki, Kd);
    }
    inline void Controller::reset(const double Kp, const double Ki, const double Kd, const double target)
    {
        this->pid.reset(Kp, Ki, Kd, target);
    }
    inline void Velocity_PID::reset()
    {
        Controller::reset();
        this->vLPF = 0;
    }
    inline void Velocity_PID::reset(const double target)
    {
        Controller::reset(target);
        this->vLPF = 0;
    }
    inline void Velocity_PID::reset(const double Kp, const double Ki, const double Kd)
    {
        Controller::reset(Kp, Ki, Kd);
        this->vLPF = 0;
    }
    inline void Velocity_PID::reset(const double Kp, const double Ki, const double Kd, const double target)
    {
        Controller::reset(Kp, Ki, Kd, target);
        this->vLPF = 0;
    }
}