Statistics
| Branch: | Revision:

root / control / pid_control.cpp @ c7e517ce

History | View | Annotate | Download (983 Bytes)

1
#include "pid_control.h"
2

    
3
PID_control::PID_control(float p_term, float d_term, float i_term, float goal) 
4
{
5
  k_p = p_term;
6
  k_d = d_term;
7
  k_i = i_term;
8
  set_config = goal;
9
  reset();
10
}
11

    
12
PID_control::PID_control(float p_term, float d_term, float i_term)
13
{
14
  k_p = p_term;
15
  k_d = d_term;
16
  k_i = i_term;
17
  set_config = 0;
18
  reset();
19
}
20

    
21
void PID_control::reset()
22
{
23
  P_err = 0;
24
  I_err = 0;
25
  D_err = 0;
26
  prev_error = 0;
27
}
28

    
29
void PID_control::change_goal(float goal)
30
{
31
  set_config = goal;
32
  reset();
33
}
34

    
35
void PID_control::change_p(float p)
36
{
37
  k_p = p;
38
  reset();
39
}
40

    
41
void PID_control::change_i(float i)
42
{
43
  k_i = i;
44
  reset();
45
}
46

    
47
void PID_control::change_d(float d)
48
{
49
  k_d = d;
50
  reset();
51
}
52

    
53
float PID_control::pid(float input, double time)
54
{
55
  float error = set_config - input;
56
  double dt = time - prev_time;
57

    
58
  P_err = error;
59
  I_err += prev_error*dt;
60
  D_err = (error - prev_error)/dt;
61

    
62
  prev_time = time;
63
  prev_error = error;
64
  return k_p*P_err + k_i*I_err + k_d*D_err;
65
}