PID控制算法

#include <algorithm>
#include <cmath>
#include <chrono>
#include <ros/time.h>
#include "Configure.hpp"

// todo 自动计算dt

class PIDController
{
public:
    void init(double kpIn, double kiIn, double kdIn, double maxVIn, double maxAIn);
    double calculate(double setpoint, double current, double currentV);

private:
    bool initialized = false;
    double kp;
    double ki;
    double kd;
    double maxV;
    double maxA;
    double prevError;
    double integral;
    double prevTime;
};

#define RATE 10.0

void PIDController::init(double kpIn, double kiIn, double kdIn, double maxVIn, double maxAIn)
{
    kp = kpIn;
    ki = kiIn;
    kd = kdIn;
    maxV = maxVIn;
    maxA = maxAIn;
    prevError = 0;
    integral = 0;
    initialized = true;
    prevTime = ros::Time::now().toSec() - 1.0 / RATE;
}

double PIDController::calculate(double setpoint, double current, double currentV)
{
    if (!initialized) {
        std::abort();
    }
    double dt = ros::Time::now().toSec() - prevTime;
    prevTime = ros::Time::now().toSec();

    double error = setpoint - current;
    integral += error * dt;
    double derivative = (error - prevError) / dt;
    prevError = error;
    double output = kp * error + ki * integral + kd * derivative;
    // 限制最大速度
    if (std::abs(output) > maxV) {
        output = std::copysign(maxV, output);
    }
    // 限制最大加速度
    else if (std::abs(output - currentV) / dt > maxA) {
        output = currentV + std::copysign(maxA * dt, output - currentV);
    }
    return output;
}