Line following C# program with the SNATCH3R

Sooner or later every enthusiast programs a Lego Mindstorms Ev3 robot to follow a line. Indeed the line following task is the first that I tried myself, and there is a post on this blog where I show the results obtained with the Sup3r Car.

Here I show how to program the SNATCH3R to follow a line with C# and the Monobrick firmware library; the main difference between the Sup3r Car and the SNATCH3R program is that the Sup3r Car uses a PID (Proportional Integrative Derivative) controller to stay on the track, while the SNATCH3R uses a much simpler look up table.

You can download the code from Smallrobots.it repository on GitHub.

The line following task for the SNATCH3R

The task consists in programming an Ev3 robot to follow a line on the floor usually marked with a coloured tape; to sense the line on the floor the robot uses an Ev3 Color Sensor in Reflection Mode.

The SNATCH3R carries also an Ev3 IR Sensor set in proximity mode to detect the obstacles along the line. Once an obstacle is detected, the robot uses the gripper to grab and remove it.

The state machine

The SNATCH3R state machine has the following states:

: Follow the line until an obstacle is found
: Grab the obstacle
: Turn right
: Move forward
: Release the obstacle
: Move backward
: Turn left and when done get back to state . 

This state machine is very simple in the sense that it is just a sequence. Once the only action inside a state is completed, the current state changes to the following one in the sequence.

The only exception to the rule above is the state: the robot changes from to only when the Ev3 IR Sensor detects an obstacle along the way.

The states of the state machine are defined as follows:

Each state has a dedicated method which updates the robot behaviour, so the state machine looks clean:

Follow the line

To stay on the track the robot must sense the line on the floor with the Ev3 Color Sensor; the sensor feedback is then compared to a predefined Set-Point value. The Set-Point value is the value that the Ev3 Color Sensor reads when the red light it emits is half on the line and half on the floor.

The algorithm is simple: if the difference between the Set-Point and the sensor feedback is positive the robot must steer toward the line, while if the sensor feedback is negative the robot must steer toward the floor. With this statement I’m assuming that the line is darker than the floor.

To avoid sharp turn, the amount of correction is not constant but depends on how far the robot is from the Set-Point: if the feedback is close to the Set-Point the robot makes a small correction, while if the feedback is far from the Set-Point the robot makes a greater correction to its course.

Have a look at the table on the left for the values used in the program proposed. For the black tape I used, and the floor in my home a good value for the SetPoint is 40, however these values should be tuned for your environment condition.

In the following code, observe also how the current state changes when an obstacle is detected by the Ev3 IR Sensor.

The look up table is a simple class which performs a linear search in an array as follows:

Grab the obstacle

The SNATCH3R performs this action in open loop: it simply rotates the Ev3 Medium Motor until its tachimeter count equals or exceeds 3500 hits, then the current state is changed to turn right.

Turn right

The action is performed in open loop. The two caterpillars move in opposite direction with equal speed until the left Ev3 Large Motor tacho count equals or exceeds 400. Actually this method turns the robot once to the left and once to the right for each obstacle encountered.

Move forward

This state contains an action performed in open loop. Both Ev3 Large Motors move forward until the left tacho count reaches or exceeds 400 (same value as turn right).

Release the obstacle

The SNATCH3R performs this action in open loop: the Ev3 Medium Motor rotates until its tachimeter count returns to zero, then the current state is changed to move backward.

Move backward

The SNATCH3R performs this action in open loop: the two Ev3 Large Motors rotate until left motor tachimeter count equals or exceeds -400 hits, then the current state is changed to turn left.

Turn left 

The action is performed in open loop. The two caterpillars move in opposite direction with equal speed until the robot is back on track. To give the Ev3 Color Sensor a bit of tolerance in finding the black tape again, the SNATCH3R counter rotate 20% less than the amount of initial rotation. After that the state machine is back again to the one 

Happy coding!