hidaka sato / PID2

Universal PID library

Dependents:   WRS2019_master FourOmniMecha WRS2020_mecanum_node

PID.cpp@0:873985df821c, 2019-12-14 (annotated)

Committer:
sgrsn
Date:
Sat Dec 14 12:23:58 2019 +0000
Revision:
0:873985df821c
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:873985df821c 1 #include "PID.h"
sgrsn 0:873985df821c 2
sgrsn 0:873985df821c 3 PID::PID(Timer *T)
sgrsn 0:873985df821c 4 {
sgrsn 0:873985df821c 5 integral = 0;
sgrsn 0:873985df821c 6 last_error = 0;
sgrsn 0:873985df821c 7 current_time = 0;
sgrsn 0:873985df821c 8 last_time = 0;
sgrsn 0:873985df821c 9
sgrsn 0:873985df821c 10 timer = T;
sgrsn 0:873985df821c 11 timer -> start();
sgrsn 0:873985df821c 12 }
sgrsn 0:873985df821c 13
sgrsn 0:873985df821c 14 float PID::controlPID(float target, float current)
sgrsn 0:873985df821c 15 {
sgrsn 0:873985df821c 16 current_time = timer -> read();
sgrsn 0:873985df821c 17 float error = target - current;
sgrsn 0:873985df821c 18 float dt = current_time - last_time;
sgrsn 0:873985df821c 19 integral += ( error + last_error ) / 2 * dt;
sgrsn 0:873985df821c 20 float differential = ( error - last_error ) / dt;
sgrsn 0:873985df821c 21 float control = Kp * error + Ki * integral + Kd * differential;
sgrsn 0:873985df821c 22 last_error = error;
sgrsn 0:873985df821c 23 last_time = current_time;
sgrsn 0:873985df821c 24 return control;
sgrsn 0:873985df821c 25 }
sgrsn 0:873985df821c 26
sgrsn 0:873985df821c 27 float PID::controlP(float target, float current, float new_Kp)
sgrsn 0:873985df821c 28 {
sgrsn 0:873985df821c 29 float error = target - current;
sgrsn 0:873985df821c 30 float control = new_Kp * error;
sgrsn 0:873985df821c 31 return control;
sgrsn 0:873985df821c 32 }
sgrsn 0:873985df821c 33 float PID::controlPI(float target, float current)
sgrsn 0:873985df821c 34 {
sgrsn 0:873985df821c 35 current_time = timer -> read();
sgrsn 0:873985df821c 36 float error = target - current;
sgrsn 0:873985df821c 37 float dt = current_time - last_time;
sgrsn 0:873985df821c 38 integral += (error + last_error) / 2 * dt;
sgrsn 0:873985df821c 39 float control = Kp * error + Ki * integral;
sgrsn 0:873985df821c 40 last_error = error;
sgrsn 0:873985df821c 41 last_time = timer->read();
sgrsn 0:873985df821c 42 return control;
sgrsn 0:873985df821c 43 }
sgrsn 0:873985df821c 44 void PID::setParameter(float new_Kp, float new_Ki, float new_Kd)
sgrsn 0:873985df821c 45 {
sgrsn 0:873985df821c 46 Kp = new_Kp;
sgrsn 0:873985df821c 47 Ki = new_Ki;
sgrsn 0:873985df821c 48 Kd = new_Kd;
sgrsn 0:873985df821c 49 }
sgrsn 0:873985df821c 50 void PID::setParameter(float new_Ku, float new_Pu)
sgrsn 0:873985df821c 51 {
sgrsn 0:873985df821c 52 Ku = new_Ku;
sgrsn 0:873985df821c 53 Pu = new_Pu;
sgrsn 0:873985df821c 54
sgrsn 0:873985df821c 55 Kp = 0.60 * Ku;
sgrsn 0:873985df821c 56 Ti = 0.50 * Pu;
sgrsn 0:873985df821c 57 Td = 0.125 * Pu;
sgrsn 0:873985df821c 58 Ki = (1 / Ti) * Kp;
sgrsn 0:873985df821c 59 Kd = Td * Kp;
sgrsn 0:873985df821c 60 }
sgrsn 0:873985df821c 61 void PID::setParameterPI(float new_Ku, float new_Pu)
sgrsn 0:873985df821c 62 {
sgrsn 0:873985df821c 63 Kp = 0.45 * Ku;
sgrsn 0:873985df821c 64 Ti = 0.83 * Pu;
sgrsn 0:873985df821c 65 Ki = (1 / Ti) * Kp;
sgrsn 0:873985df821c 66 }
sgrsn 0:873985df821c 67 void PID::reset()
sgrsn 0:873985df821c 68 {
sgrsn 0:873985df821c 69 integral = 0;
sgrsn 0:873985df821c 70 last_error = 0;
sgrsn 0:873985df821c 71 last_time = timer -> read();
sgrsn 0:873985df821c 72 }