The PID Controller – Part 3 of 4

← Back to Part 1 – Introduction and some math
← Back to Part 2 – Some more math 🙂
→ Part 4 – Application to the Ev3 Medium Motor position regulation

As described in Part 2, we need a new PIDController class that performs the following computation:

$K_1 = K_p + K_i + K_d$ ,
$K_2 = -K_p - 2K_d$ ,
$K_3 = K_d$ ,
$e_k = r_k - y_k$ ,
$\Delta u_k = K_1 e_k + K_2 e_{k-1} + K_3 e_{k-2}$ ,
$u_k = u_{k-1} + \Delta u_k$ .

You can compile the code I propose in this page using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486 to let it work on board the Lego Mindstorms Ev3 with the Monobrick firmware.

Properties

So first we define the main properties: the InputSignal, the OutputsSignal and the SetPoint. As you may notice the OutputSignal property is set as overridable in derived classes, to allow any useful post-computation you may think of.

/// <summary>
/// Get or sets the input used for PID calculation
/// 
/// The input.
public sbyte InputSignal { get; set; }

protected sbyte outputSignal;
/// 
/// Get the the control signal of the PID controller
/// 
/// The output.
public virtual sbyte OutputSignal
{
	get {
	    return outputSignal;
	}
	protected set {
	    if (outputSignal != value) {
	        outputSignal = value;
	    }
        }
}

/// 
/// Get or sets the SetPoint used for regulation
/// 
public sbyte SetPoint { get; set;}

/// <summary>

/// Get or sets the input used for PID calculation

///

/// The input.

public sbyte InputSignal { get; set; }

protected sbyte outputSignal;

///

/// Get the the control signal of the PID controller

///

/// The output.

public virtual sbyte OutputSignal

