components/PID/PID_v1.c


#include <stddef.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdint.h>
#include <math.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"

#include "PID_v1.h"

double		dispKp;			// we'll hold on to the tuning parameters in user-entered
double		dispKi;			// format for display purposes
double		dispKd;

double		kp;			// (P)roportional Tuning Parameter
double		ki;			// (I)ntegral Tuning Parameter
double		kd;			// (D)erivative Tuning Parameter

int		controllerDirection;

double		*myInput;		// Pointers to the Input, Output, and Setpoint variables
double		*myOutput;		// This creates a hard link between the variables and the 
double		*mySetpoint;		// PID, freeing the user from having to constantly tell us
	        	                // what these values are.  with pointers we'll just know.

unsigned long	lastTime;		///< Last time of the time window.
double		outputSum, lastInput;

unsigned long	SampleTime;
double		outMin, outMax;
bool		inAuto;


static const char *TAG = "pid";

void PID_Initialize(void);


void PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, PID_DIRECTION Direction) {
    myOutput = Output;
    myInput = Input;
    mySetpoint = Setpoint;
    inAuto = false;

    PID_SetOutputLimits(0, 255);

    SampleTime = 100;

    PID_SetControllerDirection(Direction);
    PID_SetTunings(Kp, Ki, Kd);

    lastTime = (xTaskGetTickCount() * portTICK_PERIOD_MS ) - SampleTime;
}


/*
 * This, as they say, is where the magic happens.  this function should be called
 *   every time "void loop()" executes.  the function will decide for itself whether a new
 *   pid Output needs to be computed.  returns true when the output is computed,
 *   false when nothing has been done.
 */
bool PID_Compute(void)
{
    if (!inAuto)
	return false;
	      
    unsigned long now = xTaskGetTickCount() * portTICK_PERIOD_MS;
    unsigned long timeChange = (now - lastTime);
    if (timeChange >= SampleTime) {
	/*Compute all the working error variables*/
	double input = *myInput;
	double error = *mySetpoint - input;
	double dInput = (input - lastInput);
	outputSum+= (ki * error);
	outputSum-= kp * dInput;

	if (outputSum > outMax)
	    outputSum= outMax;
	else if (outputSum < outMin)
	    outputSum= outMin;
	
	double output = 0;

	/*Compute Rest of PID Output*/
	output += outputSum - kd * dInput;

	if (output > outMax)
	    output = outMax;
	else if (output < outMin)
	    output = outMin;
	*myOutput = output;

	/*Remember some variables for next time*/
	lastInput = input;
	lastTime = now;
	return true;
    } else
	return false;
}


/*
 * This function allows the controller's dynamic performance to be adjusted.
 * it's called automatically from the constructor, but tunings can also
 * be adjusted on the fly during normal operation
 */
void PID_SetTunings(double Kp, double Ki, double Kd)
{
    if (Kp<0 || Ki<0 || Kd<0) {
	ESP_LOGE(TAG, "SetTunings negative input");
	return;
    }

    dispKp = Kp; dispKi = Ki; dispKd = Kd;

    ESP_LOGI(TAG, "SetTunings(%.3f, %.3f, %.3f)", Kp, Ki, Kd); 
    double SampleTimeInSec = ((double)SampleTime)/1000;
    kp = Kp;
    ki = Ki * SampleTimeInSec;
    kd = Kd / SampleTimeInSec;

    if (controllerDirection == PID_REVERSE) {
	kp = (0 - kp);
	ki = (0 - ki);
	kd = (0 - kd);
    }
}


/*
 * sets the period, in Milliseconds, at which the calculation is performed
 */
void PID_SetSampleTime(int NewSampleTime)
{
    ESP_LOGI(TAG, "SetSampleTime(%d)", NewSampleTime);

    if (NewSampleTime > 0) {
	double ratio  = (double)NewSampleTime / (double)SampleTime;
	ki *= ratio;
	kd /= ratio;
	SampleTime = (unsigned long)NewSampleTime;
    }
}


/*
 * This function will be used far more often than SetInputLimits.  while
 *  the input to the controller will generally be in the 0-1023 range (which is
 *  the default already,)  the output will be a little different.  maybe they'll
 *  be doing a time window and will need 0-8000 or something.  or maybe they'll
 *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
 *  here.
 */
void PID_SetOutputLimits(double Min, double Max)
{
    if(Min >= Max) {
        ESP_LOGE(TAG, "SetOutputLimits Min >= Max");
	return;
    }

    ESP_LOGI(TAG, "SetOutputLimits(%.0f, %.0f)", Min, Max);
    outMin = Min;
    outMax = Max;

    if(inAuto) {
	if(*myOutput > outMax)
	    *myOutput = outMax;
	else if(*myOutput < outMin)
	    *myOutput = outMin;

    	if(outputSum > outMax)
	    outputSum= outMax;
	else if(outputSum < outMin)
	    outputSum= outMin;
    }
}


/*
 * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
 * when the transition from manual to auto occurs, the controller is
 * automatically initialized
 */
void PID_SetMode(PID_MODE Mode)
{
    bool newAuto = (Mode == PID_AUTOMATIC);

    ESP_LOGI(TAG, "SetMode(%s)", (Mode) ? "AUTOMATIC":"MANUAL");
    if(newAuto && !inAuto) {  /*we just went from manual to auto*/
	PID_Initialize();
    }
    inAuto = newAuto;
}


/*
 * does all the things that need to happen to ensure a bumpless transfer
 *  from manual to automatic mode.
 */
void PID_Initialize()
{
    outputSum = *myOutput;
    lastInput = *myInput;
    if(outputSum > outMax)
	outputSum = outMax;
    else if(outputSum < outMin)
	outputSum = outMin;
}


/*
 * The PID will either be connected to a DIRECT acting process (+Output leads
 * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
 * know which one, because otherwise we may increase the output when we should
 * be decreasing.  This is called from the constructor.
 */
void PID_SetControllerDirection(PID_DIRECTION Direction)
{
    ESP_LOGI(TAG, "SetControllerDirection(%s)", (Direction) ? "REVERSE":"DIRECT");

    if(inAuto && Direction !=controllerDirection) {
	kp = (0 - kp);
	ki = (0 - ki);
	kd = (0 - kd);
    }
    controllerDirection = Direction;
}


/*
 * Just because you set the Kp=-1 doesn't mean it actually happened.  these
 * functions query the internal state of the PID.  they're here for display
 * purposes.  this are the functions the PID Front-end uses for example
 */
double PID_GetKp()
{ 
    return  dispKp;
}

double PID_GetKi()
{
    return  dispKi;
}

double PID_GetKd()
{
    return  dispKd;
}

PID_MODE PID_GetMode()
{
    return  inAuto ? PID_AUTOMATIC : PID_MANUAL;
}

PID_DIRECTION PID_GetDirection()
{
    return controllerDirection;
}