Kazushi Yamanobe / Mbed 2 deprecated R-ciliatable_4

latest version 9/26

Dependencies:   mbed

pid.cpp@0:0d02a451e79d, 2021-10-10 (annotated)

Committer:
Yamanobe
Date:
Sun Oct 10 04:47:14 2021 +0000
Revision:
0:0d02a451e79d
slave program (at 9/26)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Yamanobe 0:0d02a451e79d 1 #include "pid.h"
Yamanobe 0:0d02a451e79d 2
Yamanobe 0:0d02a451e79d 3 void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
Yamanobe 0:0d02a451e79d 4 kp = Kp;
Yamanobe 0:0d02a451e79d 5 ki = Ki;
Yamanobe 0:0d02a451e79d 6 kd = Kd;
Yamanobe 0:0d02a451e79d 7 time = dt;
Yamanobe 0:0d02a451e79d 8 frequency = 1/dt;
Yamanobe 0:0d02a451e79d 9 }
Yamanobe 0:0d02a451e79d 10
Yamanobe 0:0d02a451e79d 11 void PositionPid::calculate(float target, float nowValue){
Yamanobe 0:0d02a451e79d 12 old = now;
Yamanobe 0:0d02a451e79d 13 now = nowValue - target;
Yamanobe 0:0d02a451e79d 14 p = now;
Yamanobe 0:0d02a451e79d 15 i = i + (now + old) * 0.5f * time;
Yamanobe 0:0d02a451e79d 16 d = (now - old) * frequency;
Yamanobe 0:0d02a451e79d 17 result = kp*p + ki*i + kd*d;
Yamanobe 0:0d02a451e79d 18 if (result > 1.0f) {
Yamanobe 0:0d02a451e79d 19 result = 1.0f;
Yamanobe 0:0d02a451e79d 20 } else if (result < -1.0f) {
Yamanobe 0:0d02a451e79d 21 result = -1.0f;
Yamanobe 0:0d02a451e79d 22 }
Yamanobe 0:0d02a451e79d 23 }
Yamanobe 0:0d02a451e79d 24
Yamanobe 0:0d02a451e79d 25 float PositionPid::duty(){
Yamanobe 0:0d02a451e79d 26 return result;
Yamanobe 0:0d02a451e79d 27 }
Yamanobe 0:0d02a451e79d 28
Yamanobe 0:0d02a451e79d 29 void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
Yamanobe 0:0d02a451e79d 30 kp = Kp;
Yamanobe 0:0d02a451e79d 31 ki = Ki;
Yamanobe 0:0d02a451e79d 32 kd = Kd;
Yamanobe 0:0d02a451e79d 33 time = dt;
Yamanobe 0:0d02a451e79d 34 frequency = 1/dt;
Yamanobe 0:0d02a451e79d 35 }
Yamanobe 0:0d02a451e79d 36
Yamanobe 0:0d02a451e79d 37 void SpeedPid::calculate(float target, float nowValue){
Yamanobe 0:0d02a451e79d 38 e2 = e1;
Yamanobe 0:0d02a451e79d 39 e1 = e;
Yamanobe 0:0d02a451e79d 40 e = nowValue - target;
Yamanobe 0:0d02a451e79d 41 p = e - e1;
Yamanobe 0:0d02a451e79d 42 i = e*time;
Yamanobe 0:0d02a451e79d 43 d = (e-2*e1+e2)*frequency;
Yamanobe 0:0d02a451e79d 44 result = result + (kp*p+ki*i+kd*d);
Yamanobe 0:0d02a451e79d 45 if (result > 1.0f) {
Yamanobe 0:0d02a451e79d 46 result = 1.0f;
Yamanobe 0:0d02a451e79d 47 } else if (result < -1.0f) {
Yamanobe 0:0d02a451e79d 48 result = -1.0f;
Yamanobe 0:0d02a451e79d 49 }
Yamanobe 0:0d02a451e79d 50 }
Yamanobe 0:0d02a451e79d 51
Yamanobe 0:0d02a451e79d 52 float SpeedPid::duty(){
Yamanobe 0:0d02a451e79d 53 return result;
Yamanobe 0:0d02a451e79d 54 }