RoboCup Rescue

Workshop
Part 3
 © G. Tardiani 2015
Advanced Line Following Algorithms
• Most Lego Line Following robots are of the
Differential Steer type
• The geometry of Drive Wheels to Sensor varies from an
equilateral triangle (as shown) 
to almost flat with the sensors just in front of the wheels
• Note: Each configuration will work as an effective 
Line Following robot, however, the most important 
consideration is the radius of the tightest curve on
the course in relation to the position of the sensors B
and the inside pivot wheel!
 © G. Tardiani 2015
Simple Edge Follower
• Two Region Light Line
• Light Sensor reads 40 for Black, 50 for White
 © G. Tardiani 2015
Stage 1 – Edge Following
• This program follows one side of the line
 © G. Tardiani 2015
A Better Edge Follower
• Three Region Light Line
• We can now go straight
 © G. Tardiani 2015
Stage 2 ‐ A Better Edge Follower
• With a 3 level decision
algorithm, we can now go
straight.
• Even though this is still
a single sensor Line
Follower this logic allows
the robot to move with
less of a wriggle
• IF < 43 Turn Left
IF > 47 Turn Right
IF <=47 AND >=43 Straight
 © G. Tardiani 2015
Sensor Calibration
• Calibrating Light Sensors optimises their ability to give 
consistent readings
• Instead of percentage readings where 
white = 59 and black = 37 (approximately)
• Calibration sets the white (or Silver) value to 100 and black to 0.
• It also allows us to do some clever maths
• Note: Some Rescue robots constantly re‐calibrate as they navigate the course adjusting the white and black levels!
 © G. Tardiani 2015
Smoothing out the Line Follow Action
Calibrated Light  LS / 20 = Integer 
Sensor Reading
0 /20 =
Value
0
• The table shows sensor values as it is moved
10
5 /20 =
/20 =
0
0
over the edge of the line
15
20
/20 =
/20 =
0
1 • The full table would have 101 possible readings
25 /20 = 1
30 /20 = 1 • We will reduce 101 to 5 by dividing by 5
35 /20 = 1
40
45
/20 =
/20 =
2
2
• Note: that we get more zeros than 5’s. This can be 
50
55
/20 =
/20 =
2
2
adjusted by adding 1, 2 or 3 to the sensor reading 
60
65
/20 =
/20 =
3
3
before dividing by 5. 
70 /20 = 3 You will need to experiment to get the best set of 
75 /20 = 3
80 /20 = 4 Integer Values. Use the Round Block.
85 /20 = 4
90 /20 = 4
95 /20 = 4
100 /20 = 5
 © G. Tardiani 2015
Multiple Options
• Now we can build in some smoothness using the 
Switch block, in Tabbed view (CaseWhere)
• Adjust the speed of the motors to get the robot following the line
• Try increasing the number of tabs to further smooth out the robot
 © G. Tardiani 2015
Progression to Proportional (P)
• 2‐level robot can only turn left or right well
• 3‐level robot can also go straight
• Proportional robots have a linear relationship between the distance 
from the edge and the speed of the turn (motor speeds)
 © G. Tardiani 2015
Two Sensors are better than One!
Light Sensor  1  Light Sensor 2  LS1 ‐ LS2 / 2 =6 Int value 
Reading Reading /15 • Now the maths gets interesting
100 0 56 3
95
90 10
5 51
46
3
3 • By using 2 sensors we further smooth the robot
85 15 41 2
80
75
20
25
36
31
2
2
• (LS1 ‐ LS2) / 2 gives us a range from ‐50 to +50
70 30 26 1
65 35 21 1 • Divide that by 15 to get our 7 options 
60 40 16 1
55 45 11 0 (3, 2, 1, 0, ‐1, ‐2, ‐3)
50 50 6 0
45
40
55
60
1
‐4
0
‐1 • However, we get to many ‐4’s (out of range)
35 65 ‐9 ‐1
30
25
70
75
‐14
‐19
‐1
‐2
• Change‐ (LS1 – LS2) / 2 + 6 to fix the problem
20 80 ‐24 ‐2
15 85 ‐29 ‐2
10 90 ‐34 ‐3
5 95 ‐39 ‐3
0 100 ‐44 ‐3
 © G. Tardiani 2015
