You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
122 lines
2.5 KiB
122 lines
2.5 KiB
/* control.c */ |
|
|
|
#include <util/atomic.h> |
|
|
|
#include "common.h" |
|
#include "timer.h" |
|
#include "config.h" |
|
#include "therm.h" |
|
#include "beep.h" |
|
|
|
static temp_t last_temp; |
|
static volatile bool preheat; |
|
static bool control_running; |
|
static uint32_t control_start_time; |
|
static int16_t integral; |
|
static int16_t control_output; |
|
|
|
temp_t temperature_at_time(uint32_t time); |
|
|
|
int16_t __attribute__ ((noinline)) clip16(int16_t val, int16_t min, int16_t max) |
|
{ |
|
if (val < min) |
|
val = min; |
|
if (val > max) |
|
val = max; |
|
return val; |
|
} |
|
|
|
uint32_t control_now(void) |
|
{ |
|
uint32_t this_seconds; |
|
|
|
ATOMIC_BLOCK(ATOMIC_FORCEON) { |
|
this_seconds = seconds; |
|
} |
|
|
|
if (preheat) |
|
control_start_time = this_seconds; |
|
return this_seconds - control_start_time; |
|
} |
|
|
|
void control_start(void) |
|
{ |
|
preheat = TRUE; |
|
last_temp = therm_temp(); |
|
integral = 0; |
|
control_running = TRUE; |
|
control_output = 0; |
|
} |
|
|
|
#define TIME_UNIT 1 |
|
#define INTEGRAL_MAX (CONTROL_MAX * 2) |
|
#define INTEGRAL_MIN (-(INTEGRAL_MAX)) |
|
|
|
#define CONTROL_MAX (10 * 128) |
|
#define CONTROL_MIN (0) |
|
|
|
/* P = 1.00 means 1 degree C leads to full-scale output |
|
* This is represented by config.p == 128 |
|
* The calculations done here are in units of 0.1 degrees |
|
*/ |
|
|
|
#define ABS(x) ((x) < 0 ? -(x) : (x)) |
|
#define INT_MAX 32767 |
|
#define INT_MIN (-32768) |
|
|
|
#if 0 |
|
int16_t multiply_clip(int16_t a, int16_t b) |
|
{ |
|
if ((b == 0) || (ABS(a) > INT_MAX / ABS(b))) |
|
return ((a < 0) ^ (b < 0)) ? INT_MIN : INT_MAX; |
|
return a * b; |
|
} |
|
#endif |
|
|
|
void control_poll(void) |
|
{ |
|
temp_t temp = therm_temp(); |
|
temp_t profile_temp; |
|
int16_t error; |
|
int16_t temp_diff = temp - last_temp; |
|
int16_t maxgain; |
|
int16_t p, i, d; |
|
|
|
last_temp = temp; |
|
|
|
if (!control_running) |
|
return; |
|
|
|
profile_temp = temperature_at_time(control_now()); |
|
if (profile_temp == INVALID_TEMPERATURE) { |
|
control_running = FALSE; |
|
output0 = 0; |
|
beep_off(); |
|
return; |
|
} |
|
|
|
error = (int16_t)(profile_temp - temp); |
|
|
|
if (error <= 0) { |
|
beep_on(); |
|
preheat = FALSE; |
|
} else { |
|
beep_off(); |
|
} |
|
|
|
maxgain = (error == 0) ? INT_MAX : ((2*CONTROL_MAX) / error); |
|
maxgain = ABS(maxgain); |
|
|
|
p = clip16(config.p, -maxgain, maxgain); |
|
i = clip16(config.i, -maxgain, maxgain); |
|
d = clip16(config.d, -maxgain, maxgain); |
|
|
|
integral += i * error * TIME_UNIT; |
|
integral = clip16(integral, INTEGRAL_MIN, INTEGRAL_MAX); |
|
|
|
control_output += p * error + integral + d * temp_diff / TIME_UNIT; |
|
control_output = clip16(control_output, CONTROL_MIN, CONTROL_MAX); |
|
|
|
output0 = (TIMER_MAX/5) * control_output / (CONTROL_MAX/5); |
|
} |
|
|
|
|