Revision c7e517ce

View differences:

control/pid_control.cpp
2 2

  
3 3
PID_control::PID_control(float p_term, float d_term, float i_term, float goal) 
4 4
{
5
  P_err = 0;
6
  I_err = 0;
7
  D_err = 0;
8

  
9 5
  k_p = p_term;
10 6
  k_d = d_term;
11 7
  k_i = i_term;
12

  
13 8
  set_config = goal;
9
  reset();
14 10
}
15 11

  
16 12
PID_control::PID_control(float p_term, float d_term, float i_term)
17 13
{
18
  P_err = 0;
19
  I_err = 0;
20
  D_err = 0;
21

  
22 14
  k_p = p_term;
23 15
  k_d = d_term;
24 16
  k_i = i_term;
17
  set_config = 0;
18
  reset();
25 19
}
26 20

  
27
void PID_control::change_goal(float goal)
21
void PID_control::reset()
28 22
{
29
  set_config == goal;
23
  P_err = 0;
30 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();
31 51
}
32 52

  
33 53
float PID_control::pid(float input, double time)
34 54
{
35
  prev_error = error;
36
  error = set_config - input;
55
  float error = set_config - input;
37 56
  double dt = time - prev_time;
38 57

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

  
43 62
  prev_time = time;
44
  return k_p*P_err + k_i*I_err + k_d * D_err;
63
  prev_error = error;
64
  return k_p*P_err + k_i*I_err + k_d*D_err;
45 65
}
control/pid_control.h
8 8
    PID_control(float p_term, float d_term, float i_term); 
9 9

  
10 10
    void change_goal(float goal);
11
    void change_p(float p);
12
    void change_i(float i);
13
    void change_d(float d);
11 14

  
12 15
    float pid(float input, double time);
13 16

  
14 17
  private:
18
    void reset();
19
    
15 20
    float k_p;
16 21
    float k_i;
17 22
    float k_d;
18 23
    
19 24
    float set_config;
20
    float error;
21 25
    float prev_error;
22 26
    double prev_time;
23 27

  

Also available in: Unified diff