collab/pb-hal

git clone https://git.julia.blue/collab/pb-hal

Joseph RyanMerge pull request #2 from ut-ras/control9cbe263

master
1.1 KiB44 linesraw
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),
9    kD(d) {
10    set(0);
11    logFile.open(logName.c_str(),
12                 std::fstream::app); // change to "out" to overwrite
13}
14
15void PID::set(double set) {
16    goal = set;
17    sum = 0;
18    last = 0;
19    if(logFile.is_open()) {
20        logFile << std::endl << "Set: " << set << " P: " << kP << " I: "
21                << kI << " D: " << kD << std::endl;
22    }
23}
24
25double PID::update(double current, double dt) {
26    double err = goal - current;
27    double d = (err - last) / dt;
28    sum += err * dt;
29    // sum = std::min(std::max(controller->sum, -MAX_SUM), MAX_SUM);
30    last = err;
31    double result = kP * err + kI * sum + kD * d;
32    if(logFile.is_open()) {
33        logFile << dt << " " << kP* err << " " << kI* sum << " " << kD* d
34                << " " << result << std::endl;
35    }
36    return result;
37}
38
39PID::~PID() {
40    if(logFile.is_open()) {
41        logFile.close();
42    }
43}
44