parent
ef7866a29f
commit
d0d82ee8fb
GPG Key ID:
2C5FD5035250423D
@ -0,0 +1,106 @@ |
||||
#include "arduinopid.h" |
||||
#include <stdbool.h> |
||||
#include <stdint.h> |
||||
#include <freertos/FreeRTOS.h> |
||||
#include <freertos/task.h> |
||||
|
||||
static void clampOutput(struct PID *self) { |
||||
if (self->Output > self->outMax) self->Output = self->outMax; |
||||
else if (self->Output < self->outMin) self->Output = self->outMin; |
||||
} |
||||
|
||||
static void clampIterm(struct PID *self) { |
||||
if (self->ITerm > self->outMax) self->ITerm = self->outMax; |
||||
else if (self->ITerm < self->outMin) self->ITerm = self->outMin; |
||||
} |
||||
|
||||
void PID_Compute(struct PID *self, float Input) |
||||
{ |
||||
if (!self->ctlMode) return; |
||||
self->Input = Input; |
||||
|
||||
uint32_t now = xTaskGetTickCount(); |
||||
int32_t timeChange = (now - self->lastTime); |
||||
if (timeChange >= self->SampleTime) { |
||||
/*Compute all the working error variables*/ |
||||
float error = self->Setpoint - Input; |
||||
self->ITerm += (self->ki * error); |
||||
|
||||
clampIterm(self); |
||||
|
||||
float dInput = (Input - self->lastInput); |
||||
|
||||
/*Compute PID Output*/ |
||||
self->Output = self->kp * error + self->ITerm - self->kd * dInput; |
||||
|
||||
clampOutput(self); |
||||
|
||||
/*Remember some variables for next time*/ |
||||
self->lastInput = Input; |
||||
self->lastTime = now; |
||||
} |
||||
} |
||||
|
||||
void PID_SetSetpoint(struct PID *self, float Setpoint) |
||||
{ |
||||
self->Setpoint = Setpoint; |
||||
} |
||||
|
||||
void PID_SetTunings(struct PID *self, float Kp, float Ki, float Kd) |
||||
{ |
||||
if (Kp < 0 || Ki < 0 || Kd < 0) return; |
||||
|
||||
float SampleTimeInSec = ((float) self->SampleTime) / 1000; |
||||
self->kp = Kp; |
||||
self->ki = Ki * SampleTimeInSec; |
||||
self->kd = Kd / SampleTimeInSec; |
||||
|
||||
if (self->controllerDirection == PID_REVERSE) { |
||||
self->kp = -self->kp; |
||||
self->ki = -self->ki; |
||||
self->kd = -self->kd; |
||||
} |
||||
} |
||||
|
||||
void PID_SetSampleTime(struct PID *self, uint32_t NewSampleTime) |
||||
{ |
||||
if (NewSampleTime > 0) { |
||||
float ratio = (float) NewSampleTime |
||||
/ (float) self->SampleTime; |
||||
self->ki *= ratio; |
||||
self->kd /= ratio; |
||||
self->SampleTime = (uint32_t) NewSampleTime; |
||||
} |
||||
} |
||||
|
||||
void PID_SetOutputLimits(struct PID *self, float Min, float Max) |
||||
{ |
||||
if (Min > Max) return; |
||||
self->outMin = Min; |
||||
self->outMax = Max; |
||||
|
||||
clampOutput(self); |
||||
clampIterm(self); |
||||
} |
||||
|
||||
void PID_SetCtlMode(struct PID *self, enum PIDCtlMode Mode) |
||||
{ |
||||
bool newAuto = (Mode == PID_AUTOMATIC); |
||||
if (newAuto == !self->ctlMode) { /*we just went from manual to auto*/ |
||||
PID_Initialize(self); |
||||
} |
||||
self->ctlMode = newAuto; |
||||
} |
||||
|
||||
void PID_Initialize(struct PID *self) |
||||
{ |
||||
self->lastInput = self->Input; |
||||
self->ITerm = self->Output; |
||||
|
||||
clampIterm(self); |
||||
} |
||||
|
||||
void PID_SetControllerDirection(struct PID *self, enum PIDDirection Direction) |
||||
{ |
||||
self->controllerDirection = Direction; |
||||
} |
||||
@ -0,0 +1,52 @@ |
||||
/**
|
||||
* adapted from the Arduino PID library |
||||
*
|
||||
* Created on 2020/01/08. |
||||
*/ |
||||
|
||||
#ifndef ARDUINOPID_H |
||||
#define ARDUINOPID_H |
||||
|
||||
#include <stdint.h> |
||||
|
||||
enum PIDCtlMode { |
||||
PID_MANUAL = 0, |
||||
PID_AUTOMATIC = 1, |
||||
}; |
||||
|
||||
enum PIDDirection { |
||||
PID_DIRECT = 0, |
||||
PID_REVERSE = 1, |
||||
}; |
||||
|
||||
struct PID { |
||||
/*working variables*/ |
||||
uint32_t lastTime; |
||||
float Input, Output, Setpoint; |
||||
float ITerm, lastInput; |
||||
float kp, ki, kd; |
||||
uint32_t SampleTime; //1 sec = 1000
|
||||
float outMin, outMax; |
||||
enum PIDCtlMode ctlMode; // false
|
||||
enum PIDDirection controllerDirection; |
||||
}; |
||||
|
||||
#define PID_DEFAULT() { .SampleTime = 1000, .ctlMode=PID_MANUAL, .controllerDirection=PID_DIRECT } |
||||
|
||||
void PID_Compute(struct PID *self, float Input); |
||||
|
||||
void PID_SetTunings(struct PID *self, float Kp, float Ki, float Kd); |
||||
|
||||
void PID_SetSampleTime(struct PID *self, uint32_t NewSampleTime); |
||||
|
||||
void PID_SetOutputLimits(struct PID *self, float Min, float Max); |
||||
|
||||
void PID_SetCtlMode(struct PID *self, enum PIDCtlMode Mode); |
||||
|
||||
void PID_Initialize(struct PID *self); |
||||
|
||||
void PID_SetSetpoint(struct PID *self, float Setpoint); |
||||
|
||||
void PID_SetControllerDirection(struct PID *self, enum PIDDirection Direction); |
||||
|
||||
#endif //ARDUINOPID_H
|
||||
@ -0,0 +1,121 @@ |
||||
#include "firehazard.h" |
||||
#include "arduinopid.h" |
||||
#include <stdio.h> |
||||
#include <stdbool.h> |
||||
|
||||
#include <math.h> |
||||
#include <esp_log.h> |
||||
#include <nvs.h> |
||||
#include "driver/ledc.h" |
||||
#include "esp_err.h" |
||||
|
||||
static const char *TAG = "fire"; |
||||
|
||||
static struct PID pid = PID_DEFAULT(); |
||||
|
||||
static void pwm_init(void); |
||||
static void pwm_set(float duty); |
||||
|
||||
void fire_get_tuning(float *kp, float *ki, float *kd) { |
||||
*kp = pid.kp; |
||||
*ki = pid.ki; |
||||
*kd = pid.kd; |
||||
} |
||||
|
||||
void fire_init() { |
||||
printf("Regulator init"); |
||||
|
||||
PID_Initialize(&pid); |
||||
PID_SetOutputLimits(&pid, 0, 1); |
||||
PID_SetCtlMode(&pid, PID_MANUAL); |
||||
|
||||
PID_SetTunings(&pid, 0.3, 0.01, 0.1); // TODO load from nvs
|
||||
pwm_init(); |
||||
|
||||
fire_setlevel(20); |
||||
// fire_enable(true);
|
||||
} |
||||
|
||||
void fire_set_tuning(float kp, float ki, float kd) { |
||||
ESP_LOGI(TAG, "PID set tuning Kp=%.3f, Ki=%.3f, Kd=%.3f", kp, ki, kd); |
||||
PID_SetTunings(&pid, kp, ki, kd); |
||||
} |
||||
|
||||
float fire_get_setpoint(bool off_is_zero) { |
||||
if (off_is_zero) { |
||||
return pid.ctlMode == PID_MANUAL ? 0 : pid.Setpoint; |
||||
} else { |
||||
return pid.Setpoint; |
||||
} |
||||
} |
||||
|
||||
void fire_setlevel(float cels) { |
||||
ESP_LOGI(TAG, "PID set target %.3f°C", cels); |
||||
|
||||
if (cels < 0) cels = 0; |
||||
if (cels > MAX_SETPOINT) cels = MAX_SETPOINT; |
||||
PID_SetSetpoint(&pid, cels); |
||||
} |
||||
|
||||
void fire_enable(bool enable) { |
||||
ESP_LOGI(TAG, "Heater %s", enable ? "enable" : "disable"); |
||||
|
||||
PID_SetCtlMode(&pid, enable ? PID_AUTOMATIC : PID_MANUAL); |
||||
} |
||||
|
||||
bool fire_enabled() { |
||||
return pid.ctlMode == PID_AUTOMATIC; |
||||
} |
||||
|
||||
void fire_regulate(float cels) { |
||||
PID_Compute(&pid, cels); |
||||
|
||||
if (cels > MAX_TSENSE || cels < MIN_TSENSE) { |
||||
ESP_LOGE(TAG, "Tsense out of bounds! Stopping."); |
||||
fire_enable(false); |
||||
} |
||||
|
||||
if (pid.ctlMode == PID_MANUAL) { |
||||
pwm_set(0); |
||||
} else { |
||||
printf("PID in %.2f°C, out %.3f, I %.3f\n", cels, pid.Output, pid.ITerm); |
||||
pwm_set(pid.Output); |
||||
} |
||||
} |
||||
|
||||
|
||||
#define PWM_CHANNEL LEDC_CHANNEL_1 |
||||
#define PWM_TIMER LEDC_TIMER_1 |
||||
#define PWM_BIT_NUM LEDC_TIMER_12_BIT |
||||
|
||||
#define PWM_PIN GPIO_NUM_14 |
||||
|
||||
static void pwm_init(void) |
||||
{ |
||||
ledc_channel_config_t ledc_channel_left = {0}; |
||||
ledc_channel_left.gpio_num = PWM_PIN; |
||||
ledc_channel_left.speed_mode = LEDC_HIGH_SPEED_MODE; |
||||
ledc_channel_left.channel = PWM_CHANNEL; |
||||
ledc_channel_left.intr_type = LEDC_INTR_DISABLE; |
||||
ledc_channel_left.timer_sel = PWM_TIMER; |
||||
ledc_channel_left.duty = 0; |
||||
|
||||
ledc_timer_config_t ledc_timer = {0}; |
||||
ledc_timer.speed_mode = LEDC_HIGH_SPEED_MODE; |
||||
ledc_timer.duty_resolution = PWM_BIT_NUM; |
||||
ledc_timer.timer_num = PWM_TIMER; |
||||
ledc_timer.freq_hz = 1; // TODO ??
|
||||
|
||||
ESP_ERROR_CHECK( ledc_channel_config(&ledc_channel_left) ); |
||||
ESP_ERROR_CHECK( ledc_timer_config(&ledc_timer) ); |
||||
} |
||||
|
||||
static void pwm_set(float duty) |
||||
{ |
||||
uint32_t max_duty = (1 << PWM_BIT_NUM);// - 1
|
||||
uint32_t dutycycle = lroundf((duty) * (float)max_duty); |
||||
printf("Dutycycle %d\n", dutycycle); |
||||
|
||||
ESP_ERROR_CHECK( ledc_set_duty(LEDC_HIGH_SPEED_MODE, PWM_CHANNEL, dutycycle) ); |
||||
ESP_ERROR_CHECK( ledc_update_duty(LEDC_HIGH_SPEED_MODE, PWM_CHANNEL) ); |
||||
} |
||||
@ -0,0 +1,25 @@ |
||||
/**
|
||||
* TODO file description |
||||
*
|
||||
* Created on 2020/01/08. |
||||
*/ |
||||
|
||||
#ifndef REFLOWER_FIREHAZARD_H |
||||
#define REFLOWER_FIREHAZARD_H |
||||
#include <stdbool.h> |
||||
|
||||
#define MAX_SETPOINT 350 |
||||
#define MAX_TSENSE 400 |
||||
#define MIN_TSENSE 0 |
||||
|
||||
void fire_regulate(float cels); |
||||
void fire_init(); |
||||
void fire_setlevel(float cels); |
||||
void fire_enable(bool enable); |
||||
|
||||
float fire_get_setpoint(bool off_is_zero); |
||||
void fire_get_tuning(float *kp, float *ki, float *kd); |
||||
bool fire_enabled(); |
||||
void fire_set_tuning(float kp, float ki, float kd); |
||||
|
||||
#endif //REFLOWER_FIREHAZARD_H
|
||||
Binary file not shown.
Loading…
Reference in new issue