PID.h

Go to the documentation of this file.

//This adds an unstable change to make the D component low pass (avg  itself)
#define FUNNY_PID
 
#ifndef FUNNY_PID
 
//
// Created by ankit on 1/31/23.
//
 
#ifndef TR_EMBEDDED_PID_H
#define TR_EMBEDDED_PID_H
 
#include "mbed.h"
 
class PID {
private:
    float kP = 0;
    float kI = 0;
    float kD = 0;
 
    double integralCap = 0;
    float outputCap = 0;
 
    float lastError = 0;
    double errorIntegral = 0;
 
public:
    float feedForward = 0;
 
    float pC, iC, dC = 0;
 
    PID();
 
    PID(float kP, float kI, float kD, float integralCap = 0, float outputCap = 0);
 
    void setPID(float p, float i, float d, float integralCap = 0, float outputCap = 0);
 
    void resetPID(float kP, float kI, float kD, float integralCap = 0, float outputCap = 0);
 
    void resetErrorIntegral();
 
 
    int calculate(int desired, int current, double dt);
 
    int calculatePeriodic(float error, double dt);
 
    void limitOutput(double &PIDCalc) const;
 
    void limitErrorIntegral();
 
    void setIntegralCap(float integralCap);
 
    void setOutputCap(float outCap);
 
};
 
#endif //TR_EMBEDDED_PID_H
#else
//
// Created by ankit on 1/31/23.
//
 
#ifndef TR_EMBEDDED_PID_H
#define TR_EMBEDDED_PID_H
 
#include "mbed.h"
#define lastX 10
 
struct lastfew {
    long long last[lastX] = { 0 };
    int arm = 0;
    int lastY = 1;
 
    lastfew(int lastY_ = 1) {
        if(lastY_ > lastX){
            lastY = lastX;
        }else{
            lastY = lastY_;
        }
    }
 
    void add(long long l) {
        last[arm] = l;
        arm++;
        if (arm == lastY)
            arm = 0;
    }
 
    long long time() {
        long long t = 0;
        for (int i = 0; i < lastY; i++)
            t += last[i];
        return t / lastY;
    }
};
 
class PID {
private:
    float kP = 0;
    float kI = 0;
    float kD = 0;
 
    double integralCap = 0;
    double derivativeCap = 0;
    float outputCap = 0;
 
    float lastError = 0;
    double errorIntegral = 0;
 
public:
 
    struct config
    {
        float p = 1;
        float i = 0;
        float d = 0;
        float outCap = 15000;
        float integralCap = 500;
    };
 
 
    float feedForward = 0;
 
    float pC, iC, dC = 0;
 
    lastfew dBuffer;
 
    PID();
 
    PID(float kP, float kI, float kD, float integralCap = 0, float outputCap = 0);
 
    PID(config cfg);
 
    void setPID(float p, float i, float d, float integralCap = 0, float outputCap = 0);
 
    void resetPID(float kP, float kI, float kD, float integralCap = 0, float outputCap = 0);
 
    void resetErrorIntegral();
 
 
    int calculate(int desired, int current, double dt);
 
    int calculatePeriodic(float error, double dt);
 
    void limitOutput(double &PIDCalc) const;
 
    double limitErrorDerivative(double dTerm);
 
    void limitErrorIntegral();
 
    void setDerivativeCap(float derivativeCap);
 
    void setIntegralCap(float integralCap);
 
    void setOutputCap(float outCap);
 
};
 
 
 
#endif //TR_EMBEDDED_PID_H
#endif