--- /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 <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;
+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;
+}
+
+