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.
toaster-oven-bluepill/Core/Src/app_pid.c

118 lines
3.0 KiB

2 years ago
#include "app_pid.h"
#include <stdbool.h>
#include <stdint.h>
#include <FreeRTOS.h>
#include <task.h>
#include <stdio.h>
2 years ago
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)
2 years ago
{
if (self->ctlMode != PID_AUTOMATIC) { return; }
self->Input = input;
2 years ago
uint32_t now = xTaskGetTickCount();
int32_t timeChange = (now - self->lastTime);
if (timeChange >= self->SampleTimeTicks) {
PUTS("compute\r\n");
2 years ago
/*Compute all the working error variables*/
float error = self->Setpoint - input;
2 years ago
self->ITerm += (self->ki * error);
clampIterm(self);
float dInput = (input - self->lastInput);
PRINTF("calc x100 %d + %d - %d\r\n",
(int) (100 * (self->kp * error)),
(int) (100 * (self->ITerm)),
(int) (100 * (self->kd * dInput))
);
2 years ago
/*Compute PID Output*/
self->Output = self->kp * error + self->ITerm - self->kd * dInput;
clampOutput(self);
/*Remember some variables for next time*/
self->lastInput = input;
2 years ago
self->lastTime = now;
}
}
void PID_SetSetpoint(struct PID *self, float setpoint)
2 years ago
{
self->Setpoint = setpoint;
2 years ago
}
void PID_SetTunings(struct PID *self, float Kp, float Ki, float Kd)
{
if (Kp < 0 || Ki < 0 || Kd < 0) { return; }
float SampleTimeInSec = ((float) self->SampleTimeTicks) / configTICK_RATE_HZ;
2 years ago
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 new_sample_time)
2 years ago
{
if (new_sample_time > 0) {
float ratio = (float) new_sample_time
/ (float) self->SampleTimeTicks;
2 years ago
self->ki *= ratio;
self->kd /= ratio;
self->SampleTimeTicks = (uint32_t) new_sample_time;
2 years ago
}
}
void PID_SetOutputLimits(struct PID *self, float min, float max)
2 years ago
{
if (min > max) { return; }
self->outMin = min;
self->outMax = max;
2 years ago
clampOutput(self);
clampIterm(self);
}
void PID_SetCtlMode(struct PID *self, enum PIDCtlMode mode)
2 years ago
{
if (mode == PID_AUTOMATIC && self->ctlMode == PID_MANUAL) {
/*we just went from manual to auto*/
2 years ago
PID_Initialize(self);
}
self->ctlMode = mode;
2 years ago
}
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)
2 years ago
{
self->controllerDirection = direction;
2 years ago
}