Sup3r Car Program

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
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}$:

  • K_p = 5.0,
  • K_i = 0.5,
  • \mathit{maxOutput}_1 = 40,
  • T_{s1} = 10\mathit{ms}.

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

Sup3r Car Program in C# – Part 2
Tagged on: