Last active
July 27, 2023 18:08
-
-
Save quonic/7382d8a9a06cb793a371adcb5ecd8ad6 to your computer and use it in GitHub Desktop.
A PID controller class adapted for PowerShell. Made it because I haven't seen anyone make one for PowerShell.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#Requires -Version 5 | |
# License: MIT | |
class PID { | |
# Adapted to PowerShell from: https://github.com/tommallama/CSharp-PID/blob/master/PID_Controller/PID.cs | |
#region Public Variables | |
# Upper output limit of the controller. | |
# This should obviously be a numerically greater value than the lower output limit. | |
[double] $OutputUpperLimit | |
# Lower output limit of the controller | |
# This should obviously be a numerically lesser value than the upper output limit. | |
[double] $OutputLowerLimit | |
# Derivative filter coefficient. | |
# A smaller N for more filtering. | |
# A larger N for less filtering. | |
# Consider resetting controller if this parameter is drastically changed. | |
[double] $N | |
# Minimum allowed sample period to avoid dividing by zero! | |
# The Ts value can be mistakenly set to too low of a value or zero on the first iteration. | |
# TsMin by default is set to 1 millisecond. | |
[double] $TsMin = 0.001 | |
#endregion | |
#region Private Variables | |
# Sample period in seconds | |
[double] hidden $Ts | |
# Rollup parameter | |
[double] hidden $K | |
# Rollup parameters | |
[double] hidden $b0 | |
[double] hidden $b1 | |
[double] hidden $b2 | |
# Rollup parameters | |
[double] hidden $a0 | |
[double] hidden $a1 | |
[double] hidden $a2 | |
[double] hidden $CurrentOutput = 0 # Current output | |
[double] hidden $OutputOneOld = 0 # Output one iteration old | |
[double] hidden $OutputTwoOld = 0 # Output two iterations old | |
[double] hidden $CurrentError = 0 # Current error | |
[double] hidden $ErrorOneOld = 0 # Error one iteration old | |
[double] hidden $ErrorTwoOld = 0 # Error two iterations old | |
#endregion | |
# Constructor | |
PID([double] $Kp, [double] $Ki, [double] $Kd, [double] $N, [double] $OutputUpperLimit, [double] $OutputLowerLimit) { | |
$this.Kp = $Kp | |
$this.Ki = $Ki | |
$this.Kd = $Kd | |
$this.N = $N | |
$this.OutputUpperLimit = $OutputUpperLimit | |
$this.OutputLowerLimit = $OutputLowerLimit | |
} | |
[double] Iterate([double] $setPoint, [double] $processValue, [TimeSpan] $ts) { | |
# Ensure the timespan is not too small or zero. | |
$this.Ts = if ($ts.TotalSeconds -ge $this.TsMin) { $ts.TotalSeconds } else { $this.TsMin } | |
# Calculate rollup parameters | |
$this.K = 2 / $Ts | |
$this.b0 = [Math]::Pow($this.K, 2) * $this.Kp + | |
$this.K * $this.Ki + | |
$this.Ki * $this.N + | |
$this.K * $this.Kp * $this.N + [Math]::Pow($this.K, 2) * $this.Kd * $this.N | |
$this.b1 = ( | |
2 * $this.Ki * $this.N - | |
2 * [Math]::Pow($this.K, 2) * $this.Kp - | |
2 * [Math]::Pow($this.K, 2) * $this.Kd * $this.N | |
) | |
$this.b2 = ( | |
[Math]::Pow($this.K, 2) * $this.Kp - | |
$this.K * $this.Ki + $this.Ki * $this.N - | |
$this.K * $this.Kp * $this.N + | |
[Math]::Pow($this.K, 2) * $this.Kd * $this.N | |
) | |
$this.a0 = [Math]::Pow($this.K, 2) + $this.N * $this.K | |
$this.a1 = -2 * [Math]::Pow($this.K, 2) | |
$this.a2 = [Math]::Pow($this.K, 2) - $this.K * $this.N | |
# Age errors and output history | |
$this.ErrorTwoOld = $this.ErrorOneOld # Age errors one iteration | |
$this.ErrorOneOld = $this.CurrentError # Age errors one iteration | |
$this.CurrentError = $setPoint - $processValue # Compute new error | |
$this.OutputTwoOld = $this.OutputOneOld # Age outputs one iteration | |
$this.OutputOneOld = $this.CurrentOutput # Age outputs one iteration | |
$this.CurrentOutput = ( | |
- $this.a1 / $this.a0 * $this.OutputOneOld - | |
$this.a2 / $this.a0 * $this.OutputTwoOld + | |
$this.b0 / $this.a0 * $this.CurrentError + | |
$this.b1 / $this.a0 * $this.ErrorOneOld + | |
$this.b2 / $this.a0 * $this.ErrorTwoOld | |
) # Calculate current output | |
# Clamp output if needed | |
if ($this.CurrentOutput -gt $this.OutputUpperLimit) { | |
$this.CurrentOutput = $this.OutputUpperLimit | |
} | |
elseif ($this.CurrentOutput -lt $this.OutputLowerLimit) { | |
$this.CurrentOutput = $this.OutputLowerLimit | |
} | |
return $this.CurrentOutput | |
} | |
[void] Reset() { | |
$this.ErrorTwoOld = 0; | |
$this.ErrorOneOld = 0; | |
$this.CurrentError = 0; | |
$this.OutputTwoOld = 0; | |
$this.OutputOneOld = 0; | |
$this.CurrentOutput = 0; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment