robinkrens.nl - git repository - xy-pid-controller/blob - src/pid-controller.js

   1 class PIDController {
   2         constructor() {
   3                 this.PIDParams = {
   4                         proportionalGain: 2,
   5                         integralGain: 0.0,
   6                         derivativeGain: 2,
   7                         iMin: -1,
   8                         iMax: 1,
   9                         integralSaturation: false,
  10                         derivativeInitialized: false,
  11                         outputMax: 1,
  12                         outputMin: -1,
  13                         magnitude: 10,
  14                         force: {x: 0, y: 0},
  15                         forceMagnitude: 0,
  16                         derivativeMeasurement: "Velocity"
  17                 };
  18                 this.valueLast = {x: 0, y: 0};
  19                 this.errorLast = {x: 0, y: 0};
  20                 this.integrationStored = {x: 0, y: 0};
  21                 this.velocity = {x: 0, y: 0};
  22         }
  23
  24         Reset() {
  25                 this.derivativeInitialized = false;
  26         }
  27
  28         Update(dt, currentValue, targetValue) {
  29                 if (dt <= 0) {
  30                         return {x: 0, y: 0};
  31                 }
  32
  33                 const error = {
  34                         x: targetValue.x - currentValue.x,
  35                         y: targetValue.y - currentValue.y
  36                 };
  37
  38                 const P = {
  39                         x: this.PIDParams.proportionalGain * error.x,
  40                         y: this.PIDParams.proportionalGain * error.y
  41                 };
  42
  43                 this.integrationStored = {
  44                         x: Math.min(
  45                                 Math.max(this.integrationStored.x + (error.x * dt), this.PIDParams.iMin),
  46                                 this.PIDParams.iMax
  47                         ),
  48                         y: Math.min(
  49                                 Math.max(this.integrationStored.y + (error.y * dt), this.PIDParams.iMin),
  50                                 this.PIDParams.iMax
  51                         )
  52                 };
  53
  54                 const I = {
  55                         x: this.PIDParams.integralGain * this.integrationStored.x,
  56                         y: this.PIDParams.integralGain * this.integrationStored.y
  57                 };
  58
  59                 const errorRateOfChange = {
  60                         x: (error.x - this.errorLast.x) / dt,
  61                         y: (error.y - this.errorLast.y) / dt
  62                 };
  63
  64                 this.errorLast = error;
  65
  66                 const valueRateOfChange = {
  67                         x: (currentValue.x - this.valueLast.x) / dt,
  68                         y: (currentValue.y - this.valueLast.y) / dt
  69                 };
  70
  71                 this.valueLast = currentValue;
  72                 this.velocity = valueRateOfChange;
  73
  74                 let deriveMeasure = {x: 0, y: 0};
  75
  76                 if (this.derivativeInitialized) {
  77                         if (this.PIDParams.derivativeMeasurement === "Velocity") {
  78                                 deriveMeasure = {
  79                                         x: -valueRateOfChange.x,
  80                                         y: -valueRateOfChange.y
  81                                 };
  82                         } else {
  83                                 deriveMeasure = errorRateOfChange;
  84                         }
  85                 } else {
  86                         this.derivativeInitialized = true;
  87                 }
  88
  89                 const D = {
  90                         x: this.PIDParams.derivativeGain * deriveMeasure.x,
  91                         y: this.PIDParams.derivativeGain * deriveMeasure.y
  92                 };
  93
  94                 const result = {
  95                         x: P.x + I.x + D.x,
  96                         y: P.y + I.y + D.y
  97                 };
  98
  99                 result.x *= this.PIDParams.magnitude;
 100                 result.y *= this.PIDParams.magnitude;
 101
 102                 this.PIDParams.force = {
 103                         x: result.x * 60, // this routine is called ~60 seconds
 104                         y: result.y * 60
 105                 };
 106
 107                 this.PIDParams.forceMagnitude = Math.sqrt(result.x ** 2 + result.y ** 2) * 60;
 108
 109                 return result;
 110         }
 111 }
 112 export default PIDController;