{

get {

return outputSignal;

}

protected set {

if (outputSignal != value) {

outputSignal = value;

}

///

/// Get or sets the SetPoint used for regulation

///

public sbyte SetPoint { get; set;}

Next, we need to set up the SampleTime, the MinPower, and the MaxPower.
The SampleTime is expressed in milliseconds (one thousandth of second) and represent the time interval between two successive algorithm executions; choosing the correct sampling time for your process goes beyond the scope of this article, but as a general rule of thumb if you think that the system under control is phisically able to perform its average task in a time T ( i.e. if T is the expected or desired settling time) then choosing
$T_s \in [\frac{T}{50}; \frac{T}{40}]$
can do the trick.
MinPower and MaxPower define the range in which the OutputSignal is allowed to vary; if for any reason the OutputSignal calculated by the controller grows out of the allowed range, its value gets trimmed to the violated boundary.

private int sampleTime;
/// 
/// Get or sets the sample time
/// 
/// The sample time.
/// ArgumentException">Thrown if value is non positive
public int SampleTime
{
	get {
		return sampleTime;
	}
	set{ 
		if (value < 0) {
			ArgumentException ex = new ArgumentException ("The sample time must be a positive number");
			throw(ex);
		} else if (sampleTime != value) {
			sampleTime = value;
		}
	}
}

/// 
/// Get or sets the minimum power
/// or in other words the minimum allowable
/// value for the control signal
/// 
/// The minimum power.
public sbyte MinPower { set; get; }

/// 
/// Get or sets the maximum power
/// or in other words the maximum allowable
/// value for the control signal
/// 
/// The max power.
public sbyte MaxPower { get; set;}

private int sampleTime;

///

/// Get or sets the sample time

///

/// The sample time.

/// ArgumentException">Thrown if value is non positive

public int SampleTime

{

get {

return sampleTime;

}

set{

if (value < 0) {

ArgumentException ex = new ArgumentException ("The sample time must be a positive number");

throw(ex);

} else if (sampleTime != value) {

sampleTime = value;

}

///

/// Get or sets the minimum power

/// or in other words the minimum allowable

/// value for the control signal

///

/// The minimum power.

public sbyte MinPower { set; get; }

///

/// Get or sets the maximum power

/// or in other words the maximum allowable

/// value for the control signal

///

/// The max power.

public sbyte MaxPower { get; set;}

The last properties to be defined are the three Ks: the proportional constant $K_p$ , the integrative constant $K_i$ , and the derivative constant $K_d$ . The setter of each of these properties calls UpdateKs() to update the $K_1$ , $K_2$ , and $K_3$ .

private float kp;
/// <summary>
/// Get or sets the proportional constant
/// </summary>
/// <value>The Kp</value>
/// <exception cref="ArgumentException">Thrown if value is non positive
public float Kp {
	get {
		return kp;
	}
	set {
		if (value < 0) {
			ArgumentException ex = new ArgumentException ("The proportional constant must be a positive number");
			throw(ex);
		} else if (kp != value) {
			kp = value;
			UpdateKs();
		}
	}
}

private float ki;
/// 
/// Get or sets the integral constant
/// 
/// The Ki
/// Thrown if value is non positive
public float Ki { 
	get {
		return ki;
	}
	set {
		if (value < 0) {
			ArgumentException ex = new ArgumentException ("The integral constant must be a positive number");
			throw(ex);
		} else if (ki != value) {
			ki = value;
			UpdateKs ();
		}
	}
}

private float kd;
/// 
/// Get or sets the derivative constant
/// 
/// The Kd
/// Thrown if value is non positive
public float Kd {
	get {
		return kd;
	}
	set {
		if (value < 0) {
			ArgumentException ex = new ArgumentException ("The derivative constant must be a positive number");
			throw(ex);
		} else if (kd != value) {
			kd = value;
			UpdateKs ();
		}
	}
}

private float kp;

/// <summary>

/// Get or sets the proportional constant

/// </summary>

/// <value>The Kp</value>

/// <exception cref="ArgumentException">Thrown if value is non positive

public float Kp {

get {

return kp;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The proportional constant must be a positive number");

throw(ex);

} else if (kp != value) {

kp = value;

UpdateKs();

}

private float ki;

///

/// Get or sets the integral constant

///

/// The Ki

/// Thrown if value is non positive

public float Ki {

get {

return ki;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The integral constant must be a positive number");

throw(ex);

} else if (ki != value) {

ki = value;

UpdateKs ();

}

private float kd;

///

/// Get or sets the derivative constant

///

/// The Kd

/// Thrown if value is non positive

public float Kd {

get {

return kd;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The derivative constant must be a positive number");

throw(ex);

} else if (kd != value) {

kd = value;

UpdateKs ();

}

Fields

The fields are just $K_1$ , $K_2$ , and $K_3$ , the main thread and its wait handle as follows:

/// <summary>
/// Constants used int the "speed form" of the discrete PID algorithm
/// </summary>
private float k1;
private float k2;
private float k3;

// PIDThread
private EventWaitHandle stopPIDThread; 
private Thread PIDThread;

/// <summary>

/// Constants used int the "speed form" of the discrete PID algorithm

/// </summary>

private float k1;

private float k2;

private float k3;

// PIDThread

private EventWaitHandle stopPIDThread;

private Thread PIDThread;

Constructors

The PIDController class defines just the default constructor for fields initialization:

/// <summary>
/// Default constructor
/// </summary>
public PIDController ()
{
	// Fields initialization
	init();
}

/// 
/// Fields initialization
/// 
private void init()
{
	Ki = 0;
	Kp = 0;
	Kd = 0;
	sampleTime = 10;

	// PID Thread
	stopPIDThread = new ManualResetEvent(false);
	PIDThread = new Thread (PIDThreadComputation);
}

/// <summary>

/// Default constructor

/// </summary>

public PIDController ()

{

// Fields initialization

init();

}

///

/// Fields initialization

///

private void init()

{

Ki = 0;

Kp = 0;

Kd = 0;

sampleTime = 10;

// PID Thread

stopPIDThread = new ManualResetEvent(false);

PIDThread = new Thread (PIDThreadComputation);

}

Public methods

The public methods of the PIDController start and stop the PIDThread .

/// <summary>
/// Starts the PID
/// </summary>
public virtual void Start()
{
	PIDThread.Start();
}

/// 
/// Stops the PID
/// 
public virtual void Stop()
{
	stopPIDThread.Set ();
	if (PIDThread.IsAlive) {
		PIDThread.Join ();
	}
}

/// <summary>

/// Starts the PID

/// </summary>

public virtual void Start()

{

PIDThread.Start();

}

///

/// Stops the PID

///

public virtual void Stop()

{

stopPIDThread.Set ();

if (PIDThread.IsAlive) {

PIDThread.Join ();

}

Private methods

The PIDController class has just two private methods: a very simple UpdateKs() to calculate $K_1$ , $K_2$ and $K_3$ , and the PIDThreadComputation() which in the end score the goal; note how the control signal is saturated to the boundaries before assignment to the output.

/// <summary>
/// Updates the K1, K2 and K3 constants
/// 
private void UpdateKs(){
	k1 = ki + kp + kd;
	k2 = -kp - 2 * kd;
	k3 = kd;
}

/// 
/// The core of the PID alogrithm computation
/// 
protected void PIDThreadComputation(){
	Thread.CurrentThread.IsBackground = true;

	float e;		// error at instant k
	float e1;		// error at instant k-1
	float e2;		// error at instant k-1
	float u;		// control signal at instant k
	float delta_u;	// control signal increment at instant k

	// Initialization
	e = 0f;
	e1 = 0f;
	e2 = 0f;
	u = 0f;
	delta_u = 0f;

	// PID Computation
	while (!stopPIDThread.WaitOne(SampleTime)) {
		// updating "memory" variables
		e2 = e1;
		e1 = e;

		// updating error
		e = SetPoint - InputSignal;

		// computating delta_u and u
		delta_u = k1 * e + k2 * e1 + k3 * e2;
		u = u + delta_u;

		// Saturating u
		if (u > MaxPower)
			u = MaxPower;
		if (u < MinPower)
			u = MinPower;
		
		OutputSignal = (sbyte) u;
	}
}

/// <summary>

/// Updates the K1, K2 and K3 constants

///

private void UpdateKs(){

k1 = ki + kp + kd;

k2 = -kp - 2 * kd;

k3 = kd;

}

///

/// The core of the PID alogrithm computation

///

protected void PIDThreadComputation(){

Thread.CurrentThread.IsBackground = true;

float e; // error at instant k

float e1; // error at instant k-1

float e2; // error at instant k-1

float u; // control signal at instant k

float delta_u; // control signal increment at instant k

// Initialization

e = 0f;

e1 = 0f;

e2 = 0f;

u = 0f;

delta_u = 0f;

// PID Computation

while (!stopPIDThread.WaitOne(SampleTime)) {

// updating "memory" variables

e2 = e1;

e1 = e;

// updating error

e = SetPoint - InputSignal;

// computating delta_u and u

delta_u = k1 * e + k2 * e1 + k3 * e2;

u = u + delta_u;

// Saturating u

if (u > MaxPower)

u = MaxPower;

if (u < MinPower)

u = MinPower;

OutputSignal = (sbyte) u;

}

The whole code

Here comes the full code. This page will be shortly updated with a link to a zip file to download. You can compile the above code using Xamarin Studio 5.10.1 (Build 6), with Mono 4.2.1 as active runtime and the Monobrick Firmware Library 1.2.0.39486.

/// <summary>
/// PID Controller class
/// </summary>
public class PIDController
{
	#region Properties
	/// 
	/// Get or sets the input used for PID calculation
	/// 
	/// The input.
	public sbyte InputSignal { get; set; }

	protected sbyte outputSignal;
	/// 
	/// Get the the control signal of the PID controller
	/// 
	/// The output.
	public virtual sbyte OutputSignal
	{
		get {
			return outputSignal;
		}
		protected set {
			if (outputSignal != value) {
				outputSignal = value;
			}
		}
	}

	/// 
	/// Get or sets the SetPoint used for regulation
	/// 
	public sbyte SetPoint { get; set;}

	private int sampleTime;
	/// 
	/// Get or sets the sample time
	/// 
	/// The sample time.
	/// Thrown if value is non positive
	public int SampleTime
	{
		get {
			return sampleTime;
		}
		set{ 
			if (value < 0) {
				ArgumentException ex = new ArgumentException ("The sample time must be a positive number");
				throw(ex);
			} else if (sampleTime != value) {
				sampleTime = value;
			}
		}
	}

	/// 
	/// Get or sets the minimum power
	/// or in other words the minimum allowable
	/// value for the control signal
	/// 
	/// The minimum power.
	public sbyte MinPower { set; get; }

	/// 
	/// Get or sets the maximum power
	/// or in other words the maximum allowable
	/// value for the control signal
	/// 
	/// The max power.
	public sbyte MaxPower { get; set;}


	private float kp;
	/// 
	/// Get or sets the proportional constant
	/// 
	/// The Kp
	/// Thrown if value is non positive
	public float Kp {
		get {
			return kp;
		}
		set {
			if (value < 0) {
				ArgumentException ex = new ArgumentException ("The proportional constant must be a positive number");
				throw(ex);
			} else if (kp != value) {
				kp = value;
				UpdateKs();
			}
		}
	}

	private float ki;
	/// 
	/// Get or sets the integral constant
	/// 
	/// The Ki
	/// Thrown if value is non positive
	public float Ki { 
		get {
			return ki;
		}
		set {
			if (value < 0) {
				ArgumentException ex = new ArgumentException ("The integral constant must be a positive number");
				throw(ex);
			} else if (ki != value) {
				ki = value;
				UpdateKs ();
			}
		}
	}

	private float kd;
	/// 
	/// Get or sets the derivative constant
	/// 
	/// The Kd
	/// Thrown if value is non positive
	public float Kd {
		get {
			return kd;
		}
		set {
			if (value < 0) {
				ArgumentException ex = new ArgumentException ("The derivative constant must be a positive number");
				throw(ex);
			} else if (kd != value) {
				kd = value;
				UpdateKs ();
			}
		}
	}
	#endregion

	#region Fields
	/// 
	/// Constants used int the "speed form" of the discrete PID algorithm
	/// 
	private float k1;
	private float k2;
	private float k3;

	// PIDThread
	private EventWaitHandle stopPIDThread; 
	private Thread PIDThread;
	#endregion

	#region Constructors
	/// 
	/// Default constructor
	/// 
	public PIDController ()
	{
		// Fields initialization
		init();
	}

	/// 
	/// Fields initialization
	/// 
	private void init()
	{
		Ki = 0;
		Kp = 0;
		Kd = 0;
		sampleTime = 10;

		// PID Thread
		stopPIDThread = new ManualResetEvent(false);
		PIDThread = new Thread (PIDThreadComputation);
	}
	#endregion

	#region Public methods
	/// 
	/// Starts the PID
	/// 
	public virtual void Start()
	{
		PIDThread.Start();
	}

	/// 
	/// Stops the PID
	/// 
	public virtual void Stop()
	{
		stopPIDThread.Set ();
		if (PIDThread.IsAlive) {
			PIDThread.Join ();
		}
	}
	#endregion

	#region Private methods
	/// 
	/// Updates the K1, K2 and K3 constants
	/// 
	private void UpdateKs(){
		k1 = ki + kp + kd;
		k2 = -kp - 2 * kd;
		k3 = kd;
	}

	/// 
	/// The core of the PID alogrithm computation
	/// 
	protected void PIDThreadComputation(){
		Thread.CurrentThread.IsBackground = true;

		float e;		// error at instant k
		float e1;		// error at instant k-1
		float e2;		// error at instant k-1
		float u;		// control signal at instant k
		float delta_u;	// control signal increment at instant k

		// Initialization
		e = 0f;
		e1 = 0f;
		e2 = 0f;
		u = 0f;
		delta_u = 0f;

		// PID Computation
		while (!stopPIDThread.WaitOne(SampleTime)) {
			// updating "memory" variables
			e2 = e1;
			e1 = e;

			// updating error
			e = SetPoint - InputSignal;

			// computating delta_u and u
			delta_u = k1 * e + k2 * e1 + k3 * e2;
			u = u + delta_u;

			// Saturating u
			if (u > MaxPower)
				u = MaxPower;
			if (u < MinPower)
				u = MinPower;
			
			OutputSignal = (sbyte) u;
		}
	}
	#endregion
}

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

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

/// <summary>

/// PID Controller class

/// </summary>

public class PIDController

{

#region Properties

///

/// Get or sets the input used for PID calculation

///

/// The input.

public sbyte InputSignal { get; set; }

protected sbyte outputSignal;

///

/// Get the the control signal of the PID controller

///

/// The output.

public virtual sbyte OutputSignal

{

get {

return outputSignal;

}

protected set {

if (outputSignal != value) {

outputSignal = value;

}

///

/// Get or sets the SetPoint used for regulation

///

public sbyte SetPoint { get; set;}

private int sampleTime;

///

/// Get or sets the sample time

///

/// The sample time.

/// Thrown if value is non positive

public int SampleTime

{

get {

return sampleTime;

}

set{

if (value < 0) {

ArgumentException ex = new ArgumentException ("The sample time must be a positive number");

throw(ex);

} else if (sampleTime != value) {

sampleTime = value;

}

///

/// Get or sets the minimum power

/// or in other words the minimum allowable

/// value for the control signal

///

/// The minimum power.

public sbyte MinPower { set; get; }

///

/// Get or sets the maximum power

/// or in other words the maximum allowable

/// value for the control signal

///

/// The max power.

public sbyte MaxPower { get; set;}

private float kp;

///

/// Get or sets the proportional constant

///

/// The Kp

/// Thrown if value is non positive

public float Kp {

get {

return kp;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The proportional constant must be a positive number");

throw(ex);

} else if (kp != value) {

kp = value;

UpdateKs();

}

private float ki;

///

/// Get or sets the integral constant

///

/// The Ki

/// Thrown if value is non positive

public float Ki {

get {

return ki;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The integral constant must be a positive number");

throw(ex);

} else if (ki != value) {

ki = value;

UpdateKs ();

}

private float kd;

///

/// Get or sets the derivative constant

///

/// The Kd

/// Thrown if value is non positive

public float Kd {

get {

return kd;

}

set {

if (value < 0) {

ArgumentException ex = new ArgumentException ("The derivative constant must be a positive number");

throw(ex);

} else if (kd != value) {

kd = value;

UpdateKs ();

}

#endregion

#region Fields

///

/// Constants used int the "speed form" of the discrete PID algorithm

///

private float k1;

private float k2;

private float k3;

// PIDThread

private EventWaitHandle stopPIDThread;

private Thread PIDThread;

#endregion

#region Constructors

///

/// Default constructor

///

public PIDController ()

{

// Fields initialization

init();

}

///

/// Fields initialization

///

private void init()

{

Ki = 0;

Kp = 0;

Kd = 0;

sampleTime = 10;

// PID Thread

stopPIDThread = new ManualResetEvent(false);

PIDThread = new Thread (PIDThreadComputation);

}

#endregion

#region Public methods

///

/// Starts the PID

///

public virtual void Start()

{

PIDThread.Start();

}

///

/// Stops the PID

///

public virtual void Stop()

{

stopPIDThread.Set ();

if (PIDThread.IsAlive) {

PIDThread.Join ();

}

#endregion

#region Private methods

///

/// Updates the K1, K2 and K3 constants

///

private void UpdateKs(){

k1 = ki + kp + kd;

k2 = -kp - 2 * kd;

k3 = kd;

}

///

/// The core of the PID alogrithm computation

///

protected void PIDThreadComputation(){

Thread.CurrentThread.IsBackground = true;

float e; // error at instant k

float e1; // error at instant k-1

float e2; // error at instant k-1

float u; // control signal at instant k

float delta_u; // control signal increment at instant k

// Initialization

e = 0f;

e1 = 0f;

e2 = 0f;

u = 0f;

delta_u = 0f;

// PID Computation

while (!stopPIDThread.WaitOne(SampleTime)) {

// updating "memory" variables

e2 = e1;

e1 = e;

// updating error

e = SetPoint - InputSignal;

// computating delta_u and u

delta_u = k1 * e + k2 * e1 + k3 * e2;

u = u + delta_u;

// Saturating u

if (u > MaxPower)

u = MaxPower;

if (u < MinPower)

u = MinPower;

OutputSignal = (sbyte) u;

}

#endregion

}

Check Part 4 for the application of a PID Controller to the position regulation of the Ev3 Medium Motor

The source code happily shared above is subject to the Code Project Open Licence (CPOL) 1.02.

The PID Controller – Part 3 of 4

Posts that you may like

Properties

Fields

Constructors

Public methods

Private methods

The whole code

Share this:

Posts that you may like