1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
|
/*
* Xytronic LF-1600
* Current controller
*
* Copyright (c) 2015-2016 Michael Buesch <m@bues.ch>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include "controller_current.h"
#include "pid.h"
#include "timer.h"
#include "pwm_current.h"
#include "scale.h"
#include "filter.h"
#include "debug_uart.h"
#include <string.h>
/* Design note:
*
* The current controller is a PID controller in the lower
* setpoint area. However in the upper area the current is
* set without feedback evaluation. This is due to our feedback
* hardware not working correctly for high currents.
* The cut-off-setpoint where the PID is switched off/on
* is defined via CONTRCURR_PID_CUTOFF_HI/LO.
*
* CONTRCURR_PID_CUTOFF_HI is configured via CONF_CURRCUTOFF.
*/
/* Current controller PID parameters */
#define CONTRCURR_PID_KP 2.0
#define CONTRCURR_PID_KI 0.05
#define CONTRCURR_PID_KD 0.05
#define CONTRCURR_PID_D_DECAY 1.2
/* PID cut off current. PID is only active below this setpoint. */
#define CONTRCURR_PID_CUTOFF_HYST ((float)(CONF_CURRCUTOFFHYST))
#define CONTRCURR_PID_CUTOFF_HI ((float)(CONF_CURRCUTOFF))
#define CONTRCURR_PID_CUTOFF_LO (CONTRCURR_PID_CUTOFF_HI - CONTRCURR_PID_CUTOFF_HYST)
struct current_contr_context {
bool enabled;
bool pid_disabled;
bool restricted;
uint8_t emergency_flags;
struct pid pid;
fixpt_t feedback;
fixpt_t prev_y;
struct timer timer;
int24_t old_current_real_feedback;
int24_t old_current_used_feedback;
int24_t old_current_control;
struct lp_filter_fixpt model;
};
static struct current_contr_context contrcurr;
static const struct pid_k_set __flash contrcurr_factors = {
.kp = FLOAT_TO_FIXPT(CONTRCURR_PID_KP),
.ki = FLOAT_TO_FIXPT(CONTRCURR_PID_KI),
.kd = FLOAT_TO_FIXPT(CONTRCURR_PID_KD),
.d_decay_div = FLOAT_TO_FIXPT(CONTRCURR_PID_D_DECAY),
};
static void contrcurr_run(fixpt_t r)
{
fixpt_t dt, y, w;
if (!contrcurr.enabled)
return;
if (contrcurr.emergency_flags)
return;
/* Get delta-t that elapsed since last run, in seconds */
dt = fixpt_div(int_to_fixpt(timer_ms_since(&contrcurr.timer)),
int_to_fixpt(1000));
timer_set_now(&contrcurr.timer);
/* Check whether the PID should be enabled or disabled. */
w = pid_get_setpoint(&contrcurr.pid);
if (contrcurr.pid_disabled) {
if (w <= float_to_fixpt(CONTRCURR_PID_CUTOFF_LO))
contrcurr.pid_disabled = false;
} else {
if (w >= float_to_fixpt(CONTRCURR_PID_CUTOFF_HI)) {
contrcurr.pid_disabled = true;
lp_filter_fixpt_set(&contrcurr.model, r);
}
}
if (contrcurr.pid_disabled) {
/* We are in the upper setpoint area.
* Do not use the real feedback. Use a PT1 model instead. */
r = lp_filter_fixpt_run(&contrcurr.model, contrcurr.prev_y,
int_to_fixpt(16));
}
debug_report_int24(PSTR("cr2"), &contrcurr.old_current_used_feedback,
(int24_t)r);
/* Run the PID controller */
y = pid_run(&contrcurr.pid, dt, r);
if (contrcurr.restricted) {
/* We are in restricted mode.
* Limit current to the restricted lower area. */
if (y > float_to_fixpt(CONTRCURR_RESTRICT_MAXCURR))
y = float_to_fixpt(CONTRCURR_RESTRICT_MAXCURR);
}
debug_report_int24(PSTR("cy"), &contrcurr.old_current_control,
(int24_t)y);
/* Reconfigure the PWM unit to output the
* requested heater current (y). */
contrcurr.prev_y = y;
pwmcurr_set(y);
}
void contrcurr_set_feedback(fixpt_t r)
{
if (r != contrcurr.feedback) {
contrcurr.feedback = r;
debug_report_int24(PSTR("cr1"),
&contrcurr.old_current_real_feedback,
(int24_t)r);
}
/* Run the controller. */
contrcurr_run(r);
}
void contrcurr_set_setpoint(fixpt_t w)
{
pid_set_setpoint(&contrcurr.pid, w);
}
void contrcurr_set_restricted(bool restricted)
{
contrcurr.restricted = restricted;
}
void contrcurr_set_enabled(bool enable,
uint8_t disabled_curr_percent)
{
fixpt_t y;
if (enable != contrcurr.enabled) {
contrcurr.enabled = enable;
/* Reset the controller. */
pwmcurr_set(int_to_fixpt(0));
pid_reset(&contrcurr.pid);
contrcurr.pid_disabled = false;
contrcurr.prev_y = int_to_fixpt(0);
timer_set_now(&contrcurr.timer);
}
if (!enable) {
/* The controller is disabled.
* Set the disabled-current permanently.
*/
y = scale(disabled_curr_percent,
0, 100,
float_to_fixpt(CONTRCURR_NEGLIM),
float_to_fixpt(CONTRCURR_POSLIM));
pwmcurr_set(y);
}
}
void contrcurr_set_emerg(uint8_t emergency_flags)
{
if (emergency_flags != contrcurr.emergency_flags) {
contrcurr.emergency_flags = emergency_flags;
if (emergency_flags) {
/* In an emergency situation, disable the
* heater current to avoid damage.
*/
pwmcurr_set(float_to_fixpt(CONTRCURR_NEGLIM));
}
}
}
uint8_t contrcurr_get_emerg(void)
{
return contrcurr.emergency_flags;
}
void contrcurr_init(void)
{
struct pid_k_set k_set;
k_set = contrcurr_factors;
pid_init(&contrcurr.pid, &k_set,
float_to_fixpt(CONTRCURR_NEGLIM),
float_to_fixpt(CONTRCURR_POSLIM));
/* Enable the controller. */
contrcurr_set_enabled(true, 0);
contrcurr_set_emerg(false);
/* Assume the iron is cold and start
* with restricted current.
*/
contrcurr_set_restricted(true);
}
|
