pid.cpp

00001 #include "mbed.h"
00002 #include "pid.h"
00003 
00004 
00005 PID::PID(void){
00006     initialize();
00007 }
00008 
00009 PID::PID(double Pgain, double Igain, double Dgain){
00010     initialize();
00011     setPIDgain(Pgain,Igain,Dgain);
00012 }
00013 
00014 PID::PID(double Pgain, double Igain, double Dgain, double Max, double Min){
00015     initialize();
00016     setPIDgain(Pgain,Igain,Dgain);
00017     setMaxMin(Max,Min);
00018 }
00019 
00020 PID::~PID(){
00021 
00022 }
00023 
00024 void PID::initialize(void){
00025     kp = 0.0;   ki = 0.0;   kd = 0.0;
00026     max = 0.0; min = 0.0;
00027     dt = 0.0;
00028     integral = 0.0;
00029     for(uint8_t i=0; i<2; i++){
00030         oldval[i] = 0.0;
00031         diff[i] = 0.0;
00032     }
00033     maxcheck = false;
00034     mincheck = false;
00035 }
00036 
00037 void PID::setPIDgain(double Pgain, double Igain, double Dgain){
00038     kp = Pgain;
00039     ki = Igain;
00040     kd = Dgain;
00041 }
00042 
00043 void PID::setMaxMin(double Max, double Min){
00044     if(Max < Min) return;   //最大値<最小値であれば設定せずに終了
00045     max = Max;
00046     min = Min;
00047     maxcheck = true; mincheck = true;
00048 }
00049 
00050 void PID::switchMaxMin(bool Maxcheck, bool Mincheck){
00051     maxcheck = Maxcheck;
00052     mincheck = Mincheck;
00053 }
00054 
00055 double PID::calcPID(double nowval, double targetval, double dt){
00056     double p,i,d,pid;
00057 
00058     diff[1] = diff[0];
00059     diff[0] = nowval - targetval;
00060     if(diff[1] == 0.0) return 0.0;   //前回の値がない場合，0.0を返す
00061 
00062     integral += (diff[0] + diff[1]) / 2.0 * dt;
00063 
00064     p = kp * diff[0];
00065     i = ki * integral;
00066     d = kd * (diff[0] - diff[1]) / dt;
00067     pid = p + i + d;
00068     if(maxcheck && pid>max) pid = max;
00069     if(mincheck && pid<min) pid = min;
00070 
00071     return pid;
00072 
00073 }