航空研究会 / Mbed 2 deprecated Autoflight2022_913

test

Dependencies:   BMP280 HCSR04_from_mtmt MPU6050_2 mbed SDFileSystem3

pid/pid.cpp@2:7663d92d33ce, 2022-09-14 (annotated)

Committer:
hitonari
Date:
Wed Sep 14 14:31:16 2022 +0000
Revision:
2:7663d92d33ce
Parent:
0:2b57931c6ed4 
9/14 trim change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hitonari 0:2b57931c6ed4 1 #include "mbed.h"
hitonari 0:2b57931c6ed4 2 #include "pid.h"
hitonari 0:2b57931c6ed4 3
hitonari 0:2b57931c6ed4 4
hitonari 0:2b57931c6ed4 5 PID::PID(void){
hitonari 0:2b57931c6ed4 6 initialize();
hitonari 0:2b57931c6ed4 7 }
hitonari 0:2b57931c6ed4 8
hitonari 0:2b57931c6ed4 9 PID::PID(double Pgain, double Igain, double Dgain){
hitonari 0:2b57931c6ed4 10 initialize();
hitonari 0:2b57931c6ed4 11 setPIDgain(Pgain,Igain,Dgain);
hitonari 0:2b57931c6ed4 12 }
hitonari 0:2b57931c6ed4 13
hitonari 0:2b57931c6ed4 14 PID::PID(double Pgain, double Igain, double Dgain, double Max, double Min){
hitonari 0:2b57931c6ed4 15 initialize();
hitonari 0:2b57931c6ed4 16 setPIDgain(Pgain,Igain,Dgain);
hitonari 0:2b57931c6ed4 17 setMaxMin(Max,Min);
hitonari 0:2b57931c6ed4 18 }
hitonari 0:2b57931c6ed4 19
hitonari 0:2b57931c6ed4 20 PID::~PID(){
hitonari 0:2b57931c6ed4 21
hitonari 0:2b57931c6ed4 22 }
hitonari 0:2b57931c6ed4 23
hitonari 0:2b57931c6ed4 24 void PID::initialize(void){
hitonari 0:2b57931c6ed4 25 kp = 0.0; ki = 0.0; kd = 0.0;
hitonari 0:2b57931c6ed4 26 max = 0.0; min = 0.0;
hitonari 0:2b57931c6ed4 27 dt = 0.0;
hitonari 0:2b57931c6ed4 28 integral = 0.0;
hitonari 0:2b57931c6ed4 29 for(uint8_t i=0; i<2; i++){
hitonari 0:2b57931c6ed4 30 oldval[i] = 0.0;
hitonari 0:2b57931c6ed4 31 diff[i] = 0.0;
hitonari 0:2b57931c6ed4 32 }
hitonari 0:2b57931c6ed4 33 maxcheck = false;
hitonari 0:2b57931c6ed4 34 mincheck = false;
hitonari 0:2b57931c6ed4 35 }
hitonari 0:2b57931c6ed4 36
hitonari 0:2b57931c6ed4 37 void PID::setPIDgain(double Pgain, double Igain, double Dgain){
hitonari 0:2b57931c6ed4 38 kp = Pgain;
hitonari 0:2b57931c6ed4 39 ki = Igain;
hitonari 0:2b57931c6ed4 40 kd = Dgain;
hitonari 0:2b57931c6ed4 41 }
hitonari 0:2b57931c6ed4 42
hitonari 0:2b57931c6ed4 43 void PID::setMaxMin(double Max, double Min){
hitonari 0:2b57931c6ed4 44 if(Max < Min) return; //最大値<最小値であれば設定せずに終了
hitonari 0:2b57931c6ed4 45 max = Max;
hitonari 0:2b57931c6ed4 46 min = Min;
hitonari 0:2b57931c6ed4 47 maxcheck = true; mincheck = true;
hitonari 0:2b57931c6ed4 48 }
hitonari 0:2b57931c6ed4 49
hitonari 0:2b57931c6ed4 50 void PID::switchMaxMin(bool Maxcheck, bool Mincheck){
hitonari 0:2b57931c6ed4 51 maxcheck = Maxcheck;
hitonari 0:2b57931c6ed4 52 mincheck = Mincheck;
hitonari 0:2b57931c6ed4 53 }
hitonari 0:2b57931c6ed4 54
hitonari 0:2b57931c6ed4 55 double PID::calcPID(double nowval, double targetval, double dt){
hitonari 0:2b57931c6ed4 56 double p,i,d,pid;
hitonari 0:2b57931c6ed4 57
hitonari 0:2b57931c6ed4 58 diff[1] = diff[0];
hitonari 0:2b57931c6ed4 59 diff[0] = nowval - targetval;
hitonari 0:2b57931c6ed4 60 if(diff[1] == 0.0) return 0.0; //前回の値がない場合，0.0を返す
hitonari 0:2b57931c6ed4 61
hitonari 0:2b57931c6ed4 62 integral += (diff[0] + diff[1]) / 2.0 * dt;
hitonari 0:2b57931c6ed4 63
hitonari 0:2b57931c6ed4 64 p = kp * diff[0];
hitonari 0:2b57931c6ed4 65 i = ki * integral;
hitonari 0:2b57931c6ed4 66 d = kd * (diff[0] - diff[1]) / dt;
hitonari 0:2b57931c6ed4 67 pid = p + i + d;
hitonari 0:2b57931c6ed4 68 if(maxcheck && pid>max) pid = max;
hitonari 0:2b57931c6ed4 69 if(mincheck && pid<min) pid = min;
hitonari 0:2b57931c6ed4 70
hitonari 0:2b57931c6ed4 71 return pid;
hitonari 0:2b57931c6ed4 72
hitonari 0:2b57931c6ed4 73 }