first
Revision 0:4ecdddd912e2, committed 2017-09-24
- Comitter:
- echo_piyo
- Date:
- Sun Sep 24 05:23:58 2017 +0000
- Commit message:
- ????2??
Changed in this revision
pid.cpp | Show annotated file Show diff for this revision Revisions of this file |
pid.h | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.cpp Sun Sep 24 05:23:58 2017 +0000 @@ -0,0 +1,62 @@ +#include "pid.h" + +void PositionPid::setup(float Kp, float Ki, float Kd, float dt){ + kp = Kp; + ki = Ki; + kd = Kd; + time = dt; + frequency = 1/dt; +} + +void PositionPid::calculate(float target, float nowValue){ + old = now; + now = nowValue - target; + p = now; + i = i + (now + old)*0.5*time; + d = (now - old) * frequency; + result = kp*p + ki*i + kd*d; + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } +} + +float PositionPid::duty(){ + return result; +} + +float PositionPid::duty_enableWidth(float duty_min, float duty_max){ + if(now > duty_min && now < duty_max){ //nowは差 + return 0.0; + }else{ + return result; + } +} + +void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){ + kp = Kp; + ki = Ki; + kd = Kd; + time = dt; + frequency = 1/dt; +} + +void SpeedPid::calculate(float target, float nowValue){ + e2 = e1; + e1 = e; + e = nowValue - target; + p = e - e1; + i = e*time; + d = (e-2*e1+e2)*frequency; + result = result + (kp*p+ki*i+kd*d); + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } +} + +float SpeedPid::duty(){ + return result; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.h Sun Sep 24 05:23:58 2017 +0000 @@ -0,0 +1,62 @@ +/********************************************* +PID library + +PositionPid : 位置型 +SpeedPid : 速度型 + +setup(float Kp, float Ki, float Kd, float dt) +パラメータ、制御周期の設定 +Kp : Pゲイン +Ki : Iゲイン +Kd : Dゲイン +dt : 制御周期[s] + +calculate(float target, float nowValue) +PIDの計算をする +target : 目標値 +nowValue : 現在値 + +duty() +計算結果を-1~1で返す +*********************************************/ + +#ifndef MBED_PID_H +#define MBED_PID_H + +#include "mbed.h" + +class PositionPid +{ +public : + void setup(float Kp, float Ki, float Kd, float dt); + + void calculate(float target, float nowValue); + + float duty(); + + float duty_enableWidth(float duty_min, float duty_max); + +private : + float kp, ki, kd, + time, frequency, + old, now, + p, i, d, result; +}; + +class SpeedPid +{ +public : + void setup(float Kp, float Ki, float Kd, float dt); + + void calculate(float target, float nowValue); + + float duty(); + +private : + float kp, ki, kd, + time, frequency, + e, e1, e2, + p, i, d, result; +}; + +#endif \ No newline at end of file