Universal PID library
Dependents: WRS2019_master FourOmniMecha WRS2020_mecanum_node
PID.cpp
- Committer:
- sgrsn
- Date:
- 2019-12-14
- Revision:
- 0:873985df821c
File content as of revision 0:873985df821c:
#include "PID.h" PID::PID(Timer *T) { integral = 0; last_error = 0; current_time = 0; last_time = 0; timer = T; timer -> start(); } float PID::controlPID(float target, float current) { current_time = timer -> read(); float error = target - current; float dt = current_time - last_time; integral += ( error + last_error ) / 2 * dt; float differential = ( error - last_error ) / dt; float control = Kp * error + Ki * integral + Kd * differential; last_error = error; last_time = current_time; return control; } float PID::controlP(float target, float current, float new_Kp) { float error = target - current; float control = new_Kp * error; return control; } float PID::controlPI(float target, float current) { current_time = timer -> read(); float error = target - current; float dt = current_time - last_time; integral += (error + last_error) / 2 * dt; float control = Kp * error + Ki * integral; last_error = error; last_time = timer->read(); return control; } void PID::setParameter(float new_Kp, float new_Ki, float new_Kd) { Kp = new_Kp; Ki = new_Ki; Kd = new_Kd; } void PID::setParameter(float new_Ku, float new_Pu) { Ku = new_Ku; Pu = new_Pu; Kp = 0.60 * Ku; Ti = 0.50 * Pu; Td = 0.125 * Pu; Ki = (1 / Ti) * Kp; Kd = Td * Kp; } void PID::setParameterPI(float new_Ku, float new_Pu) { Kp = 0.45 * Ku; Ti = 0.83 * Pu; Ki = (1 / Ti) * Kp; } void PID::reset() { integral = 0; last_error = 0; last_time = timer -> read(); }