Lego Mindstorms Nxt Segway Program

NXT Segway with RiderFunProjects foryour LEGO ®MINDSTORMS ®NXT!NXT Segway with Rider::This robot simulates a, which is atwo-wheeled self-balancing vehicle that a rider stands on.

If you usually program in NXT-G with LEGO Mindstorms 1.0 or 1.1, then this is a great opportunity to give NXC a try. If you install the development.

PID Controller For Lego Mindstorms Robots A PID Controller For Lego Mindstorms RobotsA PID Controlleris acommon technique used to control a widevariety of machinery including vehicles, robots and even rockets. Thecomplete mathematical description of a PID Controller is fairly complexbut a much simpler understanding is really all that is needed to use aPID effectively.This document is a description of how to create a PIDController foruse with Lego Mindstorms Robots using the NXT-G programminglanguage.It will be easier if we have anactualtask in mind so I will describe how to create a PID to do linefollowing. Once created, the same PID can be used, with only minormodifications, with any other Mindstorms application such as getting arobot that can drive as straight as possible, or even for a robot thatcan balance with nothing but 2 wheels touching the ground like aSegway.A PID is really pretty straight forward and the typicaldescription of a PID is easily understood by anyone that has hadCalculus. This document is targeted towards First Lego League kidsin third through eighth grade. Sincethere aren't many kids that have had Calculus I'll try to build thewhole concept up from a very simple starting point without using anyCalculus.So lets start with the basic layout of a robot that would besuitable for line following. At the right is asimplified drawing of a top view of the robot with all the details weneed. Therobot is adifferential steer robot with two motors, each connected to one of thewheels A and C.

The robot has a light sensor mounted at the front thatpoints straight down so it sees nothing but the mat (floor, ground,table top, whatever the robot is on). The red circle representsthe fairly small spot on the mat that the light sensor can actually'see'. The rest of the robot is the large rectangle with an arrow, thearrow shows the normal direction of travel.Our goal is to get the robot to follow the fat black line. Linefollowing is a basic robotic behavior and is often one of thefirst things people learn.

A mobile device that can follow a linedisplays all the characteristics of a true robot. It uses sensor togather information about the world around it and changes it'sbehavior depending upon that information.Line followers can be built with one light sensor, or two, oradozen or however many you happen to have. In general, the more lightsensors you have the better you can follow a line. Here we'll limitourselves to a single Mindstorms light sensor.

Even with a singlesensor we should be able to build a robot that can track the line veryprecisely even if the line has curves in it. The thing youusually loose with a single sensor is the ability to follow the linewhile moving fast. Often, the more sensors you have the faster therobot can move while it follows the line.The first trick we'll use, which is unrelated to a PID, isthat wewon't try to actually follow the line. Instead, we'll try to follow theedgeof theline. Becauseif we follow the line itself (the black) then when the robot drifts offthe line and the sensor 'sees white' we don't know which side of theline we are on. Are we left or right of the line?

If we follow theline's edgethen we can tellwhich way weare off the edgeas the robotdrifts off the line. If the light sensor 'sees white' then we know itis left of the line's edge (and the line). If it 'sees black' then weknowit is to the right of the line's edge (and on the line). This is calleda ' left hand linefollower'since itis following the line's left edge.We need to know what values the light sensor returns when it'sees white' and when it 'sees black'. A typical uncalibrated sensormight give a 'white' reading of 50 and a 'black' reading of 40(uncalibrated, on a 0 to 100 scale). It is convenient to draw thevalues on a simple number line to help visualize how we convert light sensor values intochangesin the robot's movement. Below are our made uplight values for white and black.We'll just divide the range into two equal pieces and say thatifthe light level is less than 45 we want the robot to turn left.

If itis greater than 45 we want to turn right. I won't go into how exactlythe turns should be done. I'll just say that gentle turns work well fora fairly straight line. A line with lots of curves usually needs to bemaking sharper turns. For gentle turns you might use Power levels of50% on the fast wheel and 20% on the slow wheel.

Kode registrasi winzip for mac. Millions and billions of peoples use these archive for its flexibility and its quality. There are added some new image management tools that user can efficiently manage images and share it on different platforms. All these qualities have made WinZip 21.5.21520 a standard program for compression of data tools. There you can quickly secure zip and unzip files to personal storage space as well as speeds up e-mail transmission all the time.WinZip crack Latest version free download can compress the MP3 file with its latest technology in which the user has to not worry about any issue regarding any tool or feature.

For sharper turns ona curvy line you might need to use 30% power for the fast wheel andcoast or brake the slow wheel. Whatever power levels you use thenumbers will be the same for the two turns, you just switch which motorgets the big number and which get the smaller number (or a stopcommand).This type of a line follower will follow a line but it isn'tverypretty. It looks OK on a straight line with with the motors programmedfor gentle turns. But if the line has any curvesthen you have tell the robot to use sharper turns to follow line. Thatmakes the robotswing back and forth across the line. The robot only 'knows' how to dotwo things; turn left and turn right. This approach can be made to workbut it isnot very fast or accurate and looks terrible.In the above approach the robot never drives straight,even if it is perfectly alignedwith line's edge and the line is straight.

