ケンタ ミヤザキ / 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-24
Revision:
3:3bc8034f569e
Parent:
2:09a00a9407f8

File content as of revision 3:3bc8034f569e:

#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();
  preP = 0;
  P = 0;
  I = 0;
  D = 0;
}

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