フェイルセーフ完成版
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of Autoflight2018_53 by
pid/pid.cpp@0:17f575135219, 2018-09-07 (annotated)
- Committer:
- HARUKIDELTA
- Date:
- Fri Sep 07 04:11:48 2018 +0000
- Revision:
- 0:17f575135219
SBUS_READ
Who changed what in which revision?
User | Revision | Line number | New 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 | } |