/
PidController.cs
132 lines (110 loc) · 4.41 KB
/
PidController.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace PIDController
{
public sealed class PidController
{
private double processVariable = 0;
public PidController(double GainProportional, double GainIntegral, double GainDerivative, double OutputMax, double OutputMin)
{
this.GainDerivative = GainDerivative;
this.GainIntegral = GainIntegral;
this.GainProportional = GainProportional;
this.OutputMax = OutputMax;
this.OutputMin = OutputMin;
}
/// <summary>
/// The controller output
/// </summary>
/// <param name="timeSinceLastUpdate">timespan of the elapsed time
/// since the previous time that ControlVariable was called</param>
/// <returns>Value of the variable that needs to be controlled</returns>
public double ControlVariable(TimeSpan timeSinceLastUpdate)
{
double error = SetPoint - ProcessVariable;
// integral term calculation
IntegralTerm += (GainIntegral * error * timeSinceLastUpdate.TotalSeconds);
IntegralTerm = Clamp(IntegralTerm);
// derivative term calculation
double dInput = processVariable - ProcessVariableLast;
double derivativeTerm = GainDerivative * (dInput / timeSinceLastUpdate.TotalSeconds);
// proportional term calcullation
double proportionalTerm = GainProportional * error;
double output = proportionalTerm + IntegralTerm - derivativeTerm;
output = Clamp(output);
return output;
}
/// <summary>
/// The derivative term is proportional to the rate of
/// change of the error
/// </summary>
public double GainDerivative { get; set; } = 0;
/// <summary>
/// The integral term is proportional to both the magnitude
/// of the error and the duration of the error
/// </summary>
public double GainIntegral { get; set; } = 0;
/// <summary>
/// The proportional term produces an output value that
/// is proportional to the current error value
/// </summary>
/// <remarks>
/// Tuning theory and industrial practice indicate that the
/// proportional term should contribute the bulk of the output change.
/// </remarks>
public double GainProportional { get; set; } = 0;
/// <summary>
/// The max output value the control device can accept.
/// </summary>
public double OutputMax { get; set; } = 0;
/// <summary>
/// The minimum ouput value the control device can accept.
/// </summary>
public double OutputMin { get; set; } = 0;
/// <summary>
/// Adjustment made by considering the accumulated error over time
/// </summary>
/// <remarks>
/// An alternative formulation of the integral action, is the
/// proportional-summation-difference used in discrete-time systems
/// </remarks>
public double IntegralTerm { get; private set; } = 0;
/// <summary>
/// The current value
/// </summary>
public double ProcessVariable
{
get { return processVariable; }
set
{
ProcessVariableLast = processVariable;
processVariable = value;
}
}
/// <summary>
/// The last reported value (used to calculate the rate of change)
/// </summary>
public double ProcessVariableLast { get; private set; } = 0;
/// <summary>
/// The desired value
/// </summary>
public double SetPoint { get; set; } = 0;
/// <summary>
/// Limit a variable to the set OutputMax and OutputMin properties
/// </summary>
/// <returns>
/// A value that is between the OutputMax and OutputMin properties
/// </returns>
/// <remarks>
/// Inspiration from http://stackoverflow.com/questions/3176602/how-to-force-a-number-to-be-in-a-range-in-c
/// </remarks>
private double Clamp(double variableToClamp)
{
if (variableToClamp <= OutputMin) { return OutputMin; }
if (variableToClamp >= OutputMax) { return OutputMax; }
return variableToClamp;
}
}
}