That doesn't seem veryefficient does it?Lets try to fix that. Instead of dividing our light valuenumberline into two regions lets divide it into three.So now if the light level is less than 43 we want the robot toturnleft.

If the light value is between 44 and 47 we want it to gostraight (zoom zoom). If the light level is greater than 47 we want toturn right. This can be easily be implemented in MindstormsNXT-G witha switch (yes/no)within a switch. You actually only have to do two tests not three.This approach works better than the first one. At least nowtherobot is sometimes movingstraight forward.

As with the first approachyou still have to decide what kinds of turns you need and that usuallydepends on the characteristics of the line you are following. The robotwill probably still hunt back a forth a fair amount.The astute reader will probably have thought ' well if three lightranges are better than two than what about adding even more?' Thatis the beginning of a PID. The 'P' in 'PID': Proportion(al) is the keySo what will happen if we add more divisions to our lightscaleline? Well the first thing we have to deal with is what does 'turn'mean with more than three light ranges? In our first approach the robotcould do just two things, turn left or right. The turns were always thesame just in opposite directions.

In the second approach we added the'go straight' to the two turns. If we have more than three rangesthen we need more 'kinds' of turns.To help understand 'more kinds of turns' we will redo ornumber linea bit and convert it into a graph. Our X-axis (horizontal) will be ourlight values just like on the number lines.

The Y-axis (vertical) wellbe our 'turn' axis.On the left is our original two level setup expressed on agraph.The robot can only dotwo things (shown by the blue lines), turn right or left and the turnsare always the same exceptfor their direction. In thecenter is the three level follower. The added center range is where therobot drives straight (Turn=0).

The turns are the same as before. Ontheright is a Proportional linefollower. In a proportional line follower the turn varies smoothlybetween two limits. If the light sensor reading says we are close tothe line then we do a small turn. If we are far from the line then wedo a big turn. Proportional isan important concept. Proportional means there is a linearrelationship betweentwovariables.

To put it even simpler, proportional means a graph of thevariables against each other produces a straight line(as in theright hand graph above).As you may know, the equation of a straight line is:y = mx + bWhere y is the distance up (or down) the Y-axis, x thedistance on theX-axis, m is the slope of the line and b is the Y intercept, the pointwhere the line crosses the Y-axis when x is zero. The slopeofthe line is defined as the change in the y value divided by the changein the x value using any pair of points on the line.If you don't know much about lines (or have forgotten what youoncenew) I'll expand a bit and make some simplifications to our graph andequation.

First, we will shift the center of our light number line(the X-axis) to zero. That's easy to do. For our 40 and 50 light valuerange we just subtract 45 (that's the average of 40 and 50, (40+50)/2 )from all of our lightreadings. We will call that result the error.So, if the light value is 47 we subtract 45 and get an error=2. The errortells us how far off the line's edge we are. If the lightsensor is exactly on the line's edge our error is zero sincethe light valueis 45 and we subtract 45 from all of our readings.

If the sensoris all the way out into the white our erroris +5. All the way into theblack the error is-5.In the above graph I have shifted the axis by converting it toanerror scale. Since the line now crosses the Y-axis at zero that means bis is zero and theequationfor the line is a bit simpler;y = mxor using our labelsTurn=m. errorWe haven't yet defined what the turn axis means so for now wewilljust say the turns range from -1 (hard turn to the left) to +1 (hardturn to the right) and a zero turn means we are going straight. Theslope of the line in the graph above can be calculated using the twopoints marked in red (any two points on the line will work);slope = m = (change in y)/(change in x) = (1- (-1)) / (-5 - 5 ) =2/-10= -0.2The slope is a proportionalityconstant and is the factor that you have tomultiply the error(xvalue) by to convert it into a Turn(yvalue). That's an important thing to remember.The 'slope' has a couple names that all mean the same thing,atleast in this context.

In the PID literature slopes (proportionalityconstants, m in the equation of a line) are called ' K' (from misspellingof the word ' constant'?).Various Ksshow up all overthe PID literature and are a veryimportant. You can think of a K(or m or slope or proportionality constant) as a conversion factor.You use Ktoconvert a number that means one thing (light values or error in ourcase) into something else like a turn.

That's all that a K does. Verysimple and very powerful.So using these new names for our variables the equation of theline is;Turn=K.( error)In words that's 'take the errorandmultiply it by the proportionality constant K to get the neededturn.The value Turn isthe outputof our P controller and is calledthe ' P term'since this is onlya proportional controller.You may have noticed that in the last graph the line does notextendoutside the error rangeof -5to +5. Outsidethe range of -5 to +5 we can't tell how far the sensor is fromthe line. All 'white' looks the same once the sensorcan't see any black at all. Remember that this range isarbitrary, your range will be determined by you light sensor setup, thecolors of the mat etc. Once the light sensor gets too far from theline edge it starts to give a constant reading, that means the lightsensor reading is no longer proportional to the error.

We can onlyjudge howfar the sensor is from the line's edge when the sensor is actuallypretty close to it. Over that narrow range the light sensor reading isproportional to the distance. So our sensor setup has a limited rangeover which it gives proportional information. Outside that range ittells us the correct direction but the magnitude (size) is wrong. Thelight sensor reading, or the error,is smaller than it should be anddoesn't give asgood an idea of what the turn should be to fix the error.In thePID literature the range over which the sensor gives a proportionalresponse is called the ' proportionalrange' (go figure:D ).

The proportional range is anotherveryimportant concept in PIDs. In our line follower the proportional rangefor the light sensor is 40 to 50, for the error it is -5 to+5. Our motorsalso have a proportional range, from -100 (full power backwards) to+100 (full power forwards).

I'll just say a couple thingsabout it the importance of the proportional range:(1) You want the proportional range to be as wide as possible.Ourlight sensor's proportional range is pretty small, that is, the sensorhas to be pretty close to the line edge to get proportionalinformation. Exactly how wide the range is depends mostly on how highthe sensors is above the mat. If the sensor is very close to the mat,say 1/16 inch, then the sensor is seeing a very small circleonthemat.

A small side to side movement of the light sensor will swing the error from -5 to+5, that's allthe way through our proportional range. You might say the sensor has'tunnel vision' as it can only see a very small part of themat.

Thesensor has to be very close to the line edge to get a reading thatisn't either 'white' or 'black'. If the sensor is moved higher off themat then it sees a larger circle on the mat.

At a height of about 1/2inch the light sensor appears to be looking at a circle on the mat thatis about 1/2 inch across. With the sensor up this high the proportionalrange is much wider, since the light sensor only needs to stay within+/- 1/2inch of the line edge to maintain a proportional output. Unfortunately,there are two drawbacks to a highlight sensor.

First, a high light sensor 'sees', and responds to, theroom lightsmuchmorethan a low sensor. A high sensor also has less difference between blackand white than a low sensor.

At a sufficiently large distance black andwhite will give the same reading.(2) Outside the proportional range the controller will movethingsin the correct direction but it will tend to under correct. Thecontroller's proportional response is limited by the proportionalrange.

Cheat engine 6.5.1 download. From P to actual motor power levelsHow can we implement theturns? What should the actual motor power levels be?

One way to do theturns is to define a ' Targetpowerlevel', whichI'llcall ' Tp'. Tp is the power level ofboth motorswhen the robot is supposed to go straight ahead, which it does whenthe error =0. Whentheerror isnot zero we use theequation Turn= K.( error) to calculatehow to change the power levels for the two motors. One motor will get apower level of Tp+ Turn, the othermotor will get apower level of Tp-Turn.Note that since our erroris-5 to +5 that means Turncanbe either positive or negative which corresponds to turns in oppositedirections. It turns out that that is exactly whatwe want since it will automaticallyset the correct motor as the fast motor and the other one as theslow motor. One motor (we'll assume it is the motor on the left of therobot plugged into port A) will always get the Tp+ Turnvalue as it's power level.

The other motor (right side of robot, portC) will always get Tp- Turn as it's powerlevel. If error ispositive then Turn ispositive and Tp+ Turnis greater than Tpand theleft motor speeds up while the right motor slows down.

