robocup rescue workshop 2015 · g. tardiani stage 1 –edge following 2015 • this program follows...

RoboCup RescueWorkshopPart 3

© G. Tardiani 2015

B

Advanced Line Following Algorithms

• Most Lego Line Following robots are of theDifferential Steer type

• The geometry of Drive Wheels to Sensor varies from anequilateral 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 onthe course in relation to the position of the sensorsand the inside pivot wheel!

© G. Tardiani 2015Simple Edge Follower

• Two Region Light Line• Light Sensor reads 40 for Black, 50 for White

© G. Tardiani 2015Stage 1 – Edge Following

• This program follows one side of the line

© G. Tardiani 2015A Better Edge Follower

• Three Region Light Line• We can now go straight

© G. Tardiani 2015Stage 2 ‐ A Better Edge Follower

• With a 3 level decisionalgorithm, we can now gostraight.

• Even though this is stilla single sensor LineFollower this logic allowsthe robot to move withless of a wriggle

• IF < 43 Turn LeftIF > 47 Turn RightIF <=47 AND >=43 Straight

© G. Tardiani 2015Sensor 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 2015Smoothing out the Line Follow Action

• The table shows sensor values as it is movedover the edge of the line

• The full table would have 101 possible readings• We will reduce 101 to 5 by dividing by 5• Note: that we get more zeros than 5’s. This can be adjusted by adding 1, 2 or 3 to the sensor reading before dividing by 5. You will need to experiment to get the best set of Integer Values. Use the Round Block.

Calibrated Light Sensor Reading

LS / 20 = Integer Value

0 /20 = 05 /20 = 0

10 /20 = 015 /20 = 020 /20 = 125 /20 = 130 /20 = 135 /20 = 140 /20 = 245 /20 = 250 /20 = 255 /20 = 260 /20 = 365 /20 = 370 /20 = 375 /20 = 380 /20 = 485 /20 = 490 /20 = 495 /20 = 4

100 /20 = 5

© G. Tardiani 2015Multiple 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 2015Progression 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 2015Two Sensors are better than One!

• Now the maths gets interesting• By using 2 sensors we further smooth the robot• (LS1 ‐ LS2) / 2 gives us a range from ‐50 to +50• Divide that by 15 to get our 7 options (3, 2, 1, 0, ‐1, ‐2, ‐3)

• However, we get to many ‐4’s (out of range)• Change‐ (LS1 – LS2) / 2 + 6 to fix the problem

Light Sensor 1 Reading

Light Sensor 2 Reading

LS1 ‐ LS2 / 2 =6 Int value /15

100 0 56 395 5 51 390 10 46 385 15 41 280 20 36 275 25 31 270 30 26 165 35 21 160 40 16 155 45 11 050 50 6 045 55 1 040 60 ‐4 ‐135 65 ‐9 ‐130 70 ‐14 ‐125 75 ‐19 ‐220 80 ‐24 ‐215 85 ‐29 ‐210 90 ‐34 ‐35 95 ‐39 ‐30 100 ‐44 ‐3

© G. Tardiani 2015Two 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 2015Proportional 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 2015Equation 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 2015Understanding 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 2015Understanding 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 2015Pseudocode 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 = 32Offset = 46

• (60+32) / 2 = 46

© G. Tardiani 2015

Pseudocode for P ControlKp & Tp

• Kp controls how fast the controller will try to get back to theline 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 2015A Proportional (P) Algorithm

Kp = 10 // Initialize our three variablesoffset = 45 // White value + Black value /2Tp = 50 // Target power

Loop foreverLightValue = 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 motorpowerC = Tp ‐ Turn // the power level for the C motor

MotorB.direction=forward // issue the command with theMotorB.power=powerA // new power level in a MOTOR blockMotorC.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 2015The 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 2015Adding 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 2015Ki 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) AlgorithmKp = 10 // Initialize our variablesKi = 1 // Our new Integral Constantoffset = 45 // White value + Black value /2Tp = 50Integral = 0 // Set the integral accumulator to zero (0)

Loop foreverLightValue = read light sensor // what is the current light reading?error = LightValue ‐ offset // calculate the error by subtracting the offsetintegral = integral + error // the new Integral accumulatorTurn = Kp * error + Ki * integral // ”P” term and the “I” term


MotorB.direction=forward // issue the command with theMotorB.power=powerB // new power level in a MOTOR blockMotorC.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 2015Adding 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

© G. Tardiani 2015

Proportional Integral Derivative (PID) AlgorithmKp = 10 // Initialize our variablesKi = 1 Kd =100 // Our new Derivative Constantoffset = 45 // White value + Black value /2Tp = 50Integral = 0lastError = 0 // Set the LastError to zero (0)derivative = 0 // Set the Derivative to zero (0)

Loop foreverLightValue = read light sensor // what is the current light reading?error = LightValue ‐ offset // calculate the error by subtracting the offsetintegral = integral + error // the integral accumulatorderivative = error – lastError // calculate the derivativeTurn = (Kp * error) + (Ki * integral) + (Kd * derivative) // ”P” term and the “I” term and the “D”


MotorB.direction=forward // issue the command with theMotorB.power=powerB // new power level in a MOTOR blockMotorC.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 2015Tuning 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 2015A 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 2015Simpler 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 2015PID 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 FollowingMultiple Sensors

• To increase reliability and also allow the Rescue Robot to navigatethe 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 2015EV3 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 2015Eight Sensor Proportional Follower

• This algorithm uses the Mindsensors LightSensorArray

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

© G. Tardiani 2015Proportional Line Following in Rescue

• The Proportional Line Following algorithms may give a Rescuerobot 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 2015Acknowledgements

• J. Sluka – http://www.inPharmix.com.au• RoboCatz ‐ http://robocatz.com/linefollowing.htm

© G. Tardiani 20153rd Party Sensors and Actuators

• MTA – Everything Lego Educationhttp://www.teaching.com.au

• Omni Wheels – RotaCaster designed for Legohttp://www.rotacaster.com.au/

• Linear Actuators – Firgelli have NXT and EV3 modelshttp://www.firgelli.com

• HiTechnic – Official 3rd party Lego Sensorshttp://www.hitechnic.com/

• MindSensor – Unofficial 3rd party Lego Sensorshttp://www.mindsensors.com/

• Dexter Industries – Advanced sensorshttp://www.dexterindustries.com

robocup rescue workshop 2015 · g. tardiani stage 1 –edge following 2015 • this program follows...

Documents