Two Sensor Line Follower–The start of P!
• Using the Advanced Maths block simplifies things in EV3
• (((a‐b)/2)+6)/15
• The (a‐b)/2 is the start of the Proportional solution
 © G. Tardiani 2015
Proportional Line Follower
• Error, is the value on the 
blue line away from zero.
• The further into the Black Line, the 
more negative the error therefore the 
faster the robot needs to turn to get 
back to the edge of the line
• Conversely, the further into the White 
zone, the more positive the error
 © G. Tardiani 2015
Equation of a Line
• y = mx + b
• Y is the distance up or down the y axis
• X is the distance on the x axis
• m is the slope of the line
• B is where y intercepts the line when x = zero
• We know B will cross at 0,0 so the equation is

• Y = mX
 © G. Tardiani 2015
Understanding Proportional Control
• Error – calculates how far away from the line edge the robot is,
the X value
• Turn – How hard we need to turn to get back to the edge of the line 
can be the Y value
• Now we have

• Turn = m * Error
 © G. Tardiani 2015
Understanding Proportional Control
• We are left with M which is the slope
• The slope is a proportionality constant and is the factor that you have 
to multiply the error (x value) by, to convert it into a Turn (y value)
• In PID terms this is called Kp (Konstant) remember that mathematician's can’t spell

• Turn = Kp * Error
 © G. Tardiani 2015
Pseudocode for P Control ‐ Error
• First we need to measure the values the light sensor returns 
for white and black
• From those two number we can calculate the Offset
• The offset is just the average of the white and black readings
• If white = 60 and black = 32
Offset = 46

• (60+32) / 2 = 46
 © G. Tardiani 2015
Pseudocode for P Control
Kp & Tp
• Kp controls how fast the controller will try to get back to the
line edge when it has drifted away from it. 
(Kontrol)

• Tp controls how fast the robot is moving along the line. 
(Target power) Start low, eg. 20

• Offset is the sensor value when exactly on the edge of the line, OR 
the average value of Black and White.
 © G. Tardiani 2015
A Proportional (P) Algorithm
Kp = 10                                // Initialize our three variables
offset = 45                                                     // White value + Black value /2
Tp = 50 // Target power

Loop forever
LightValue = read light sensor    // what is the current light reading?
error = LightValue ‐ offset    // calculate the error by subtracting the offset 
Turn = Kp * error     // ”P” term, the amount to change the  motors' power 
powerB = Tp + Turn                                   // the power level for the A motor
powerC = Tp ‐ Turn                   // the power level for the C motor

MotorB.direction=forward  // issue the command with the
MotorB.power=powerA // new power level in a MOTOR block
MotorC.direction=forward  // same for the other motor 
MotorB.power=powerC // but using the other power level
end loop forever                       // done with this loop, go back to 
// the beginning and do it again
 © G. Tardiani 2015
The Proportional EV3 code 
• Adjust the Kp down to slow the robots reaction speed
• Adjust the Tp down to slow the robots target power
 © G. Tardiani 2015
Adding the I (Integral) to PID
• The Integral is the running sum of the Error over time
• It helps remove small errors that might make the robot over react
• For example the robot might be near the line, however, P alone 
causes it to wriggle back to a perfect alignment.
The Integral tells the robot to ‘Chill’ don’t over react lets wait until 
there is a real error and then we will turn.
• Integral lets the robot travel straight on a straight line even if it’s 
drifting slightly off the line and delays the robots reactions until they 
are really necessary, like a bend in the line.
 © G. Tardiani 2015
Ki is our new Integral Constant
• We use the Integral to accumulate the errors using the equation
• Integral = integral + error
• This strange equation simply adds the error to the current value of 
integral each time the algorithm iterates (loops). (An Accumulator)
• Yes the Integral will grow and grow which may cause the robot to 
overshoot the line if allowed to continue growing
• There are ways to control it using time, resetting to zero when the 
sign changes and Dampening by multiplying it by less than one.
• For this example, we will keep things simple
 © G. Tardiani 2015
