miki sumito
/
four_wheel_omni
四輪オムニの足回りを動かすプログラム
Diff: PID/PID.cpp
- Revision:
- 0:39c2bb18192b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID/PID.cpp Sat Oct 08 11:13:31 2022 +0000 @@ -0,0 +1,33 @@ +#include "PID.hpp" +PID::PID(double kp,double ki,double kd,double dt) : Kp(kp),Ki(ki),Kd(kd),dt(dt) +{ + pwm = 0; + e[0] = 0; + e[1] = 0; + integral = 0; +} + +double PID::set(double nowvalue,double tar) +{ + e[1] = tar - nowvalue; + integral += (e[0] + e[1]) * dt / 2; + getP = e[1]; + getI = integral; + getD = (e[1] - e[0]) / dt; + pwm += (getP * Kp) + (getI * Ki) + (getD * Kd); + e[0] = e[1]; + return pwm; +} + +void PID::safety(double max,double min) +{ + pwm >= max ? pwm = max : pwm = pwm; + pwm <= min ? pwm = min : pwm = pwm; +} + +void PID::reset() +{ + pwm = 0; +} + +PID::~PID(){}; \ No newline at end of file