Kobayashi Akihiro / ActiveCaster

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.

Dependents:   ActiveCaster_ ActiveCaster_2

PIDclass.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 #include "PIDclass.h"
e5119053f6 0:5e4f1e288e2a 2
e5119053f6 0:5e4f1e288e2a 3 // コンストラクタPIDパラメータを引数を用いて初期化する
e5119053f6 0:5e4f1e288e2a 4 PID::PID(float xKp, float xKi, float xKd, float xint_time)
e5119053f6 0:5e4f1e288e2a 5 {
e5119053f6 0:5e4f1e288e2a 6 Kp = xKp;
e5119053f6 0:5e4f1e288e2a 7 Ki = xKi;
e5119053f6 0:5e4f1e288e2a 8 Kd = xKd;
e5119053f6 0:5e4f1e288e2a 9 int_time = xint_time;
e5119053f6 0:5e4f1e288e2a 10
e5119053f6 0:5e4f1e288e2a 11 preError = 0.0; // 1個前のエラーの値
e5119053f6 0:5e4f1e288e2a 12 intError = 0.0; // 積分値の初期化
e5119053f6 0:5e4f1e288e2a 13
e5119053f6 0:5e4f1e288e2a 14 init_done = false;
e5119053f6 0:5e4f1e288e2a 15 }
e5119053f6 0:5e4f1e288e2a 16
e5119053f6 0:5e4f1e288e2a 17 void PID::PIDinit(float ref, float act)
e5119053f6 0:5e4f1e288e2a 18 {
e5119053f6 0:5e4f1e288e2a 19 preError = ref - act;
e5119053f6 0:5e4f1e288e2a 20 intError = 0.0; // 積分値の初期化
e5119053f6 0:5e4f1e288e2a 21
e5119053f6 0:5e4f1e288e2a 22 init_done = true;
e5119053f6 0:5e4f1e288e2a 23 }
e5119053f6 0:5e4f1e288e2a 24
e5119053f6 0:5e4f1e288e2a 25 // PID制御の実体部
e5119053f6 0:5e4f1e288e2a 26 float PID::getCmd(float ref, float act, float maxcmd)
e5119053f6 0:5e4f1e288e2a 27 {
e5119053f6 0:5e4f1e288e2a 28 float cmd, Error, dError;
e5119053f6 0:5e4f1e288e2a 29 cmd = 0.0;
e5119053f6 0:5e4f1e288e2a 30
e5119053f6 0:5e4f1e288e2a 31 if(init_done) {
e5119053f6 0:5e4f1e288e2a 32 Error = ref - act;
e5119053f6 0:5e4f1e288e2a 33 cmd += Error * Kp;
e5119053f6 0:5e4f1e288e2a 34
e5119053f6 0:5e4f1e288e2a 35 dError = (Error - preError);// / int_time; int_timeが0.01のときdErrorの値が大きくなりすぎてしまうのでコメントアウト
e5119053f6 0:5e4f1e288e2a 36 cmd += dError * Kd;
e5119053f6 0:5e4f1e288e2a 37
e5119053f6 0:5e4f1e288e2a 38 intError += (Error + preError) / 2 * int_time;
e5119053f6 0:5e4f1e288e2a 39 cmd += intError * Ki;
e5119053f6 0:5e4f1e288e2a 40
e5119053f6 0:5e4f1e288e2a 41 preError = Error;
e5119053f6 0:5e4f1e288e2a 42
e5119053f6 0:5e4f1e288e2a 43 if(cmd > maxcmd) cmd = maxcmd;
e5119053f6 0:5e4f1e288e2a 44 if(cmd < -maxcmd) cmd = -maxcmd;
e5119053f6 0:5e4f1e288e2a 45 }
e5119053f6 0:5e4f1e288e2a 46 return cmd;
e5119053f6 0:5e4f1e288e2a 47 }
e5119053f6 0:5e4f1e288e2a 48
e5119053f6 0:5e4f1e288e2a 49 void PID::setPara(float xKp, float xKi, float xKd)
e5119053f6 0:5e4f1e288e2a 50 {
e5119053f6 0:5e4f1e288e2a 51 Kp = xKp;
e5119053f6 0:5e4f1e288e2a 52 Ki = xKi;
e5119053f6 0:5e4f1e288e2a 53 Kd = xKd;
e5119053f6 0:5e4f1e288e2a 54 }