Proportional Integral (PI) Algorithm
Kp = 10                                                           // Initialize our variables
Ki = 1                                // Our new Integral Constant
offset = 45                                                     // White value + Black value /2
Tp = 50
Integral = 0                                                    // Set the integral accumulator to zero (0)
Loop forever
LightValue = read light sensor    // what is the current light reading?
error = LightValue ‐ offset    // calculate the error by subtracting the offset
integral = integral + error // the new Integral accumulator
Turn = Kp * error + Ki * integral // ”P” term and the “I” term 
powerB = Tp + Turn                                   // the power level for the A motor
powerC = Tp ‐ Turn                   // the power level for the C motor

MotorB.direction=forward  // issue the command with the
MotorB.power=powerB // new power level in a MOTOR block
MotorC.direction=forward  // same for the other motor 
MotorB.power=powerC // but using the other power level
end loop forever                       // done with this loop, go back to 
// the beginning and do it again
© G. Tardiani 2015
The PI EV3 code
 © G. Tardiani 2015
Adding D (Derivative) to complete PID
• The Derivative has a crystal ball and can predict the future?
• We look into the future and assume that the next ‘change’ in the 
error is the same as the last ‘change’ in the error!
• The change in the error is the Derivative, therefore
• theCurrentDerivative = theCurrentError – the PreviousError, leading 
to
• nextError = theCurrentError + theCurrentDerivative
• However, to keep things simple we simply add
• Kd * Derivative to our Turn equation
Proportional Integral Derivative (PID) Algorithm

© G. Tardiani 2015
Kp = 10                                                  // Initialize our variables
Ki = 1                               
Kd =100            // Our new Derivative Constant
offset = 45                                   // White value + Black value /2
Tp = 50
Integral = 0
lastError = 0 // Set the LastError to zero (0)
derivative = 0 // Set the Derivative to zero (0)
Loop forever
LightValue = read light sensor    // what is the current light reading?
error = LightValue ‐ offset    // calculate the error by subtracting the offset
integral = integral + error // the integral accumulator
derivative = error – lastError // calculate the derivative
Turn = (Kp * error) + (Ki * integral) + (Kd * derivative)   // ”P” term and the “I” term and the “D” 
powerB = Tp + Turn                                   // the power level for the A motor
powerC = Tp ‐ Turn                   // the power level for the C motor
MotorB.direction=forward  // issue the command with the
MotorB.power=powerB // new power level in a MOTOR block
MotorC.direction=forward  // same for the other motor 
MotorB.power=powerC // but using the other power level
lastError = error // save the current error so it can be the lastError next time 
end loop forever                       // done with this loop, go back and do it again
© G. Tardiani 2015
The PID EV3 code
 © G. Tardiani 2015
Tuning the PID controller
• Add things slowly and in small amounts
• By setting Ki & Kd to zero will turn them off giving you a P controller
• Set the Kp max power divided by the expected highest error eg 5
• That way, an error of +5 will set the motor power to it’s max (100)
alternatively, set Kp to 1. Increase if the robot doesn’t LineFollow
• If the robot oscillates wildly then decrease Kp and keep changing it 
until your robot follows the line with a smooth oscillation
• To get started try: Kp = 2.5,   Ki = 0.05,   Kd = 5
• Remember that Tp controls the motor power start at 25 slowly increase
• In Rescue a reliable robot usually does best. Chase speed at your peril
 © G. Tardiani 2015
A word of warning to Rescuers
• The above PID algorithm should allow you to program your 
Rescue robot to follow the line really, really well
• However, your Rescue robot needs to do a whole bunch of things at 
the same time, for instance, find the Obstacle and the Chemical Spill 
and the Victims both real and not, as well as the Platform and, and!
• So in reality the application of a PID line following program is complex
• Keep things as simple as possible
• Look at the simpler Proportional Line Followers in the next group of 
slides and make smart decisions when solving the RCJA Rescue!
 © G. Tardiani 2015
Simpler Proportional Line Following
• Complete PID mathematics is very complex and the above 
method is a very simplified example but still way to complex
• Check out J. Sluka’s – http://www.inPharmix.com.au document for 
the complete guide to ‘simplified’ PID line following
• The following slides introduce workable Proportional type Line 
Following examples that could be used with a Rescue robot.
• Note: All sensors and motors react differently on each robot, field and 
venue so any of the examples shown will need adjustments to 
program values before they will start to work as indicated.
 © G. Tardiani 2015
