航空研究会 / Mbed 2 deprecated sbus_test_2018

a

Dependencies:   HCSR04_2 MPU6050_2 mbed SDFileSystem3

Fork of AutoFlight2018_Control by 航空研究会

pid/pid.cpp@1:09a162f4f6ce, 2018-09-07 (annotated)

Committer:
TUATBM
Date:
Fri Sep 07 03:42:49 2018 +0000
Revision:
1:09a162f4f6ce
Parent:
0:813f5cd20cf1 
a

Who changed what in which revision?

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