lknds
Dependencies: mbed TrapezoidControl Pulse QEI
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 }
Generated on Sun Jul 17 2022 15:33:13 by 1.7.2