PIDclass.cpp

00001 #include "PIDclass.h"
00002 
00003 // コンストラクタPIDパラメータを引数を用いて初期化する
00004 PID::PID(float xKp, float xKi, float xKd, float xint_time)
00005 {
00006     Kp = xKp;
00007     Ki = xKi;
00008     Kd = xKd;
00009     int_time = xint_time;
00010 
00011     preError = 0.0; // 1個前のエラーの値
00012     intError = 0.0; // 積分値の初期化
00013 
00014     init_done = false;
00015 }
00016 
00017 void PID::PIDinit(float ref, float act)
00018 {
00019     preError = ref - act;
00020     intError = 0.0; // 積分値の初期化
00021 
00022     init_done = true;
00023 }
00024 
00025 // PID制御の実体部
00026 float PID::getCmd(float ref, float act, float maxcmd)
00027 {
00028     float cmd, Error, dError;
00029     cmd = 0.0;
00030 
00031     if(init_done) {
00032         Error = ref - act;
00033         cmd += Error * Kp;
00034 
00035         dError = (Error - preError);// / int_time; int_timeが0.01のときdErrorの値が大きくなりすぎてしまうのでコメントアウト
00036         cmd += dError * Kd;
00037 
00038         intError += (Error + preError) / 2 * int_time;
00039         cmd += intError * Ki;
00040 
00041         preError = Error;
00042 
00043         if(cmd > maxcmd) cmd = maxcmd;
00044         if(cmd < -maxcmd) cmd = -maxcmd;
00045     }
00046     return cmd;
00047 }
00048 
00049 void PID::setPara(float xKp, float xKi, float xKd)
00050 {
00051     Kp = xKp;
00052     Ki = xKi;
00053     Kd = xKd;
00054 }