![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
エルロン調節 自動着陸やり直し搭載
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_26 by
pid/pid.cpp
- Committer:
- HARUKIDELTA
- Date:
- 2018-09-07
- Revision:
- 0:17f575135219
File content as of revision 0:17f575135219:
#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; }