diff -r 000000000000 -r b74b0e4902c3 components/PID/PID_v1.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/components/PID/PID_v1.c Sat Oct 20 13:23:15 2018 +0200 @@ -0,0 +1,286 @@ + +#include +#include +#include +#include +#include +#include + +#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; +int pOn; + +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; +double outputSum, lastInput; + +unsigned long SampleTime; +double outMin, outMax; +bool inAuto, pOnE; + + +static const char *TAG = "pid"; + +void PID_Initialize(void); + + + +void PID(double* Input, double* Output, double* Setpoint, double Kp, double Ki, double Kd, PID_PON POn, 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, POn); + + 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); + + /*Add Proportional on Measurement, if P_ON_M is specified*/ + if (!pOnE) + outputSum-= kp * dInput; + + if (outputSum > outMax) + outputSum= outMax; + else if (outputSum < outMin) + outputSum= outMin; + + /*Add Proportional on Error, if P_ON_E is specified*/ + double output; + if (pOnE) + output = kp * error; + else + 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, PID_PON POn) +{ + if (Kp<0 || Ki<0 || Kd<0) { + ESP_LOGE(TAG, "SetTunings negative input"); + return; + } + + pOn = POn; + pOnE = POn == PID_P_ON_E; + + dispKp = Kp; dispKi = Ki; dispKd = Kd; + + ESP_LOGI(TAG, "SetTunings(%.3f, %.3f, %.3f, %s)", Kp, Ki, Kd, (POn) ? "P_ON_E":"P_ON_M"); + 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; +} + +