If the error changes signand becomesnegative (meaning we have crossed over the line's edge and are 'seeingblack')then Tp+ Turnis now less than Tpand theleft motor slows down and the right motor speeds up since Tp-Turn is greaterthan Tp. (Rememberthat the negative ofanegative is a positive).

Hopefully it'll be a bit clearer aswe go on.Pseudo Code for a P ControllerFirst we need to measure the values the light sensor returnsforwhite and black. From those two number we can calculate the offset, that is, howmuch tosubtract from a raw light reading to convert it to an error value. The offset is just theaverage of thewhite and black readings. For simplicity I'll assume that the offsethas already been measured and stored in a variable called offset.

(Anice upgrade would be to have the robot measure the white andblack levels and calculate the offset.)We will also need a storage location for the K constant, we'llcall that Kp(the Konstantfor the proportionalcontroller). And,an initial guess as to what Kpshould be. There are a lot of ways to get that first Kp value. You canguessand then refine it by trial and error. Or, you can try to estimate avaluebased on the characteristics of the sensor and robot.

We'll do thelatter. We will use a Tp(target power) of 50, when the error is zero both motors will run atpower level 50. The error ranges from -5 to +5.

We'll guessthat we want the power to go from 50 to 0 when the error goes from 0to -5. That means the Kp(theslope remember, the change in y dividedbythe change in x) is;Kp= (0 -50)/(-5 - 0) = 10.We will use the Kp=10value toconvert an error valueinto a turn value.In wordsour conversion is 'for every 1 unit changein theerror wewill increase thepower of one motor by 10'. The other motor's powergets decreased by 10.So, in pseudo code ('pseudo code' means this isn't actualNXT-G, or any other type of program code, instead it is just a detailedlisting of what we want the program to do):Kp = 10! Initialize our three variablesoffset = 45Tp = 50Loop foreverLightValue = read light sensor! What is the current light reading?error = LightValue - offset!

Calculate the error by subtracting the offsetTurn = Kp. error!

The 'P term', how much we want to change the motors' powerpowerA = Tp + Turn! The power level for the A motorpowerC = Tp - Turn!

The power level for the C motorMOTOR A direction=forward power=powerA! Issue the command with the new power level in a MOTOR blockMOTOR C direction=forward power=powerC! Same for the other motor but using the other power levelend loop forever! Done with this loop, go back to the beginning and do it againThat's it, well almost. There is a subtle problem that shouldbecorrected.

But give it a try anyway. If your robot appears toavoid the line edge, instead of trying to find it, the most likelycauseisthat you have swapped the turn directions. Change Kp to -10 andseewhat happens. If that fixes the turn directions then change Kp back to+10 and change the signs in the two power lines to;powerA = Tp - TurnpowerC = Tp + TurnThere are two 'tunable parameters' and one constant in this Pcontroller. The constant is the offsetvalue (the average of white and black light sensorreadings).You'll need towrite a short program to measure thelight levels on your mat with your robot. You need a 'black' and a'white' value. Calculate the average and put it into the P controllerprogram in theoffset variable.Almost allline followers require that you (or code written by you and executed bythe robot) do thisstep.The Kpvalue and the target power Tpare the tunable parameters.

A tunable parameter has to be determinedby trial and error. Kpcontrols howfast the controller will try to get back to the line edge when it hasdrifted away from it. Tpcontrols how fast the robot is moving along the line.If the line is pretty straight you can use a large Tp to get the robotrunning at highspeed and a small Kp sotheturns (corrections) are gentle.If the line hascurves, especially sharp ones, there will be a maximum Tp value thatwill work. If Tpis biggerthan that maximum it won't matter whatKp is, therobot will loose theline when it encounters acurve because it is moving too fast. If Tpis really small then almostany Kpvalue will work sincethe robot will be moving very slowly.Thegoal is to get the robot moving as fast as possible while stillbeing able to follow the line of interest.We had guesstimated astarting value for Kpof 10.For Tp youmight start at evenlower thansuggested above, perhaps 15(the robot will be moving pretty slow). Try it and see how it works.

Ifyou loosethe line because the robot seems to turn sluggishly then increase Kp bya coupleand try again. If you loose the line because the robot seemshyperactive in hunting back and forth for the line then decrease Kp. Ifthe robot seems to follow the linepretty well then increase Tpand see if you can followthe line at the faster speed. For each new Tp you will need todetermine a new Kp,though Kpusually won't change too much.Following a straight line is usually pretty easy. Following alinewith gentle curves is a bit harder.

Following a line with sharp curvesis the hardest. If the robot is moving slow enough then almost any linecan be followed, even with a very basic controller.

We want toget good line following, good speed and the ability to handle gentlecorners. (Lines with sharp corners usually take more specialized linefollowers or very low robot speeds.)It is likely that the best P controller will bedifferent for each kind of line (line width, sharpness of curves etc.)and for different robots. In otherwords, a P controller (or aPID controller for that matter) is tuned for a particular kind of lineand robotand will notnecessarily work well for other lines or robots. The code will work formany robots (and many tasks) but the parameters, Kp, Tpand offset,have to betuned for each robot and each application.Doing math on a computer that doesn't know what adecimal point is causes some problemsNOTE: This work wasdone usingNXT-G version 1.1 which only supports integers. NXT-G version 2supports floating point numbers so the following may not be needed ifyou have version 2 or later.In the process of tuning the P controller you will be tweakingthe Kpvalue up and down.

Theexpected range of values that Kpmight bedepends on exactly what the P controller is doing. How big is the inputrange and how big is the output range? For our line follower Pcontroller theinput range is about 5 light units, and the output range is 100 powerunits, so it seems likely that Kpwill be in the vicinity of 100/5=20. Insome cases the expected Kpwon't be that big. What happens if theexpected Kpis one?

Sincevariables in NXT-G are limited tointegers, when you try to tune the Kpvalue all you can tryis.-2, -1, 0, 1, 2, 3. You can't enter 1.3 so you can't try Kp=1.3.

You can'tuseany number with a decimal point! But there will probably be alarge difference in the robot behavior when you change the Kp by thesmallest possible change of 1to 2. With Kp=2therobot tries twice as hard to correct theerror compared to Kp=1.Themotor power level changes twice as much forthe same change in the light levels. We really would like to have finercontrol of Kp.It is pretty easy to fix this problem. All we will do ismultiplythe Kp by apower of ten toincrease the useable range within the integerrestriction.

If it isexpected that Kpmight be near1 then a value of 100 as themultiplier would be a good bet. Indeed, it is probably best to just goahead and always use 100. Kpasthe number you actually enter into theprogram. Once Kphas beenmultiplied by 100 we can now enter what wouldhave been 1.3 as 130. 130 has no decimal point so NXT-G is happy withthenumber.But doesn't that trash the calculation? Yes it does but it iseasy to fix.

Once we have calculated the P term we will divide by 100to remove our multiplier. Remember our equation that defines the Pcontroller from earlier;Turn= Kp.( error)We will multiply Kpby100, which means our calculated Turnis100 times bigger than it should be. Before we use Turn we must divideit by 100 tofixthat.So, our new and improved pseudo code for a line following Pcontroller is:Kp = 1000! REMEMBER we are using Kp.100 so this is really 10!offset = 45!

Initialize the other two variablesTp = 50Loop foreverLightValue = read light sensor! What is the current light reading?error = LightValue - offset! Calculate the error by subtracting the offsetTurn = Kp. error! The 'P term', how much we want to change the motors' powerTurn = Turn/100!

REMEMBER to undo the affect of the factor of 100 in Kp!powerA = Tp + Turn! The power level for the A motorpowerC = Tp - Turn! The power level for the C motorMOTOR A direction=forward power=powerA! Actually issue the command in a MOTOR blockMOTOR C direction=forward power=powerC! Same for the other motor but using the other power levelend loop forever!

Done with loop, go back and do it again. Wait, what was the 'Subtle Problem' you mentioned with thefirst version of the P controller?There are always subtle problems. Sometime they matter andsometimesthey don't.;)In this case, one problem is that when we calculate the motorpowerlevel (e.g., powerC=Tp-Turn) it is possible to get a negative numberfor the power.We want a negative number to meanthat the motor should reverse direction. But the data port on a NXT-GMOTORblock doesn't understand that.

The power level is always a numberbetween 0 and +100. The motor's direction is controlled by adifferent input port.

To get the motor to react correctly when thepoweris negative you'll need to handle it in the program. Here is one way todo that;If powerA 0 then! Positive motor power is no problemMOTOR A direction= forward power=powerAelsepowerA = powerA.

(-1)! Negative motor power needs to be made intoMOTOR A direction= reverse power=powerA!

A positive number and the motor directionend If! Needs to be reversed on the control panelThe MOTOR block receives the power (powerA for the A motor)via adata wire. The direction is set with the check boxes in the motor'sparameter window.You will need a similar chunk of code for the C motor. Nowwhen thecalculated power goes negative the motors will be properly controlled.One thing this does is allow the P controller to go all the way toa 'zero turning radius turn' and the robot can spin in place if needed.Of course, that may not actually help.There are a couple other things that might be subtle problems.Whathappens when you send a power level that is greater than 100 to themotor? It turns out that the motor just treats the numberas 100. That is good for the program but not the best thing tohave happen in a P (or PID) controller.

You would really prefer thatthe controller never tries to ask the motors to do something theycan't. If the requested power isn't too far above 100 (or below -100)then you are probably OK. If the requested power is a lot biggerthan 100 (or a lot less than -100) then it often means the controlleris spiraling out ofcontrol.

So, make sure you have a fire extinguisher handy!P Controller SummaryHopefully you've picked up enough to understand a P(proportional)controller. It is pretty simple. Use a sensor to measure something thatyou are trying to control. Convert that measurement to an error. For the linefollower we didthat by subtracting the average of black and white light values. Multiply the error by a scalingfactor called Kp.The result is a correction forthe system.

In our line follower example the correction is applied asan increase/decrease in the power level of the motors. The scalingfactor Kp isdetermined usinga bit of educated guessing and then fine tuned by trial and error.P controllers can handle a surprising wide range of controlproblems, not just following a line with a Lego robot. In general, Pcontrollers work very well when a few conditions are met. The sensor needs to have wide dynamic range (whichunfortunatelyis not true for our line following robot).

The thing being controlled (motors in our case) should alsohave a wide dynamic range, that is they should have a wide range of'power' levels with individual 'power' levels that are close together(the NXT motors are pretty good in this respect). Both the sensor and the thing being controlled must respondquickly.

'Quick' in this case is 'much faster than anything else thatis happening in the system'. Often when you are controlling motors itisn't possible to get 'quick' response since motors take time to reactto a change in power. It can take a few tenths of a second for Legomotors to react to a change in power levels. That means the robot'sactions are lagging behind the P controller's commands. That makesaccurate control difficult with a P controller.Adding 'I' To The Controller: The PI Controller('I': whathave you done for me lately?)To improve the response of our P controller we will add a newtermto the equation. This term is called the integral, the 'I' inPID.

Integralsare a very important part of advanced mathematics, fortunately the partwe need is pretty straight forward.The integral is the running sum of the error.Yep, it's that simple. There are a few subtle issues we'llskip forthe moment.Each time we read the light sensor and calculate an error we will addthat error toa variable we will call integral(clever eh?). Integral = integral + errorThat equation might look a little odd, and it is. It isn'twrittenas a mathematical statement, it is written in a common form used inprogramming to add up a series of values. Mathematically it doesn'tmake any sense. In computer programming the equals sign has a somewhatdifferent meaning than in math. (I'll use the same typewriter font Iused for the pseudo code examples to highlight that it is a programmingform and not a proper mathematical form.) The '=' means do the math onthe right and save the result in the variable named on the left.

Wewant the computer to get the old value of integral, add the error to it thensave the resultback in integral.Next, justlike the P term, we will multiply the integralby a proportionalityconstant, that's another K. Since this proportionality constant goeswith theintegral term we will call it Ki.Just like the proportional term we multiply the integralby the constant ( Ki)toget a correction. For our line following robot it is an addition to ourTurn variable.Turn=Kp.( error)+ Ki.( integral)The above is the basic equation for a PI controller. Turn is ourcorrection for themotors.

The proportional term is Kp.( error) and theintegral term is Ki.( integral).What exactly does the integral term do for us? If the error keeps thesame sign forseveral loops the integralgrowsbigger and bigger.

For example, if we check the light sensor andcalculate that the erroris1, then a short time later we check the sensor again and the error is 2, thenthe next time the error is2 again, then the integralwill be 1+2+2=5. The integralis 5 but the errorat this particular step isonly 2. The integral can be a large factor in the correction butit usually takes a while for the integralto build up to the point where it starts to contribute.Another thing that the integral does is it helps remove smallerrors.If in our line follower the light sensor is pretty close to the line'sedge, but not exactly on it, then the errorwill be small and it will only take a small correction tofix.You might be able to fix that small errorby changing Kpin theproportional term but that will often lead to a robot that oscillates(wobbles back and forth).The integral termis perfectfor fixing small errors. Since the integraladds up the errors,several consecutive small errors eventually makesthe integral bigenough to make adifference.One way to think about the integralterm is that it is the controller's 'memory'. The integral is thecumulative historyof the errorand gives thecontroller a method to fix errors that persist for a long time.Some subtle issues with the integralYep, the integral has more detail. Fortunately they aren't toopainful.I glossed over a minor issue (OK, it really isn't minor but wearegoing to make it so), the time. The integral is really the sum of the error.(delta time).Delta time ( dT)is the time between the lasttime we checked the sensor and the time of the most recent check of thesensor;integral = integral + error.(dT)Soevery timewe add to integral thethingwe should add is the errortimesthe dT.

Itis fairly easy tohave the robot measure the dT.We would just read a timer each time we read the light sensor. If wesubtract the last time from the current time we get the time since thelast reading dT.(There arebetter ways to do this but I'll skip'm since they are not needed.) Butwouldn't it be nice if didn't have to measure the dT and do themultiplication? Well,what if the dTis always thesame?

Every time we add to integralwehave that same dT term.So wecan take that factor of dToutof error.( dT) and just do thesumming the waywe did before; integral = integral + errorOnly when we want to do another calculation with integral do weactually need tomultiply by dT.But waitthere's more.We can do even more to hide the time term. The integral termin the PIcontroller equation is Ki.( integral). dT. ButKi is aparameter that we haveto fine tune (just like Kp)sowhy not just replace the Ki. dT part with a new Ki?

The new Ki is different fromthe originalbut since we don't know either one it doesn't really matter which onewe use or what we call it. No matter what we call it or whatitrepresents we still have to find the correct value largely by trial anderror.So we have completely removed the time element for theintegralterm with the restriction that all the times steps, dTs, are the same(or about thesame).The integral has a memory like an elephantOne last detail should be mentioned about the integral. Usuallythe integral canonly be moved towardszero, where it doesn't contribute anything to the controller, by havingerror valuesadded that arethe opposite sign of most of the ones that we have already collected inintegral.For example, ifover several cycles through the loop the errors are1,2,2,3,2,1 that adds upto an integral of11. But theerror atthe last data pointis only 1, which is much smaller than the integral at thatpoint. The only wayfor the integral tomovetowards zero is to get a string of negative errors to counterbalance theearlier string of positive errorsto 'wind down' the integral.For example, if the next few errors are -2,-2,-3 then the integral willdrop from 11 to 4 and we would still need more negative errors to get the integral down tozero.

In addition, the integralwantsthe total error tobe evenly distributedbetween positive and negative errors.If something happens that pushes our line following robot totheleft of the line's edge the integraltermnot only wants to get back to the line's edge it also wants toovershoot the edge to the right by as much as the original disturbancewas the left. So the integral tends to 'wind-up' if there are largeerrors that persist for a while. This can cause problems withcontrollers that include an integralterm.Sometimes this tendency of the integralterm to want to overshoot when it tries to correct the error is a bigenough problem thatthe programmer must do something to the integralterm so it won't cause problems. If integral wind-up isa problem twocommon solutions are (1) zero the integral,that is set the variable integralequalto zero, every time the erroriszero or the error changessign. (2) 'Dampen' the integral by multiplying theaccumulated integral bya factor less than onewhen a new integral iscalculated. For example;integral = (2/3).integral + errorThis reduces the previous integral value by 1/3 each timethroughthe loop. If you think of the integral term as the controllers 'memory'then this damping is forcing it to become forgetful of thingsthat happened a 'long' time ago.Pseudo code for the PI controllerTo add the integral term to the controller we need to add anewvariable for Kiand one forthe integral itself.Anddon't forget that we are multiplying our Ks by 100 to help with theinteger math restrictions.Kp = 1000!

REMEMBER we are using Kp.100 so this is really 10!Ki = 100! REMEMBER we are using Ki.100 so this is really 1!offset = 45! Initialize the variablesTp = 50integral = 0! The place where we will store our integralLoop foreverLightValue = read light sensor!

What is the current light reading?error = LightValue - offset! Calculate the error by subtracting the offsetintegral = integral + error!

Our new integral termTurn = Kp.error + Ki.integral! The 'P term' and the 'I term'Turn = Turn/100! REMEMBER to undo the affect of the factor of 100 in Kp!powerA = Tp + Turn! The power level for the A motorpowerC = Tp - Turn! The power level for the C motorMOTOR A direction=forward power=powerA!

Actually issue the command in a MOTOR blockMOTOR C direction=forward power=powerC! Actually issue the command in a MOTOR blockend loop forever! Done with this loop, go back to the beginning and do it again.

Adding 'D' To The Controller: The Full PID Controller('D':whatis going to happen next?)Our controller now contains a proportional (P) term that triestocorrect the current error andan integral (I)term that tries tocorrect past errors, isthere a way for thecontroller to look ahead in time and perhaps try to correct error that hasn'teven occurred yet?Yes, and the solution is another concept fromadvancedmathematics called the derivative.Ahhh, there's the 'D' in PID. Like the integral,the derivative canrepresent some pretty serious mathematics.

Fortunately for us, what weneed for the PID is fairly simple.We can look into the future byassuming that the next changein the error isthe same asthe last changeinthe error.That means the nexterroris expected to be thecurrent error plusthe changein the error betweenthe two precedingsensor samples. The change in the errorbetween twoconsecutivepoints is called the derivative.Thederivative is the same as theslope of a line.That might sound a bit complex to calculatebut it reallyisn'ttoo bad. A sample set of data will help illustrate how it works. Letsassume that the current erroris 2 and the error beforethat was 5. What would we predict the next error to be? Well,thechange in error is the derivativewhichis;(the currenterror )-(the previous error )which for our numbers is 2 - 5 = -3. The current derivative thereforeis -3.

To usethe derivative topredict thenext error wewoulduse(next error) = (the current error)+ ( the current derivative)which for our numbers is 2 + (-3) = -1. So wepredict the next error willbe -1. Inpractice we don't actually go all the way and predict the next error. Instead wejust use the derivativedirectly in the controllerequation.The D term, like the I term, should actually includeatimeelement, and the 'official' D term is;Kd ( derivative )/( dT )Just as with the proportionaland integralterms we have to multiplyby a constant. Since this is the constant that goes with the derivative it iscalled Kd.Notice also that for thederivative term we divide by dTwhereasin the integral termwe hadmultiplied by dT.Don'tworry too much about why that is since we are going to dothe samekinds of tricks to get rid of the dTfrom the derivative termaswe did for the integral term.The fraction Kd/dTis aconstant if dTis the same forevery loop. So we can replace Kd/dTwith another Kd.Since this K, like the previous Ks, is unknown and has to be determinedby trial and error it doesn't matter if it is Kd/dT or just a newvalue for Kd.We can now write the complete equation for aPID controller:Turn= Kp.( error)+ Ki.( integral) + Kd.( derivative)It is pretty obvious that 'predicting the future' would be ahandything to be able to do but how exactly does it help? And how accurateis the prediction?If the current erroris worsethan the previous error thenthe D term tries tocorrect the error.If hecurrent error isbetterthan the previous error thenthe D term tries to stopthe controller from correcting the error.It is the second case that is particularly useful.

If the error is gettingclose to zero thenwe are approaching the point where we want to stop correcting. Sincethe system probably takes a while to respond to changes in the motors'power we want to start reducing the motor power before the error has actuallygone to zero,otherwise we will overshoot. When put that way it might seem that theequation for the D term would have to be more complex than it is, butit isn't.

The only thing you have to worry about is doing thesubtraction in the correct order. The correct order for this type ofthing is 'current' minus 'previous'. So to calculate the derivative we takethe current error andsubtract the previous error. Pseudo code for the PID controllerTo add the derivative term to the controller we need to add anewvariable for Kd andavariable to remember the last error.And don't forget that we are multiplying our Ks by 100 to help with theinteger math.Kp = 1000! REMEMBER we are using Kp.100 so this is really 10!Ki = 100!

REMEMBER we are using Ki.100 so this is really 1!Kd = 10000! REMEMBER we are using Kd.100 so this is really 100!offset = 45! Initialize the variablesTp = 50integral = 0! The place where we will store our integrallastError = 0!

The place where we will store the last error valuederivative = 0! The place where we will store the derivativeLoop foreverLightValue = read light sensor! What is the current light reading?error = LightValue - offset! Calculate the error by subtracting the offsetintegral = integral + error! Calculate the integralderivative = error - lastError! Calculate the derivativeTurn = Kp.error + Ki.integral + Kd.derivative! The 'P term' the 'I term' and the 'D term'Turn = Turn/100!

REMEMBER to undo the affect of the factor of 100 in Kp, Ki and Kd!powerA = Tp + Turn! The power level for the A motorpowerC = Tp - Turn! The power level for the C motorMOTOR A direction=forward power=PowerA! Actually issue the command in a MOTOR blockMOTOR C direction=forward power=PowerC! Same for the other motor but using the other power levellastError = error!

Save the current error so it can be the lastError next time aroundend loop forever! Done with loop, go back and do it again.We now have the pseudo code for our complete PID controllerfor aline following robot. Now comes what is often the tricky part, 'tuning'the PID. Tuning is the process of finding the best, or at least OK,values for Kp,Ki and Kd.

Tuning A PID Controller Without Complex Math(but we stillhave todo somemath)Very smart people have already figured out how to tune a PIDcontroller. Since I'm not nearly as smart as they are, I'll use whatthey learned. It turns out that measurement of couple ofparameters for the system allows you to calculate 'pretty good' valuesfor Kp, Ki and Kd. It doesn'tmatter much what theexact system is that is being controlled the tuning equations almost always workpretty well.There are several techniques to calculate the Ks, one of is called the' Ziegler–Nichols Method',which is what we will use.A googlesearch will locate many web pages that describe this technique in allit's gory detail. The version that I'll use is almost straightfrom the (the same treatment is found in manyotherplaces).

I'll just make one small change by including the loop time ( dT) in thecalculations shown in thetable below.To tune your PID controller you follow these steps:. Set the Kiand Kdvalues to zero, which turns thoseterms off and makes the controller act like a simple P controller. Set the Tpterm to a smallish one. For our motors 25 might be a good place tostart.

Set the Kpterm to a 'reasonable' value. What is 'reasonable'?.

I just take the maximum value we want to send to themotor'spower control (100) and divide by the maximum useable error value. Forour line following robot we've assumed the maximum error is 5 so ourguess at Kpis 100/5=20. Whenthe error is +5 the motor's power will swing by 100 units. When theerror is zero the motor's power will sit at the Tp level. Or, just set Kpto 1(or 100) and see what happens.

If you have implemented that the K's are all entered as100times their actual value you have to take that into account here. 1 isentered as 100, 20 as 2000, 100 as 10000.

Run the robot and watch what it does. If it can't followthe lineand wanders off then increaseKp.If it oscillates wildly then decrease Kp.Keep changing the Kpvalueuntil you find one that follows the line and gives noticeableoscillation but not really wild ones. We will call this Kp value 'Kc'('critical gain'in the PID literature).

Using the Kc value as Kp,run the robot along the line and try to determine how fast it isoscillating. This can be tricky but fortunately the measurement doesn'thave to be all that accurate. The oscillation period ( Pc) is how long ittakes the robot to swing from one side of the line to the other thenback to the side where it started. For typical Lego robots Pc willprobably be in the range of about 0.5 seconds to a second or two.

You also need to know how fast the robot cycles throughit'scontrol loop. I just set the loop to a fixed number of steps (like10,000) and time how long the robot takes to finish (or have the robotdo the timing and display the result.) The time per loop ( dT) is the measuredtime divided bythe number of loops. For a full PID controller, written in NXT-G,without any added buzzes or whistles, the dT will be in therange of 0.015 to0.020 seconds per loop. Use the table below to calculate a set of Kp, Ki,and Kcvalues.

If you justwant a P controller then use the line in the table marked P tocalculate the 'correct' Kp( Ki' and Kd' are both zero).If you want a PIcontroller then use the next line. The full PID controller is thebottom line. If you have implemented that the K's are all entered as 100timestheir actual value you don'thave to take that into account in these calculations. Thatfactor of 100 is already take into account in the Kp = Kc value youdetermined.

Run the robot and see how it behaves. Tweak the Kp,Ki and Kd values to get thebest performanceyou can.