Skip to content

Instantly share code, notes, and snippets.

@rioki
Last active Jan 5, 2021
Embed
What would you like to do?
// PID Controller
// Copyright 2017 Sean Farrell <sean.farrell@rioki.org>
//
// This program is free software. It comes without any warranty, to
// the extent permitted by applicable law. You can redistribute it
// and/or modify it under the terms of the Do What The Fuck You Want
// To Public License, Version 2, as published by Sam Hocevar.
// See http://www.wtfpl.net/ for more details.
#include "PidController.h"
#include <algorithm>
PidController::PidController(float _kP, float _kI, float _kD)
: kP(_kP), kI(_kI), kD(_kD) {}
void PidController::calibrate(float _kP, float _kI, float _kD)
{
kP = _kP;
kI = _kI;
kD = _kD;
}
float PidController::control(float setpoint, float processValue, float dt)
{
float error = setpoint - processValue;
// proportional
float pOut = kP * error;
// integral
integral = (integral * std::max(INTEGRAL_WINDOW - dt, 0.0f)) + error * dt;
float iOut = kI * integral;
// derivative
float derivative = (error - lastError) / dt;
float dOut = kD * derivative;
lastError = error;
return pOut + iOut + dOut;
}
void PidController::reset()
{
lastError = 0.0f;
integral = 0.0f;
}
// PID Controller
// Copyright 2017 Sean Farrell <sean.farrell@rioki.org>
//
// This program is free software. It comes without any warranty, to
// the extent permitted by applicable law. You can redistribute it
// and/or modify it under the terms of the Do What The Fuck You Want
// To Public License, Version 2, as published by Sam Hocevar.
// See http://www.wtfpl.net/ for more details.
#ifndef _PID_CONTROLLER_H_
#define _PID_CONTROLLER_H_
//! PID Controller
//!
//! @note Only one process can be controlled with one instance of the
//! PID controller because error and integral values are carried over
//! from one call of control to the next.
class PidController
{
public:
//! Construct a PID controller
//!
//! @param kP the proportional control factor
//! @param kI the integral control factor
//! @param kD the derivative control factor
explicit PidController(float kP, float kI, float kD);
//! Configure the controller with a new set of control factors.
//!
//! @param kP the proportional control factor
//! @param kI the integral control factor
//! @param kD the derivative control factor
//!
//! @note It may be advisable to call reset after recalibrating the PID
//! controller to remove the control memory. This will result in
//! control bounce though and may not be desired.
void calibrate(float kP, float kI, float kD);
//! Compute the control value with given setpoint and process value.
//!
//! @param setpoint the value to controll towards
//! @param processValue the current value of the process under control
//! @param dt the delta duration since the last call to control
float control(float setpoint, float processValue, float dt);
//! Reset the controller error and integral values
//!
//! @note You should call the reset function each time the process
//! under control was significantly changed. This will remove any
//! memory of the previous process state and reduce potential knock effect.
void reset();
private:
float kP = 1.0f;
float kI = 0.0f;
float kD = 0.0f;
float lastError = 0.0f;
float integral = 0.0f;
// Size to consider for the integral memory in seconds.
constexpr static auto INTEGRAL_WINDOW = 1.0f;
};
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment