/*
PDControl.cpp
(C)2013 kou029w - MIT License
*/

#include <Arduino.h>
#include "PDControl.h"

PDControl_c::PDControl_c(int p=1, int d=0, float decR=0.0){
  lastInput = 0;
  diff = 0;
  Kp = p;
  Kd = d;
  diffDecayRate = decR;
}

int PDControl_c::operator()(int input){
  int output;
  
  diff = diffDecayRate*diff;

  diff = (input - lastInput) + diff;
  output = Kp*input + Kd*diff;
  
  lastInput = input;
  
  return output;
}

FastPDControl_c::FastPDControl_c(int p=1, int d=0, unsigned char hl=0){
  lastInput = 0;
  diff = 0;
  Kp = p;
  Kd = d;
  diffHalfLife = hl;
  _count = 0;
}

int FastPDControl_c::operator()(int input){
  int output;
  
  if(_count < diffHalfLife){
    _count++;
  }else{
    _count = 0;
    diff>>1;
  }
  
  diff = (input - lastInput) + diff;
  output = Kp*input + Kd*diff;
  
  lastInput = input;
  
  return output;
}