toaster-oven-bluepill/Core/Src/app_pid.c

#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;
}