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.
123 lines
2.5 KiB
123 lines
2.5 KiB
9 years ago
|
/* 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);
|
||
|
}
|
||
|
|