56 lines
856 B
C++
56 lines
856 B
C++
|
/*
|
||
|
|
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;
|
||
|
|
}
|