航空研究会 / Mbed 2 deprecated AutoFlight2017_now2

aaa

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

pid/pid.cpp@1:f31e17341659, 2018-08-28 (annotated)

Committer:
TUATBM
Date:
Tue Aug 28 07:12:16 2018 +0000
Revision:
1:f31e17341659
Parent:
0:92024886c0be 
aaa

Who changed what in which revision?

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