39 |
39 |
40 void UpdatePID(pid_var *pid) |
40 void UpdatePID(pid_var *pid) |
41 { |
41 { |
42 if (pid->Mode == PID_MODE_AUTO) { |
42 if (pid->Mode == PID_MODE_AUTO) { |
43 |
43 |
44 double dTerm; |
44 double dInput; |
45 |
45 |
46 if (pid->Type == PID_TYPE_HEAT) |
46 if (pid->Type == PID_TYPE_HEAT) |
47 pid->Err = pid->SetP - pid->Input; |
47 pid->Err = pid->SetP - pid->Input; |
48 else |
48 else |
49 pid->Err = pid->Input - pid->SetP; |
49 pid->Err = pid->Input - pid->SetP; |
50 |
50 |
|
51 dInput = (pid->Input - pid->InputLast); |
|
52 |
51 /* |
53 /* |
52 * Calculate the integral state with appopriate limiting. |
54 * Calculate the integral state with appopriate limiting. |
53 * Use ErrLastLast as iState |
|
54 */ |
55 */ |
55 pid->iState += (pid->iGain * pid->Err); |
56 pid->iState += (pid->iGain * pid->Err); |
|
57 pid->iState -= (pid->pGain * dInput); // Add Proportional on Measurement |
56 if (pid->iState > pid->iMax) |
58 if (pid->iState > pid->iMax) |
57 pid->iState = pid->iMax; |
59 pid->iState = pid->iMax; |
58 else if (pid->iState < 0) |
60 else if (pid->iState < 0) |
59 pid->iState = 0; |
61 pid->iState = 0; |
60 |
62 |
61 dTerm = (pid->Input - pid->InputLast); |
63 pid->OutP = pid->iState - pid->dGain * dInput; |
62 |
|
63 pid->OutP = (pid->pGain * pid->Err) + pid->iState - (pid->dGain * dTerm); |
|
64 pid->InputLast = pid->Input; |
64 pid->InputLast = pid->Input; |
65 |
65 |
66 } else if (pid->Mode == PID_MODE_BOO) { |
66 } else if (pid->Mode == PID_MODE_BOO) { |
67 /* |
67 /* |
68 * Mode Bang On Off |
68 * Mode Bang On Off |