collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
9cbe263
tof
1#include <stdlib.h> 2#include <algorithm> 3#include "../inc/pid.h" 4 5#define MAX_SPEED (.4) 6#define MAX_SUM (1000) 7 8PID ::PID (double p ,double i ,double d , std::string logName ) :kP (p ),kI (i ), 9kD (d ) { 10set (0 ); 11logFile .open (logName .c_str (), 12 std::fstream::app );// change to "out" to overwrite 13} 14 15void PID ::set (double set ) { 16goal = set ; 17sum = 0 ; 18last = 0 ; 19if (logFile .is_open ()) { 20logFile << std::endl <<"Set: " <<set <<" P: " <<kP <<" I: " 21 <<kI <<" D: " <<kD << std::endl ; 22 } 23} 24 25double PID ::update (double current ,double dt ) { 26double err = goal - current ; 27double d = (err - last ) /dt ; 28sum += err * dt ; 29// sum = std::min(std::max(controller->sum, -MAX_SUM), MAX_SUM); 30last = err ; 31double result = kP * err + kI * sum + kD * d ; 32if (logFile .is_open ()) { 33logFile <<dt <<" " <<kP * err <<" " <<kI * sum <<" " <<kD * d 34 <<" " <<result << std::endl ; 35 } 36return result ; 37} 38 39PID ::~PID () { 40if (logFile .is_open ()) { 41logFile .close (); 42 } 43} 44