PID.cpp

00001 /*
00002 * PID.cpp
00003 *
00004 * Created: 2016/07/01 17:47:16
00005 *  Author: yuuki
00006 */
00007 
00008 #include "PID.h"
00009 #include <stdlib.h>
00010 
00011 namespace PID_SPACE
00012 {
00013     
00014     PID::PID(double deltaTime)
00015     {
00016         this->deltaTime = deltaTime;
00017         this->dataRangeLower = 0;
00018         this->dataRangeUpper = 100;
00019         kp = 0;
00020         ki = 0;
00021         kd = 0;
00022     }
00023     
00024     PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
00025     {
00026         this->deltaTime = deltaTime;
00027         this->dataRangeLower = dataRangeLower;
00028         this->dataRangeUpper = dataRangeUpper;
00029         kp = 0;
00030         ki = 0;
00031         kd = 0;
00032     }
00033     
00034     PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
00035     {
00036         this->deltaTime = deltaTime;
00037         this->dataRangeLower = dataRangeLower;
00038         this->dataRangeUpper = dataRangeUpper;
00039         kp = KP;
00040         ki = KI;
00041         kd = KD;
00042     }
00043     
00044     void PID::SetParam(double KP, double KI, double KD)
00045     {
00046         kp = KP;
00047         ki = KI;
00048         kd = KD;
00049     }
00050     
00051     double PID::SetPV(double sensorData, double targetData)
00052     {
00053         double p, i, d;
00054         
00055         diff[0] = diff[1];
00056         diff[1] = sensorData - targetData;
00057         integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
00058         
00059         p = kp * diff[1];
00060         i = ki * integral;
00061         d = kd * ((diff[1] - diff[0]) / deltaTime);
00062         mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
00063         return mv;
00064     }
00065     
00066     double PID::GetMV()
00067     {
00068         return mv;
00069     }
00070     
00071     double PID::limit(double data,double lower,double upper)
00072     {
00073         if(data < lower)        return lower;
00074         else if(data > upper)   return upper;
00075         else                    return data;
00076     }
00077 
00078     //速度系PID関数
00079     double PID::SetSpeed(double sensorData,double targetData)
00080     {
00081         //TODO:速度単位系の変換
00082     }
00083     
00084 }