航空研究会 / Mbed 2 deprecated Autoflight2018_54

フェイルセーフ完成版

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of Autoflight2018_53 by 航空研究会

pid/pid.cpp@43:4413ee61bc39, 2018-09-26 (annotated)

Committer:
taknokolat
Date:
Wed Sep 26 11:26:28 2018 +0000
Revision:
43:4413ee61bc39
Parent:
0:17f575135219 
a;

Who changed what in which revision?

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