PID method using multiple sensors
• There are a couple approaches, here are two:  (Jim from InPhamix)
1. Put the sensors close together and use them like they are one big sensor by just adding their 
light readings. When you are aligned with the line one sensor reads white and the other black. 
The PID is the same as before though the calibrations (Ks) will be different. The pair of sensors gives a much wider 
dynamic range and the robot can get a proportional reading of error over a larger range of deviations from the line. 
You should check the pair of sensors response as you swing over the line to make sure it is reasonably linear, 
particularly when one of the sensors goes out of the linear range. If there is a brake in the response then move the 
sensors closer or farther apart to minimize. If the total sensor width is wider than your line you might have problems 
that could be fixed in code with enough effort.
2. Separate the two light sensors by a bit more than the line's width and try to straddle the line. The target value is the 
difference of the readings when you are straddling the line correctly. The error is the target minus the difference 
between the two sensors at any given moment. You'll need to think about the signs to get the robot to turn in the 
correct direction.
For example, when perfectly straddling the line left=45 and right=47 (they should be identical except no two sensors 
are identical). So the TARGET is 45‐47=‐2. If at a particular instance the left sensor is partly over the line then 
left=42(more black) and right=53(more white) and the difference is 42‐53=‐11. To get the error subtract the target 
from the difference or vice versa depending on the signs of things (whatever it is it will always be the same in the 
code) ‐2 ‐ (‐11) = +9. The sign of the result gives the direction to turn, in this example plus means turn left.
 © G. Tardiani 2015
Proportional Line Following
Multiple Sensors
• To increase reliability and also allow the Rescue Robot to navigate
the entire course more than one sensor is needed.
• The following examples take advantage of the Move Steering Block 
which does some of the maths for us in steering the robot.
• We will start again with a single Proportional Line Follower and 
progress all the way up to an 8 sensor array.
• Note: RoboCup Rescue does not allow pre‐programmed PID Blocks 
OR Light Sensor arrays with built in PID algorithms.
Teams must program the array using the individual light sensor values
 © G. Tardiani 2015
EV3 Move Steering Block
• The Move Steering Block allows us to input steering values between
+100 to –100
• A Steering Input of zero (0) will make the robot drive straight
• +/‐50 will make the robot turn, with one motor on and the other stopped
• +/‐100 will make the robot pivot, with one motor forward and the other 
motor in reverse
• If the pivot is to aggressive then adjust your program to a manageable 
max and min value of e.g. +/‐75 for a less aggressive pivot 
 © G. Tardiani 2015
Single Sensor Proportional Follower
 © G. Tardiani 2015
Two Sensor Proportional Follower
 © G. Tardiani 2015
Three Sensor Proportional Follower
 © G. Tardiani 2015
Four Sensor Proportional Follower
 © G. Tardiani 2015
Eight Sensor Proportional Follower

• This algorithm uses the 
Mindsensors LightSensorArray

• Note: The Mindsensors LineLeader is illegal to use in RoboCup Junior

 © G. Tardiani 2015
Proportional Line Following in Rescue
• The Proportional Line Following algorithms may give a Rescue
robot a line following advantage however;
• You will need to solve the problem of how the robot navigates the 
Intersections on the Rescue field.
• These examples will all need programming adjustments to suit robots
• The Multi‐tasking method in part 2 of this tutorial can still be used to 
solve the Rescue challenge.

• Good luck and hope this helps you with  development
 © G. Tardiani 2015
Acknowledgements
• J. Sluka – http://www.inPharmix.com.au
• RoboCatz ‐ http://robocatz.com/linefollowing.htm
 © G. Tardiani 2015
3rd Party Sensors and Actuators
• MTA – Everything Lego Education
http://www.teaching.com.au
• Omni Wheels – RotaCaster designed for Lego
http://www.rotacaster.com.au/
• Linear Actuators – Firgelli have NXT and EV3 models
http://www.firgelli.com
• HiTechnic – Official 3rd party Lego Sensors
http://www.hitechnic.com/
• MindSensor – Unofficial 3rd party Lego Sensors
http://www.mindsensors.com/
• Dexter Industries – Advanced sensors
http://www.dexterindustries.com