ケンタ ミヤザキ / PID

　

Dependents:   nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more

PID.cpp

Committer:
kenken0721
Date:
2018-03-14
Revision:
1:90e1008a1bf9
Parent:
0:d8b5fa590ca2
Child:
2:09a00a9407f8

File content as of revision 1:90e1008a1bf9:

#include "PID.h"
#include "mbed.h"

PID::PID(double kp, double ki, double kd, double setpoint){
    KP = kp;
    KI = ki;
    KD = kd;
    Setpoint = setpoint;
    init();
}

void PID::init(){
  timer.reset();
  timer.start();
  preTime = 0;
  P = 0;
  I = 0;
  D = 0;
}

double PID::compute(double input){
    dt = (timer.read_us() - preTime) / 10000;
    preTime = timer.read_us();
    P  = Setpoint - input;
    I += P * dt;
    D  = (P - preP) / dt;
    preP = P;
    return (KP * P + KI * I + KD * D);
}