ケンタ ミヤザキ / PID

　

Dependents:   nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more

PID.cpp@0:d8b5fa590ca2, 2018-01-16 (annotated)

Committer:
kenken0721
Date:
Tue Jan 16 14:27:31 2018 +0000
Revision:
0:d8b5fa590ca2
Child:
1:90e1008a1bf9
pid??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenken0721 0:d8b5fa590ca2 1 #include "PID.h"
kenken0721 0:d8b5fa590ca2 2 #include "mbed.h"
kenken0721 0:d8b5fa590ca2 3
kenken0721 0:d8b5fa590ca2 4 PID::PID(double kp, double ki, double kd, double setpoint){
kenken0721 0:d8b5fa590ca2 5 KP = kp;
kenken0721 0:d8b5fa590ca2 6 KI = ki;
kenken0721 0:d8b5fa590ca2 7 KD = kd;
kenken0721 0:d8b5fa590ca2 8 Setpoint = setpoint;
kenken0721 0:d8b5fa590ca2 9 init();
kenken0721 0:d8b5fa590ca2 10 }
kenken0721 0:d8b5fa590ca2 11
kenken0721 0:d8b5fa590ca2 12 void PID::init(){
kenken0721 0:d8b5fa590ca2 13 timer.reset();
kenken0721 0:d8b5fa590ca2 14 timer.start();
kenken0721 0:d8b5fa590ca2 15 preTime = 0;
kenken0721 0:d8b5fa590ca2 16 P = 0;
kenken0721 0:d8b5fa590ca2 17 I = 0;
kenken0721 0:d8b5fa590ca2 18 D = 0;
kenken0721 0:d8b5fa590ca2 19 }
kenken0721 0:d8b5fa590ca2 20
kenken0721 0:d8b5fa590ca2 21 double PID::compute(double input){
kenken0721 0:d8b5fa590ca2 22 dt = (timer.read() - preTime) / 10000;
kenken0721 0:d8b5fa590ca2 23 preTime = timer.read();
kenken0721 0:d8b5fa590ca2 24 P = Setpoint - input;
kenken0721 0:d8b5fa590ca2 25 I += P * dt;
kenken0721 0:d8b5fa590ca2 26 D = (P - preP) / dt;
kenken0721 0:d8b5fa590ca2 27 preP = P;
kenken0721 0:d8b5fa590ca2 28 return (KP * P + KI * I + KD * D);
kenken0721 0:d8b5fa590ca2 29 }