parent
ef7866a29f
commit
d0d82ee8fb
@ -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