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.
106 lines
2.7 KiB
106 lines
2.7 KiB
#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;
|
|
}
|
|
|