Sup3r Car Program in C# - Part 2

Download the source code (Please register before proceeding to download)

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.

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

← Part 1 – The track following problem
→ Part 3 – The (simple) code architecture

The control strategy

As discussed in Part 1, the task of keeping the vehicle on the track can be translated in maintaining the feedback of the Ev3 Color sensor on a predefined set point $l_{sp}$ .

To achieve this goal, assuming that we want that the vehicle follows the left edge of a black stripe on a light grey floor, we program the vehicle to steer to the left if the sensor feedback value is smaller than the set point (the vehicle is mostly on the black stripe) and to steer to the right if the feedback is instead greater than the set point (the vehicle is mostly on the floor).

The strategy is then to design two nested control loops. The external loop, I call it the Drive loop, determines how much to steer and in which direction; the internal loop, I call it the Steer loop, takes the input from the external loop and steer accordingly.

Control strategy for the track following program.

The picture above represents the strategy where:

$l_{sp}$ is the reflected light set point; the vehicle will steer to keep the sensor readings as close to this value as possible;
$C_1$ is the proportional controller that computes how much to steer to compensate for the track following error;
$\alpha_{sp}$ is the output of the $C_1$ controller, and it is the set point for the $C_2$ proportional integrative controller;
$u$ is the output of the $C_2$ controller, and it is the torque requested to the Ev3 Medium Motor to steer;
$P_2$ is the Ev3 Medium Motor;
$P_1$ is the vehicle equipped with the Ev3 Color Sensor $S_1$ ;
$S_2$ is the Ev3 Medium Motor tachometer;
$S_1$ is the Ev3 Color Sensor in reflected light mode;
$\alpha$ is the reading of Ev3 Medium Motor tachometer;
$l$ is the reading of Ev3 Color Sensor.

Here are the parameters I have chosen for the Drive loop, which is a proportional controller with a saturation $\mathit{maxOutput}_2$ , a dead band $d_b$ and a sample time $T_{s2}$ :

$l_{sp} = 15.0$ ,
$K_p = 0.8$ ,
$\mathit{maxOutput}_2 = 40$ ,
$d_b=5.0$ ,
$T_{s2} = 100\mathit{ms}$ .

And for the Steer loop, which is instead a proportional integrative controller with a saturation $\mathit{maxOutput}_1$ and a sample time $latex T_{s1}$: