![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
CommonLibraries/PID/PID.cpp@24:370616a56815, 2019-09-18 (annotated)
- Committer:
- M_souta
- Date:
- Wed Sep 18 02:03:56 2019 +0000
- Revision:
- 24:370616a56815
- Parent:
- 4:ba9df71868df
ii
Who changed what in which revision?
User | Revision | Line number | New 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 | } |