大季 矢花 / Mbed 2 deprecated 2019_Master_9_30_last

aa

Dependencies:   mbed TrapezoidControl QEI

CommonLibraries/PID/PID.cpp@4:ba9df71868df, 2018-10-01 (annotated)

Committer:
7ka884
Date:
Mon Oct 01 13:47:19 2018 +0000
Revision:
4:ba9df71868df
huhuhuh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
7ka884 4:ba9df71868df 1 /*
7ka884 4:ba9df71868df 2 * PID.cpp
7ka884 4:ba9df71868df 3 *
7ka884 4:ba9df71868df 4 * Created: 2016/07/01 17:47:16
7ka884 4:ba9df71868df 5 * Author: yuuki
7ka884 4:ba9df71868df 6 */
7ka884 4:ba9df71868df 7
7ka884 4:ba9df71868df 8 #include "PID.h"
7ka884 4:ba9df71868df 9 #include <stdlib.h>
7ka884 4:ba9df71868df 10
7ka884 4:ba9df71868df 11 namespace PID_SPACE
7ka884 4:ba9df71868df 12 {
7ka884 4:ba9df71868df 13
7ka884 4:ba9df71868df 14 PID::PID(double deltaTime)
7ka884 4:ba9df71868df 15 {
7ka884 4:ba9df71868df 16 this->deltaTime = deltaTime;
7ka884 4:ba9df71868df 17 this->dataRangeLower = 0;
7ka884 4:ba9df71868df 18 this->dataRangeUpper = 100;
7ka884 4:ba9df71868df 19 kp = 0;
7ka884 4:ba9df71868df 20 ki = 0;
7ka884 4:ba9df71868df 21 kd = 0;
7ka884 4:ba9df71868df 22 }
7ka884 4:ba9df71868df 23
7ka884 4:ba9df71868df 24 PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
7ka884 4:ba9df71868df 25 {
7ka884 4:ba9df71868df 26 this->deltaTime = deltaTime;
7ka884 4:ba9df71868df 27 this->dataRangeLower = dataRangeLower;
7ka884 4:ba9df71868df 28 this->dataRangeUpper = dataRangeUpper;
7ka884 4:ba9df71868df 29 kp = 0;
7ka884 4:ba9df71868df 30 ki = 0;
7ka884 4:ba9df71868df 31 kd = 0;
7ka884 4:ba9df71868df 32 }
7ka884 4:ba9df71868df 33
7ka884 4:ba9df71868df 34 PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
7ka884 4:ba9df71868df 35 {
7ka884 4:ba9df71868df 36 this->deltaTime = deltaTime;
7ka884 4:ba9df71868df 37 this->dataRangeLower = dataRangeLower;
7ka884 4:ba9df71868df 38 this->dataRangeUpper = dataRangeUpper;
7ka884 4:ba9df71868df 39 kp = KP;
7ka884 4:ba9df71868df 40 ki = KI;
7ka884 4:ba9df71868df 41 kd = KD;
7ka884 4:ba9df71868df 42 }
7ka884 4:ba9df71868df 43
7ka884 4:ba9df71868df 44 void PID::SetParam(double KP, double KI, double KD)
7ka884 4:ba9df71868df 45 {
7ka884 4:ba9df71868df 46 kp = KP;
7ka884 4:ba9df71868df 47 ki = KI;
7ka884 4:ba9df71868df 48 kd = KD;
7ka884 4:ba9df71868df 49 }
7ka884 4:ba9df71868df 50
7ka884 4:ba9df71868df 51 double PID::SetPV(double sensorData, double targetData)
7ka884 4:ba9df71868df 52 {
7ka884 4:ba9df71868df 53 double p, i, d;
7ka884 4:ba9df71868df 54
7ka884 4:ba9df71868df 55 diff[0] = diff[1];
7ka884 4:ba9df71868df 56 diff[1] = sensorData - targetData;
7ka884 4:ba9df71868df 57 integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
7ka884 4:ba9df71868df 58
7ka884 4:ba9df71868df 59 p = kp * diff[1];
7ka884 4:ba9df71868df 60 i = ki * integral;
7ka884 4:ba9df71868df 61 d = kd * ((diff[1] - diff[0]) / deltaTime);
7ka884 4:ba9df71868df 62 mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
7ka884 4:ba9df71868df 63 return mv;
7ka884 4:ba9df71868df 64 }
7ka884 4:ba9df71868df 65
7ka884 4:ba9df71868df 66 double PID::GetMV()
7ka884 4:ba9df71868df 67 {
7ka884 4:ba9df71868df 68 return mv;
7ka884 4:ba9df71868df 69 }
7ka884 4:ba9df71868df 70
7ka884 4:ba9df71868df 71 double PID::limit(double data,double lower,double upper)
7ka884 4:ba9df71868df 72 {
7ka884 4:ba9df71868df 73 if(data < lower) return lower;
7ka884 4:ba9df71868df 74 else if(data > upper) return upper;
7ka884 4:ba9df71868df 75 else return data;
7ka884 4:ba9df71868df 76 }
7ka884 4:ba9df71868df 77
7ka884 4:ba9df71868df 78 //速度系PID関数
7ka884 4:ba9df71868df 79 double PID::SetSpeed(double sensorData,double targetData)
7ka884 4:ba9df71868df 80 {
7ka884 4:ba9df71868df 81 //TODO:速度単位系の変換
7ka884 4:ba9df71868df 82 }
7ka884 4:ba9df71868df 83
7ka884 4:ba9df71868df 84 }