car/cpp/pid.cpp at master · berickson/car · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
#include "pid.h"

// note: error is sp - pv
void PID::add_reading(double t, double error) {

  if(!isnan(last_t)) {
    double dt = t - last_t;
    if(!isnan(last_e) && dt > 0)
      slope_e = (error-last_e) / dt;
    sum_e += error * dt;
  }

  last_e = error;
  last_t = t;
}

double PID::output() {
  double rv = 0;

  // P
  if(!isnan(last_e))
    rv += k_p * last_e;

  // I
  rv += k_i * sum_e;

  // D
  if(!isnan(slope_e))
    rv += k_d * slope_e;

  return rv;
}

#include <iostream>

using namespace std;

void test_pid() {
  PID pid;
  pid.k_d = 1;
  cout << pid.output() << endl;
  pid.add_reading(1,1);
  cout << pid.output() << endl;
  pid.add_reading(2,2);
  cout << pid.output() << endl;
  pid.add_reading(3,0);
  cout << pid.output() << endl;
  pid.add_reading(4,0);
  cout << pid.output() << endl;
}