Project

General

Profile

Statistics
| Branch: | Revision:

root / control / pid_control.cpp @ a6ca8c2b

History | View | Annotate | Download (683 Bytes)

1 a6ca8c2b Priya
#include "pid_control.h"
2
#include "assert.h"
3
4
PID_control::PID_control(int p_term, int d_term, int i_term, float goal); 
5
{
6
  P_err = 0;
7
  I_err = 0;
8
  D_err = 0;
9
10
  k_p = p_term;
11
  k_d = d_term;
12
  k_i = i_term;
13
14
  set_config = goal;
15
}
16
17
PID_control::PID_control(int p_term, int d_term, int i_term)
18
{
19
  P_err = 0;
20
  I_err = 0;
21
  D_err = 0;
22
23
  k_p = p_term;
24
  k_d = d_term;
25
  k_i = i_term;
26
}
27
28
void PID_control::change_goal(float goal)
29
{
30
  set_config == goal;
31
  I_err = 0;
32
}
33
34
float PID_control::pid(float input)
35
{
36
  prev_error = error;
37
  error = set_config - input;
38
39
  P_err = error;
40
  I_err += prev_error;
41
  D_err = error - prev_error;
42
43
  return k_p*P_err + k_i*I_err + k_d * D_err;
44
}