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.
117 lines
3.0 KiB
117 lines
3.0 KiB
#include "app_pid.h"
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <FreeRTOS.h>
|
|
#include <task.h>
|
|
#include <stdio.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 != PID_AUTOMATIC) { return; }
|
|
self->Input = input;
|
|
|
|
uint32_t now = xTaskGetTickCount();
|
|
int32_t timeChange = (now - self->lastTime);
|
|
if (timeChange >= self->SampleTimeTicks) {
|
|
PUTS("compute\r\n");
|
|
/*Compute all the working error variables*/
|
|
float error = self->Setpoint - input;
|
|
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))
|
|
);
|
|
|
|
/*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->SampleTimeTicks) / configTICK_RATE_HZ;
|
|
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)
|
|
{
|
|
if (new_sample_time > 0) {
|
|
float ratio = (float) new_sample_time
|
|
/ (float) self->SampleTimeTicks;
|
|
self->ki *= ratio;
|
|
self->kd /= ratio;
|
|
self->SampleTimeTicks = (uint32_t) new_sample_time;
|
|
}
|
|
}
|
|
|
|
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)
|
|
{
|
|
if (mode == PID_AUTOMATIC && self->ctlMode == PID_MANUAL) {
|
|
/*we just went from manual to auto*/
|
|
PID_Initialize(self);
|
|
}
|
|
self->ctlMode = mode;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|