大季 矢花 / Mbed 2 deprecated MB2019_main_11_12last

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Communication/PID/PID.cpp@5:3ae504b88679, 2018-10-01 (annotated)

Committer:
kishibekairohan
Date:
Mon Oct 01 09:07:27 2018 +0000
Revision:
5:3ae504b88679
maruyama

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kishibekairohan 5:3ae504b88679 1 #include "PID.h"
kishibekairohan 5:3ae504b88679 2 #include <stdlib.h>
kishibekairohan 5:3ae504b88679 3
kishibekairohan 5:3ae504b88679 4 namespace PID_SPACE
kishibekairohan 5:3ae504b88679 5 {
kishibekairohan 5:3ae504b88679 6
kishibekairohan 5:3ae504b88679 7 PID::PID(double deltaTime)
kishibekairohan 5:3ae504b88679 8 {
kishibekairohan 5:3ae504b88679 9 this->deltaTime = deltaTime;
kishibekairohan 5:3ae504b88679 10 this->dataRangeLower = 0;
kishibekairohan 5:3ae504b88679 11 this->dataRangeUpper = 100;
kishibekairohan 5:3ae504b88679 12 kp = 0;
kishibekairohan 5:3ae504b88679 13 ki = 0;
kishibekairohan 5:3ae504b88679 14 kd = 0;
kishibekairohan 5:3ae504b88679 15 }
kishibekairohan 5:3ae504b88679 16
kishibekairohan 5:3ae504b88679 17 PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper)
kishibekairohan 5:3ae504b88679 18 {
kishibekairohan 5:3ae504b88679 19 this->deltaTime = deltaTime;
kishibekairohan 5:3ae504b88679 20 this->dataRangeLower = dataRangeLower;
kishibekairohan 5:3ae504b88679 21 this->dataRangeUpper = dataRangeUpper;
kishibekairohan 5:3ae504b88679 22 kp = 0;
kishibekairohan 5:3ae504b88679 23 ki = 0;
kishibekairohan 5:3ae504b88679 24 kd = 0;
kishibekairohan 5:3ae504b88679 25 }
kishibekairohan 5:3ae504b88679 26
kishibekairohan 5:3ae504b88679 27 PID::PID(double deltaTime, double dataRangeLower, double dataRangeUpper, double KP, double KI, double KD)
kishibekairohan 5:3ae504b88679 28 {
kishibekairohan 5:3ae504b88679 29 this->deltaTime = deltaTime;
kishibekairohan 5:3ae504b88679 30 this->dataRangeLower = dataRangeLower;
kishibekairohan 5:3ae504b88679 31 this->dataRangeUpper = dataRangeUpper;
kishibekairohan 5:3ae504b88679 32 kp = KP;
kishibekairohan 5:3ae504b88679 33 ki = KI;
kishibekairohan 5:3ae504b88679 34 kd = KD;
kishibekairohan 5:3ae504b88679 35 }
kishibekairohan 5:3ae504b88679 36
kishibekairohan 5:3ae504b88679 37 void PID::SetParam(double KP, double KI, double KD)
kishibekairohan 5:3ae504b88679 38 {
kishibekairohan 5:3ae504b88679 39 kp = KP;
kishibekairohan 5:3ae504b88679 40 ki = KI;
kishibekairohan 5:3ae504b88679 41 kd = KD;
kishibekairohan 5:3ae504b88679 42 }
kishibekairohan 5:3ae504b88679 43
kishibekairohan 5:3ae504b88679 44 double PID::SetPV(double sensorData, double targetData)
kishibekairohan 5:3ae504b88679 45 {
kishibekairohan 5:3ae504b88679 46 double p, i, d;
kishibekairohan 5:3ae504b88679 47
kishibekairohan 5:3ae504b88679 48 diff[0] = diff[1];
kishibekairohan 5:3ae504b88679 49 diff[1] = targetData - sensorData;
kishibekairohan 5:3ae504b88679 50 integral += ((diff[1] + diff[0]) / 2.0) * deltaTime;
kishibekairohan 5:3ae504b88679 51
kishibekairohan 5:3ae504b88679 52 p = kp * diff[1];
kishibekairohan 5:3ae504b88679 53 i = ki * integral;
kishibekairohan 5:3ae504b88679 54 d = kd * ((diff[1] - diff[0]) / deltaTime);
kishibekairohan 5:3ae504b88679 55 mv = PID::limit(p + i + d, dataRangeLower, dataRangeUpper);
kishibekairohan 5:3ae504b88679 56 return mv;
kishibekairohan 5:3ae504b88679 57 }
kishibekairohan 5:3ae504b88679 58
kishibekairohan 5:3ae504b88679 59 double PID::GetMV()
kishibekairohan 5:3ae504b88679 60 {
kishibekairohan 5:3ae504b88679 61 return mv;
kishibekairohan 5:3ae504b88679 62 }
kishibekairohan 5:3ae504b88679 63
kishibekairohan 5:3ae504b88679 64 double PID::limit(double data,double lower,double upper)
kishibekairohan 5:3ae504b88679 65 {
kishibekairohan 5:3ae504b88679 66 if(data < lower) return lower;
kishibekairohan 5:3ae504b88679 67 else if(data > upper) return upper;
kishibekairohan 5:3ae504b88679 68 else return data;
kishibekairohan 5:3ae504b88679 69 }
kishibekairohan 5:3ae504b88679 70
kishibekairohan 5:3ae504b88679 71 }
kishibekairohan 5:3ae504b88679 72