esp32 firmware for a toaster reflow oven WIP!!!!!

arduinopid.c 2.7KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107
  1. #include "arduinopid.h"
  2. #include <stdbool.h>
  3. #include <stdint.h>
  4. #include <freertos/FreeRTOS.h>
  5. #include <freertos/task.h>
  6. static void clampOutput(struct PID *self) {
  7. if (self->Output > self->outMax) self->Output = self->outMax;
  8. else if (self->Output < self->outMin) self->Output = self->outMin;
  9. }
  10. static void clampIterm(struct PID *self) {
  11. if (self->ITerm > self->outMax) self->ITerm = self->outMax;
  12. else if (self->ITerm < self->outMin) self->ITerm = self->outMin;
  13. }
  14. void PID_Compute(struct PID *self, float Input)
  15. {
  16. if (!self->ctlMode) return;
  17. self->Input = Input;
  18. uint32_t now = xTaskGetTickCount();
  19. int32_t timeChange = (now - self->lastTime);
  20. if (timeChange >= self->SampleTime) {
  21. /*Compute all the working error variables*/
  22. float error = self->Setpoint - Input;
  23. self->ITerm += (self->ki * error);
  24. clampIterm(self);
  25. float dInput = (Input - self->lastInput);
  26. /*Compute PID Output*/
  27. self->Output = self->kp * error + self->ITerm - self->kd * dInput;
  28. clampOutput(self);
  29. /*Remember some variables for next time*/
  30. self->lastInput = Input;
  31. self->lastTime = now;
  32. }
  33. }
  34. void PID_SetSetpoint(struct PID *self, float Setpoint)
  35. {
  36. self->Setpoint = Setpoint;
  37. }
  38. void PID_SetTunings(struct PID *self, float Kp, float Ki, float Kd)
  39. {
  40. if (Kp < 0 || Ki < 0 || Kd < 0) return;
  41. float SampleTimeInSec = ((float) self->SampleTime) / 1000;
  42. self->kp = Kp;
  43. self->ki = Ki * SampleTimeInSec;
  44. self->kd = Kd / SampleTimeInSec;
  45. if (self->controllerDirection == PID_REVERSE) {
  46. self->kp = -self->kp;
  47. self->ki = -self->ki;
  48. self->kd = -self->kd;
  49. }
  50. }
  51. void PID_SetSampleTime(struct PID *self, uint32_t NewSampleTime)
  52. {
  53. if (NewSampleTime > 0) {
  54. float ratio = (float) NewSampleTime
  55. / (float) self->SampleTime;
  56. self->ki *= ratio;
  57. self->kd /= ratio;
  58. self->SampleTime = (uint32_t) NewSampleTime;
  59. }
  60. }
  61. void PID_SetOutputLimits(struct PID *self, float Min, float Max)
  62. {
  63. if (Min > Max) return;
  64. self->outMin = Min;
  65. self->outMax = Max;
  66. clampOutput(self);
  67. clampIterm(self);
  68. }
  69. void PID_SetCtlMode(struct PID *self, enum PIDCtlMode Mode)
  70. {
  71. bool newAuto = (Mode == PID_AUTOMATIC);
  72. if (newAuto == !self->ctlMode) { /*we just went from manual to auto*/
  73. PID_Initialize(self);
  74. }
  75. self->ctlMode = newAuto;
  76. }
  77. void PID_Initialize(struct PID *self)
  78. {
  79. self->lastInput = self->Input;
  80. self->ITerm = self->Output;
  81. clampIterm(self);
  82. }
  83. void PID_SetControllerDirection(struct PID *self, enum PIDDirection Direction)
  84. {
  85. self->controllerDirection = Direction;
  86. }