![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
割り込みテスト用(28は間違い)
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_27_test by
Diff: pid/pid.cpp
- Revision:
- 0:17f575135219
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid/pid.cpp Fri Sep 07 04:11:48 2018 +0000 @@ -0,0 +1,73 @@ +#include "mbed.h" +#include "pid.h" + + +PID::PID(void){ + initialize(); +} + +PID::PID(double Pgain, double Igain, double Dgain){ + initialize(); + setPIDgain(Pgain,Igain,Dgain); +} + +PID::PID(double Pgain, double Igain, double Dgain, double Max, double Min){ + initialize(); + setPIDgain(Pgain,Igain,Dgain); + setMaxMin(Max,Min); +} + +PID::~PID(){ + +} + +void PID::initialize(void){ + kp = 0.0; ki = 0.0; kd = 0.0; + max = 0.0; min = 0.0; + dt = 0.0; + integral = 0.0; + for(uint8_t i=0; i<2; i++){ + oldval[i] = 0.0; + diff[i] = 0.0; + } + maxcheck = false; + mincheck = false; +} + +void PID::setPIDgain(double Pgain, double Igain, double Dgain){ + kp = Pgain; + ki = Igain; + kd = Dgain; +} + +void PID::setMaxMin(double Max, double Min){ + if(Max < Min) return; //最大値<最小値であれば設定せずに終了 + max = Max; + min = Min; + maxcheck = true; mincheck = true; +} + +void PID::switchMaxMin(bool Maxcheck, bool Mincheck){ + maxcheck = Maxcheck; + mincheck = Mincheck; +} + +double PID::calcPID(double nowval, double targetval, double dt){ + double p,i,d,pid; + + diff[1] = diff[0]; + diff[0] = nowval - targetval; + if(diff[1] == 0.0) return 0.0; //前回の値がない場合,0.0を返す + + integral += (diff[0] + diff[1]) / 2.0 * dt; + + p = kp * diff[0]; + i = ki * integral; + d = kd * (diff[0] - diff[1]) / dt; + pid = p + i + d; + if(maxcheck && pid>max) pid = max; + if(mincheck && pid<min) pid = min; + + return pid; + +}