Sat, 05 Dec 2015 21:46:22 +0100
Switched to PID code from Arduino
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
1 | /***************************************************************************** |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
2 | * Copyright (C) 2015 |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
3 | * |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
4 | * Michiel Broek <mbroek at mbse dot eu> |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
5 | * |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
6 | * This file is part of the mbsePi-apps |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
7 | * |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
8 | * This is free software; you can redistribute it and/or modify it |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
9 | * under the terms of the GNU General Public License as published by the |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
10 | * Free Software Foundation; either version 2, or (at your option) any |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
11 | * later version. |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
12 | * |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
13 | * mbsePi-apps is distributed in the hope that it will be useful, but |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
14 | * WITHOUT ANY WARRANTY; without even the implied warranty of |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
16 | * General Public License for more details. |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
17 | * |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
18 | * You should have received a copy of the GNU General Public License |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
19 | * along with ThermFerm; see the file COPYING. If not, write to the Free |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
20 | * Software Foundation, 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
21 | * |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
22 | * Based on the Arduino PID Library 1.1.1 by Brett Beauregard <br3ttb@gmail.com> |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
23 | * This Library is licensed under a GPLv3 License |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
24 | *****************************************************************************/ |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
25 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
26 | #include "brewco.h" |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
27 | #include "pid.h" |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
28 | #include "util.h" |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
29 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
30 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
31 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
32 | * The parameters specified here are those for for which we can't set up |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
33 | * reliable defaults, so we need to have the user set them. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
34 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
35 | void PID_init(pid_var *pid, double *Input, double *Output, double *Setpoint, double Kp, double Ki, double Kd, int ControllerDirection) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
36 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
37 | pid->myOutput = Output; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
38 | pid->myInput = Input; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
39 | pid->mySetpoint = Setpoint; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
40 | pid->inAuto = FALSE; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
41 | PID_setOutputLimits(pid, 0, 255); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
42 | pid->SampleTime = 100; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
43 | PID_setControllerDirection(pid, ControllerDirection); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
44 | PID_setTunings(pid, Kp, Ki, Kd); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
45 | pid->lastTime = millis() - pid->SampleTime; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
46 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
47 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
48 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
49 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
50 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
51 | * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
52 | * when the transition from manual to auto occurs, the controller is |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
53 | * automatically initialized |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
54 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
55 | void PID_setMode(pid_var *pid, int Mode) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
56 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
57 | int newAuto = (Mode == P_AUTOMATIC); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
58 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
59 | if (newAuto != pid->inAuto) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
60 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
61 | * we just went from manual to auto |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
62 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
63 | pid->ITerm = *pid->myOutput; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
64 | pid->lastInput = *pid->myInput; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
65 | if (pid->ITerm > pid->outMax) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
66 | pid->ITerm = pid->outMax; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
67 | else if (pid->ITerm < pid->outMin) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
68 | pid->ITerm = pid->outMin; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
69 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
70 | pid->inAuto = newAuto; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
71 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
72 | |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
73 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
74 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
75 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
76 | * This function will be used far more often than SetInputLimits. while |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
77 | * the input to the controller will generally be in the 0-1023 range (which is |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
78 | * the default already,) the output will be a little different. maybe they'll |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
79 | * be doing a time window and will need 0-8000 or something. or maybe they'll |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
80 | * want to clamp it from 0-125. who knows. at any rate, that can all be done |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
81 | * here. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
82 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
83 | void PID_setOutputLimits(pid_var *pid, double Min, double Max) |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
84 | { |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
85 | if (Min >= Max) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
86 | return; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
87 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
88 | pid->outMin = Min; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
89 | pid->outMax = Max; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
90 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
91 | if (pid->inAuto == P_AUTOMATIC) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
92 | if (*pid->myOutput > pid->outMax) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
93 | *pid->myOutput = pid->outMax; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
94 | else if (*pid->myOutput < pid->outMin) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
95 | *pid->myOutput = pid->outMin; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
96 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
97 | if (pid->ITerm > pid->outMax) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
98 | pid->ITerm = pid->outMax; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
99 | else if (pid->ITerm < pid->outMin) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
100 | pid->ITerm = pid->outMin; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
101 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
102 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
103 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
104 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
105 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
106 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
107 | * This function allows the controller's dynamic performance to be adjusted. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
108 | * it's called automatically from the constructor, but tunings can also |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
109 | * be adjusted on the fly during normal operation. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
110 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
111 | void PID_setTunings(pid_var *pid, double Kp, double Ki, double Kd) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
112 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
113 | if (Kp < 0 || Ki < 0 || Kd < 0) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
114 | return; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
115 | pid->dispKp = Kp; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
116 | pid->dispKi = Ki; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
117 | pid->dispKd = Kd; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
118 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
119 | double SampleTimeInSec = ((double)pid->SampleTime) / 1000; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
120 | pid->Kp = Kp; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
121 | pid->Ki = Ki * SampleTimeInSec; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
122 | pid->Kd = Kd / SampleTimeInSec; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
123 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
124 | if (pid->Direction == P_REVERSE) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
125 | pid->Kp = (0 - pid->Kp); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
126 | pid->Ki = (0 - pid->Ki); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
127 | pid->Kd = (0 - pid->Kd); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
128 | } |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
129 | } |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
130 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
131 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
132 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
133 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
134 | * The PID will either be connected to a DIRECT acting process (+Output leads |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
135 | * to +Input) or a REVERSE acting process(+Output leads to -Input.) we need to |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
136 | * know which one, because otherwise we may increase the output when we should |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
137 | * be decreasing. This is called from PID_init(). |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
138 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
139 | void PID_setControllerDirection(pid_var *pid, int Direction) |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
140 | { |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
141 | if (pid->inAuto && Direction != pid->Direction) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
142 | pid->Kp = (0 - pid->Kp); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
143 | pid->Ki = (0 - pid->Ki); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
144 | pid->Kd = (0 - pid->Kd); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
145 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
146 | pid->Direction = Direction; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
147 | } |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
148 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
149 | |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
150 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
151 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
152 | * sets the period, in Milliseconds, at which the calculation is performed. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
153 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
154 | void PID_setSampleTime(pid_var *pid, int NewSampleTime) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
155 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
156 | if (NewSampleTime > 0) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
157 | double ratio = (double)NewSampleTime / (double)pid->SampleTime; |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
158 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
159 | pid->Ki *= ratio; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
160 | pid->Kd /= ratio; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
161 | pid->SampleTime = NewSampleTime; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
162 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
163 | } |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
164 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
165 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
166 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
167 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
168 | * Just because you set the Kp=-1 doesn't mean it actually happened. these |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
169 | * functions query the internal state of the PID. they're here for display |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
170 | * purposes. this are the functions the PID Front-end uses for example |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
171 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
172 | double PID_getKp(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
173 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
174 | return pid->dispKp; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
175 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
176 | |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
177 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
178 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
179 | double PID_getKi(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
180 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
181 | return pid->dispKi; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
182 | } |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
183 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
184 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
185 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
186 | double PID_getKd(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
187 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
188 | return pid->dispKd; |
434
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
189 | } |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
190 | |
eb724767860d
Brewco first phase development configuration structure.
Michiel Broek <mbroek@mbse.eu>
parents:
diff
changeset
|
191 | |
446
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
192 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
193 | int PID_getMode(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
194 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
195 | return pid->inAuto ? P_AUTOMATIC : P_MANUAL; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
196 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
197 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
198 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
199 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
200 | int PID_getDirection(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
201 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
202 | return pid->Direction; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
203 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
204 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
205 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
206 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
207 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
208 | * This, as they say, is where the magic happens. this function should be called |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
209 | * every time "void loop()" executes. the function will decide for itself whether a new |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
210 | * pid Output needs to be computed. returns true when the output is computed, |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
211 | * false when nothing has been done. |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
212 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
213 | int PID_compute(pid_var *pid) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
214 | { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
215 | if (! pid->inAuto) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
216 | return FALSE; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
217 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
218 | long now = millis(); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
219 | long timeChange = (now - pid->lastTime); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
220 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
221 | if (timeChange >= pid->SampleTime) { |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
222 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
223 | * Compute all the working error variables |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
224 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
225 | double input = *pid->myInput; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
226 | double error = *pid->mySetpoint - input; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
227 | pid->ITerm += (pid->Ki * error); |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
228 | if (pid->ITerm > pid->outMax) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
229 | pid->ITerm = pid->outMax; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
230 | else if (pid->ITerm < pid->outMin) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
231 | pid->ITerm = pid->outMin; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
232 | double dInput = input - pid->lastInput; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
233 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
234 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
235 | * Compute PID output |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
236 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
237 | double output = pid->Kp * error + pid->ITerm - pid->Kd * dInput; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
238 | if (output > pid->outMax) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
239 | output = pid->outMax; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
240 | else if (output < pid->outMin) |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
241 | output = pid->outMin; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
242 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
243 | /* |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
244 | * Remember some variables for the next time |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
245 | */ |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
246 | pid->lastInput = input; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
247 | pid->lastTime = now; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
248 | return TRUE; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
249 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
250 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
251 | return FALSE; |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
252 | } |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
253 | |
78e9d5234d15
Switched to PID code from Arduino
Michiel Broek <mbroek@mbse.eu>
parents:
435
diff
changeset
|
254 |