
Using a PIDbased Technique
For Competitive Odometry and DeadReckoning
By G.W. Lucas  About The Robot Differential Steering and PID Equations for Steering Tuning the PID Parameters Approximation Issues Conclusion 
Introduction When the Connecticut Robotics Society (CRS) ran its first annual odometry and deadreckoning event this June, the winning entry completed the 24.7foot (7.5 m) circuit and terminated its run within 2 inches (5 cm) of its starting position. Although Peeves, a Lego Mindstorms™ based robot, came perilously close to the inner wall on the third leg of the course, it managed to recover and navigate four 90degree turns and 20 feet of corridors to reach a firstplace finish. 
In the CRS odometry competition, the winning robot is the one
that finishes closest to its starting position after completing one full lap of the
course. To some observers, the fact that a Legobased platform could achieve such a high
level of navigational accuracy was a surprise. Although easy to assemble, Legobased
vehicles are often not capabable of highprecision maneuvers. How was Peeves able to
navigate the course so accurately? Two factors contributed to its success. First, the robot took advantage of the Lego Mindstorms rotation sensors (encoders) which allowed it to measure wheel position with a 1.8degree resolution (1.3mm linear displacement). These devices were coupled to the motors and controller in a closedloop system that allowed the robot to monitor its performance at all times. 
Figure 1. The Connecticut Robotics Society DeadReckoning Course. Competing robots complete one circuit of the course (shown in gray). The wining robot is the one that finishes closest to its starting position. To do so, it must navigate four 5foot corridors and four 1.18foot curves – altogether, a 24.7 foot (7.5m) circuit. 
Second, the robot was able to rely on the accuracy of its deadreckoning
calculations because of a machinecontrol algorithm that held it to a straightline path
when it was traversing corridors. The algorithm, which is known as PID (for Proportional,
Integral, and Derivative control), was the key to the system’s performance. By
holding the robot on a straightline course, the PIDbased control software ensured that
the robot's position could be computed accurately with simple calculations. Robotics
practitioners often cite software as the lynchpin of a viable implementation. That claim
was never more true than it was for CRS odometry competition. In this article, I will discuss the deadreckoning techniques used for the competition, and provide details about its implementation in the Lego Mindstorms system. Completing the contest course required two kinds of maneuvers: straightline traversals and 90degree turns. I will provide background on Peeves' approach to both. My primary focus will be on the PID algorithm and how it was used to keep the robot on track when following a straightline path. I will also discuss how the PID control logic was used to absorb orientation errors when the robot executes "not exactly 90degree" turns. 
Building the CRS
Odometry Course The Connecticut Robotics Society odometry course was constructed from the cheapest material I could find. Wood is so expensive these days that I chose PVC pipe. The price difference between 1/2 and 3/4 inch pipe was so small that I went with the heavier 3/4 inch. I'm glad that I did because Max – a fifty pound robot that has been known to knock holes in drywall – crashed squarely into the pipe during a practice run the month before the contest. — John Gallichotte 
The Peeves Project was my first experience with PID techniques and, although I am scarcely a master of the material, I was impressed with how well it worked for my robot. This article is written in the hope that the techniques that benefited one robot can also benefit others. If you wish to adapt some of these ideas to your own projects, there should be enough information in this article to get you started (web links and book references are included). I also hope that true subjectmatter experts will join in followup discussion on the robotics newsgroups, thus helping to make machinecontrol algorithms accessible to a wider group of robot implementers. 
A hypertext version of the Peeves code accompanies this article (see peeves.html). If you are interested, you may also download the
latest version of the source code that was used for the deadreckoning event at The Rossum Project, a web site dedicated to the
promotion of free opensource software for robotics. 
About The Robot: Legos,
Language, and Differential Steering Figure 2 shows the basic layout of the Peeves platform. The robot was built from the stock Lego Mindstorms kit with the addition of 2 rotation sensors that were purchased separately. The brain of the Mindstorm system is the RCX "brick." The RCX brick is a Legointerlocking unit that contains batteries, microcontroller unit, LED display, and an IR communications link. The RCX is built on a Hitachi H8/3272 processor with 32 K of external RAM. It features three PulseWidth Modulation (PWM) outputs that can be used to drive 9volt DC motors or other devices. Output can be adjusted to 8 levels by increments of 1/8^{th}, in forward or reverse. The drivers can also be set to brake or freeturning states. The RCX also provides 3 sensor ports based on 10bit A/D converters. The Mindstorms kit comes with two highefficiency 9volt DC gearmotors that feature an internal fly wheel and reduction gearing. Peak, unloaded motor speed is 350 RPM when running on fresh batteries. 
Figure 2. Peeves, a Lego Mindstorms based robot. The beam members, visible to the front of the robot are used to pin the construction together. The two large wheels are powered, the smaller, passive wheel to the robot's back is provides support. Because the small wheel is always tangent to the robot's direction of travel, it also serves as a telltale, revealing small changes in orientation during test runs. 
The two large wheels to the front of Peeves are individually powered and
controlled by the standard Lego gearmotors in the classic Differential Steering
configuration. Basically, differential steering is the same system that is used for wheel
chairs. Two independently controlled wheels provide both drive and steering. The rear
wheel is a purely passive support. In Figure 2, the motors are the gray blocks positioned
to the front of the robot, beside each wheel. The blue boxes to the rear of the motors are
encoders (Lego calls them "rotation sensors") which measure rotation with an
accuracy of 22.5 degrees (16 intervals per revolution). This resolution value is improved
through reduction gearing (see below). One of the most appealing features of the Lego Mindstorms system is the speed with which a robot can be constructed. When the CRS contest was announced early in April, the Mindstorms kit was still in shrinkwrap. Two months later, a completed robot was running the course. That convenience does come with certain constraints. Design options are limited by what can be accomplished using Lego parts (though Lego enthusiasts like to say, "you are limited only by your imagination"). And while individual Lego components are marvels of massproduced precision, the overall assembly of a Mindstorms system is sometimes less perfect than a robot builder would prefer. This is particularly true with regard to the wheel and drive systems. Peeves is heavy enough that the flex of its plastic axles give the wheels a significant (and asymmetric) inward tilt. When possible, the effects of these limitations were reduced through appropriate mechanical design. 
I programmed Peeves using Dave Baum's NQC (for "Not
Quite C") programming language. NQC is not part of the stock Lego package.
Ordinarily, the RCX microcontroller unit is programmed using Lego's graphical programming
environment to generate "RCX byte code." The byte code is downloaded to the RCX
via its IR link and then is run by the RCX firmware, which implements a bytecode
interpreter. Of course, there are significant limits to how sophisticated a program you can write in a graphical programming environment. The stock Lego utilities were inadequate to the task of coding a deadreckoning robot, so a different solution was necessary. Dave Baum's NQC is one of a number of alternate programming tools available as opensource software for the RCX. NQC compiles to the bytecode and runs on top of the RCX firmware. Other solutions (such as legOS and lejos) actually replace the firmware providing more efficient and fully featured implementations. I chose NQC because it appeared to be the simplest, most easily mastered solution given the short turnaround time for my project. However, NQC does have to operate within the limitations imposed by the RCX firmware. For example, the firmware supports a maximum of 32 variables, does not support arrays or floatingpoint data types, and is limited to twobyte signed integers. Some of these limitations are lifted if you install the new 2.0 release of the RCX firmware, now in beta release. 
RCX Firmware The RCX firmware implements an interpreter that executes instructions given in the binary formatted RCX byte code. The firmware supports multitasking, timers, sensors, motor control, a datalog, etc. Lego's RCX Code Lego's standard graphicsbased programming environment. Multitasking, events and conditional branching based on sensors and timers, but no variables or arithmetic. A wonderful introduction to programming for a child, but not practical for any but the simplest applications. NQC (Not Quite C) Using a simple Clike syntax, Dave Baum's NQC compiles to byte code and runs on top of the RCX firmware. Thus, it can share the system with programs written in RCX code. Important limitations are imposed by its dependency on the firmware. Available free of charge at http://bricxcc.sourceforge.net/nqc/. Source code available. Installation is painless. legOS A complete C/C++ implementation with full access to the 16K of memory and support for 4byte floating point numbers. Replaces the RCX firmware. Probably the best choice in terms of efficient use of processor, smallest image, best use of memory. Available free of charge at http://legos.sourceforge.net. Requires a separate installation of the GNU gcc compiler. If you have never installed gcc, expect to spend a week of evenings doing so. lejos, TinyVM Two Java Virtual Machines (JVM's) for the RCX. Both replace the RCX firmware. Nearly full featured implementations of the language (restricted to 4byte integer and float data types). TinyVM is quite small (under 10K). Its successor, lejos is larger, but provides a Math class with sine, cosine, exponential and log functions. Available free of charge at http://lejos.sourceforge.net. Requires separate installation of a Java compiler. Other Alternatives There is a surprisingly large number of alternative programming options for the RCX. Luis Villa compiled a list at http://linuxdoc.org/HOWTO/mini/Lego/ 
Readers who are unfamiliar with Lego's more technical products may wonder
how a robot built from Lego blocks can hold together. To permit the construction of a
relatively solid unit, Lego provides a series of "beam" components that
interlock with its standard blocks. The sides of these beams are molded with holes that
permit them to be pegged together with small plastic pins. Thus secured, the system holds
snugly together against moderate vibration and mechanical stress... a feature that was
very important in the deadreckoning event. Any wobbling or distortion of the robot's
frame while in motion degrades the wheel alignment introducing both random and systematic
error to the navigation problem. To prevent that, Peeves makes liberal use of Lego's pin
and beam construction elements. To provide further stability, the robot's heaviest
component, the RCX brick (which holds 6 AA batteries) is positioned as low as possible
within the constraints of the overall design. The center of gravity is slightly behind the
main wheels so that the tail wheel is lightly loaded, but not so much that the system tips
forward when it stops. Figure 3 shows the gear train used in Peeves. The configuration is a compromise between the geometry imposed by the Lego components and the need to obtain as fine a resolution from the wheel encoders as possible. The more accurately the robot can measure wheel rotation, the more accurately it can deadreckon. Each motor is coupled to an encoder through a 1:1 gear train that ensures they are synchronized. For the Peeves configuration, peak motor speed is about 290 RPM (unloaded, they peak at about 350 RPM). The rotation sensors are rated for a maximum speed of 500 RPM. The gear train provides a 12.5 to 1 reduction ratio from motor to wheels. Thus, for each rotation of the wheel, the encoder rotates 12.5 times allowing the system to measure 12.5x16=200 samples per revolution. Although I experimented with gearing that provided a higher resolution (with less backlash), it exceeded the 500 RPM sample rate supported by the sensors and had to be abandoned. The placement of the large, 40tooth gears in the robot's gear train requires that Peeves use the larger wheels provided in the Mindstorms kit. The Lego wheels are rather wide and soft for a deadreckoning robot (harder, narrow wheels provide better accuracy), but provide good traction. Their 8.16 cm diameter tires means that each "click" of the encoders covered a distance of 8.16x3.1415/200=0.128 cm. This value, the click, is used extensively in the Peeves software. 
Figure 3. The robot's gear train provides a 12.5:1 reduction ratio using 8, 16, and 40tooth gears. The gray box to the left of the unit is a Lego gearmotor, the blue module to the right is a Lego rotation sensor. The motor and sensor are coupled in a 1:1 ratio. Also note the use of a twopin retainer to locks two Lego beams in place. 
Reduction ratio of gearing  12.5:1 
Diameter and width of tires  8.16 x 1.5 cm (3.2 x 0.6 in.) 
Circumference of tires  25.6 cm (10.1 in.) 
Encoder "clicks" per wheel revolution  200 
Distance per click  0.128 cm (0.05 in) 
Clicks per unit distance (handy when writing navigation code)  7.8 click/cm (19.8 click/in.) 
Distance between wheels, centertocenter, (this value is the parameter b used in the equations of motion below)  19.75 cm, 7.7 in, 154 clicks 
Difference of right and left encoder values when making a 90degree turn  242 
Maximum motor speed  270 RPM 
Maximum forward robot speed  9.2 cm/sec (3.6 in/sec, 72 clicks/sec) 
Mass (weight)  638 grams (1.4 lb.) 
Table 1. Vital statistics. 
Differential Steering and
PID The two gearmotors that came with the Mindstorms kit that was used to build Peeves are not perfectly matched. Due to variations in manufacture, one runs about 5 percent slower than the other. An imbalance in motor output is important in a differential steering system because even small variations in wheel speed can result in the robot straying from a straightline path. The default tendency for Peeves was to curve to its left. 
Unfortunately, a differentially steered vehicle is not an inherently stable system. Left to its own devices, it will not travel in a straight line, or even along a simple curve. But whenever an author says that a differentially steered vehicle is not an inherently stable system, it is useful to point out that neither is a bicycle. Yet, somehow, a bicycle manages to operate just fine. Despite its lack of inherent stability, a bike has no trouble getting around. It can do so, of course, because it is coupled with an active controller unit... in this case, the rider.  The Bicycle Metaphor The bicycle metaphor is borrowed from John Lienhard's The Engines of Our Ingenuity broadcasts on National Public Radio (episode 106). You may read Dr. Lienhard's commentaries on the history of science and technology at http://www.uh.edu/engines/ If you have a chance, be sure to listen to some of his audio's. They are a real treat. 

A robot also has an active controller unit. And, given the right software,
it can guide a differentialsteering system with smooth precision. In the Peeves
implementation, the basis of that software is a technique known as PID. The acronym PID is short for Proportional, Integral, and Derivative. In the PID approach, a controller monitors the error in the system (its deviation from some desired value, or set point) and makes corrections based on three criteria. The Proportional response is based on the magnitude of the observed error, the Integral of that error (error accumulated over time), and the Derivative of the error (the rate at which the error changes over time). The PID approach was the basis of the system used to control Peeves and keep it following a straight line. To illustrate the idea behind the PID technique, let's apply it to the familiar problem of steering an automobile on highway. We desire to keep the car in the center of its lane. If the car begins to drift to the right, we apply a correction by turning the steering wheel to the left to stop the movement away from the centerline. In this case, the set point is simply for the car to travel in a straight line. We define the speed of the car's lateral movement as our observed rate of error. The faster it moves away from the centerline, the higher the error. Naturally, a competent driver does not overcorrect. So, if the error is small, our response is small. If the error is large, our response is large. In other words, our initial correction is Proportional to the error rate (giving us the ‘P’ in PID). If we turn the wheel enough, the car will stop moving to the right and drive in a straight line. Although the car will have drifted from the center of the lane, we will have achieved an error rate of zero because we define the error in terms of the lateral speed, not distance offcenter. Still, we do care about the car not being in the center of its lane. So we define a second kind of error, the accumulated error. In mathematical terms, this value is simply the Integral of the error over time (giving us the ‘I’ in PID). And, referring back to our driving example, we realize that the correction applied to the steering is often proportional to values: the instantaneous drift rate, and the integrated error. If all goes well, the car will reverse its rightward drift and return to the center of its lane fairly quickly. Perhaps a little too quickly... If we do not back off from the correction as we approach the center of the lane (zero accumulated error), the car will overshoot the centerline and begin to drift to the left. If that happens, we need to make a second, counteradjustment so that the car returns to the right. But if we overcorrect, we may need another counteradjustment to the left. If this continues, the system will oscillate. To damp that oscillation, the PID technique adds a third factor based on the rateofchange for the error function, that is its Derivative (giving us the ‘D’ in PID). The car analogy illustrates the three basic components of the PID technique. In general, we consider the instantaneous and integrated (accumulated) error and make a proportional adjustment. Although this adjustment is often enough, it sometimes leads to a system that tends to overcorrect and oscillate as it returns back to the set point. In such cases, we add the third correction to help settle the system. For purposes of this discussion, we will let e(t) represent a general error function and use the following variation of the PID equation: 
[1]  
where  
is the correction factor to be applied
to the system; is the adjustment coefficient for the observed error; is the adjustment coefficient for the integrated error; is the adjustment coefficient for the derivative of the error. 
This is a nonstandard treatment of the PID equation. Most texts place a
negative sign behind the derivative term to reflect its role as a damping factor. I use
the plus sign because that is how I ultimately coded the term in the Peeves logic. Because
of the way I defined my error functions, the value of derivative term is often the
opposite sign of the error term (see below). Note also, that some implementations omit the
derivative term. Such an approach is suited to cases where the derivative has a small
magnitude or where a significant lag in sampling and processing measurements makes it
impossible to compute a derivative in a timely manner. In such cases, authors often refer
to a PI solution rather than a PID solution. There are several web sites describing the PID technique. A good place to start can be found at the Control Engineering Tutorials web site or at the joint CarnegieMellon/University of Michigan Control Tutorials for Matlab pages. Many books on control theory also discuss the PID technique. At least one of these is available on line: Lee, Newell, and Cameron's Process Management and Control (the discussion of PID tuning techniques in chapter 5 is especially useful). PID for Peeves: How one robot implements the algorithm One common use of the PID technique is in controlling motor speed. If I had taken this approach in the Peeves implementation, I could have used two sets of PID calculations to maintain a constant speed for each motor. Because the Lego rotation sensors are coupled directly to the motors, they form a closedloop system. Feedback from each sensor can be used as input into the PID calculation for the corresponding motor. Additional calculations can combine sensor inputs to track data such as orientation and position, coordinating the motors and adjusting their respective speeds as needed. Unfortunately, implementing such an approach turned out to be more complicated than I could accomplish given my short development time and the coding limitations of the RCX/NQC environment. Instead, I took a less ambitious approach. Rather than looking at the problem as a matter of controlling two motors, I looked at the robot as a whole. For the CRS contest, it didn't matter how fast the robot traveled. So controlling the absolute speed of each motor was not as important as controlling their speed relative to each other. The Peeves implementation focuses on the robot's path, and disregards its speed. The important thing is simply that the vehicle goes straight. 
The implementation uses a single PID calculation based on its
observed lateral drift rate to set PWM output levels (and thus, the relative speed) for the
pair of motors. Essentially, this implementation attempts to control a
secondary effect (the overall behavior of the robot), rather than a primary effect
(individual motor speed). It was a design choice that worked, but may not have been the
best way to get the job done. In the future, I hope to explore the twincontroller
implementation. Perhaps that will be the subject of another article. For now, let's look
at how Peeves implements its PID controller. Figure 4 shows the track of a robot on a Cartesian coordinate plane. If the robot is to follow a straightline course, it should move along the xaxis. As error accumulates, it tends to move off the axis. There were a couple of different ways that I could have defined the error functions for the PID equation. For example, I could have focussed on the robot's orientation. But, after a bit of trial and error, I settled on the definitions used in the automobile example used above. 
Figure 4. The straightline path is along the xaxis. The error function is defined to be the rates of change of the y coordinate over time (a straight line being zero error). The integrated error is treated as being the distance from the xaxis, and is sometimes referred to as the accumulated error or the lateral offset from center. 
Thus, the error function e(t) is given by
[2] 
The equivalent to the PID equation in [1] is derived by substituting the results from [2] and performing the appropriate integration and differentiation. 
[3] 
As I was coding, this approach got a bit confusing at times. Because my
error function, e(t), is actually a derivative of another function, y(t),
the positions of the operators change in two forms of the PID equations [1] and [3]. So
when I was referring to "the derivative term in the PID equation", I had to
remind myself that I meant the derivative of e(t), not y(t). As you read the
notes below, you may find it useful to refer to the following table.

Table 2. Relationships between error functions. 
Once I decided how I was going to define the error function for the PID equation, I needed to complete the following steps:
Equations for Differential Steering Using the PID technique requires that we have some way to compute the error function, its integral, and its derivative. How do we convert data collected by the encoders into a description of the robot's movements? To do so, we need to know something about the behavior of a differential steering system. Fortunately, I had already tackled the details for an earlier article I had written for the Rossum Project (see An Elementary Trajectory Model for the Differential Steering System). From this source, and others, I was able to obtain equations that described the orientation of the robot, and its ycoordinate as functions of wheel velocities and time. These equations made the simplifying assumption that the wheel velocities were held constant. This assumption seemed reasonable since the large reduction ratio of Peeves' gear train provided a high torque relative to the robot's mass. When motor power levels were changed, the wheels would spin "up to speed" quite quickly. So as long as the robot stayed close to its desired straightline course, the simplifying assumptions would not result in too much error. The Rossum Project web site provided an applet that allowed me to experiment with the consequences of different settings and test this assumption (see MotionApplet). The following equations were used in the Peeves project to describe the motion of the robot. They can be applied to any differentialsteering system as long as you are careful about the simplifying assumptions mentioned above. 
[4]  
[5]  
[6]  
[7]  
[8]  
The following equation is not used in the deadreckoning implementation, but is included for completeness:  
[9] 
where
is the robot's initial position,  
are the robot's position as a function of time,  
are the robot's initial and computed orientation, respectively, (always given in radians),  
are the velocities of the right and left wheels, respectively,  
is the distance between wheels, centertocenter,  
r  is the robot's turn radius,  
is time, in seconds. 
Tuning the PID Parameters
Once I had equations for the PID component functions, the next step was to obtain parameters for the PID equation. Because the selection of parameters depends on the physical characteristics of the system, there is no stock set of values that can be applied to every implementation. Instead, the parameters for the equations must be tuned to the particular platform they are intended to control. The selection of PID parameters is the aspect of the Peeves Project with which I am least satisfied. Although I eventually arrived at values that work, they do not represent an optimal solution. In general, there are three approaches that can be used to find values for the PID parameters: 
Table 3. Three approaches to tuning PID parameters. 
The analytical treatment is probably the best, but requires a thorough
understanding of the physics behind the system under consideration and a basic
understanding of techniques from control theory. Although my description might seem a bit
intimidating, the subject is well covered in many undergraduatelevel textbooks, including
the online version of Process Management
and Control referenced above. Unfortunately, the twomonth lead time for the CRS odometry contest did not allow enough time for me to get "up to speed" on the material. In the absence of a good theoretical model of the system, I attempted to use an empirical technique. There are a number of wellknown empirical techniques that can be used to tune PID parameters. One of the oldest, and most straightforward, of these is a method promulgated by Zeigler and Nichols in the 1940's. In the ZeiglerNichols openloop technique, a developer runs the system "onthebench" and observes its response to changes in input level. Test results are then plugged into an series of simple equations that produce estimates for the PID coefficients. ZeiglerNichols and related empirical methods often yield better results than a simple trial and error approach. They also have the advantage of not requiring knowledge of the process dynamics of a system. You may read a short article about the ZeiglerNichols technique in the Control Engineering Archives. The online text Process Motion and Control, Chapter 5, discusses PID tuning techniques at length and offers several more recent, but computationally intensive, alternatives to ZeiglerNichols. Although I attempted to use the ZeiglerNichols method to develop PID parameters for my own system, I'm afraid I struck out. My bench tests yielded unsuitable results. I concluded that either I was misapplying the technique or that the dynamics of the differentialsteering system did not match some of the basic assumptions of ZeiglerNichols. With no time left for further research, I resorted to trial and error. Thanks to tuning guidelines in the CarnegieMellon PID Tutorial, I was able to develop a set of parameters that worked reasonably well for Peeves. While the values I obtained were not optimal, an optimal set of parameters was not absolutely required for the odometry contest. Because the robot was expected to stick close to a straightline path, the error values would be quite small and well within the capabilities of the Peeves implementation. It was only when I tried to stress the system in noncompetition testing that shortcomings became apparent. To see the effect of the suboptimal selection of parameters, consider Figure 5. The figure shows the results from an experiment where the initial values of the accumulated error (integral error) was set to 40 clicks (51.2 mm, or about 2 inches). The robot was instructed to run a straightline path and store its accumulated and instantaneous error values in its datalog. Recall that the integrated error is the robot's lateral offset from the straightline path, or its ycoordinate, and is given by the function y(t) (a distance here measured in millimeters). And, again, we define its observed error, e(t), to be the rate of change of its ycoordinate with respect to time, or dy/dt (expressed as lateral movement in millimeters per second). Note that the PID algorithm corrects for the integrated (accumulated) error, an offset of 50 millimeters to the left, by steering to the right. The integrated error drops quite quickly, with relatively little oscillation. As the integrated error gets smaller, so does the magnitude of the controller's response. Ordinarily, this behavior is exactly what we would hope to see. Unfortunately, when the integrated error becomes less than 7 millimeters, the magnitude of the system's response is rather too small and it becomes slow to correct for the remaining offset. Figure 5. Measured data from straightline test. While I was disappointed by Peeves' inability to correct for the remaining offset in a timely manner, it was not that serious an issue in the contest. At most, it degrades the final accuracy of the robot by half a centimeter. Still, in a competition where millimeters count, it could become a serious issue. I plan on continuing my research on the subject and hope to offer more useful information in a future article.
Once the PID parameters were established, it was time to code the equations for the error functions in the NQC (Mindstorms) programming language. As noted above, the only data type supported by the RCX firmware is the 2byte signed integer. So not only were floatingpoint variables unavailable, but any integer arithmetic would have to remain within the range of 32768 to 32767. I would also have to work out approximations for the sine and cosine functions used in the equations shown in [6], [7], and [8]. Perhaps the most common way of handling fractional values on a system that supports only integers is to "scale" the values by multiplying them by some constant. The trick in choosing a scalar is to pick one large enough to avoid a loss of precision while finding one small enough that integer overflow doesn't become a problem. In the case of measures of distance, I was lucky enough to avoid the problem of scaling and roundoff altogether. As noted above, each click of the encoder corresponds to a distance of 1.28 mm. Rather than trying to find a way to scale conventional units of distance, I simply defined the "encoder click" as my fundamental unit of distance. By treating all distances in terms of "clicks", the Peeves software was left free of any need to perform conversions or account for loss of precision. The treatment of time was also easy. For the version of the RCX firmware that I was running, system timers had the resolution of 0.1 second, a fact that presented a natural value for scaling (a newer version of the firmware provides 0.01second resolution). In cases where the error functions required time values in their denominators, I simply multiplied the numerators by appropriate powers of 10. Angles were only a little harder. The equations used to describe the differentialsteering system treat angles in radians, not degrees. Although it was tempting to scale them by simply converting them to degrees, a better solution suggested itself. In the equations used for Peeves, angles are always used in close association with the parameter b (the distance between wheels, centertocenter), which in Peeves case has a value of 154 "clicks". So, internally, angles were multiplied by b to scale them to integers. For example, to specify a right angle, the NQC code for Peeves uses the value , or about 242 clicks. NQC does not provide utilities for computing the sine and cosine functions that occur in the differentialsteering equations. Fortunately, the trig functions in equations [6], [7], and [8] are used to operate on the angle deviation from the straightline path. Since the whole point of the PID implementation was to keep the robot traveling in a straight line, I could assume that these angles would be quite small. And, for angles sufficiently close to zero, the Taylor series provides the convenient approximations and . A little algebra and a willingness to disregard terms that were small contributors to the overall value, yielded the following approximations for [6], [7], and [8]: 
[10]  
[11]  
[12]  
where 

are the displacements of the right and left
wheels over the time interval t. 
The only thing left to do was to add the PID coefficients into the
equations and express them in code. A bit more work was required to obtain accurate
calculations within the restrictions of 2byte integers. You may see the results in the
source code at peeves.html.
The CRS deadreckoning course can be treated as a sequence of maneuvers, alternating between straightline and cornering operations. Competing robots begin the course traveling in a straight line down a corridor. Initially, the Peeves code sets to motors to default "gostraight" values that were determined through experimentation. Due to motorrelated issues mentioned above, even the best choice of available parameters resulted in the robot traveling a slightly curved path. So as soon as the motors are activated, the program enters a PIDcontrol loop. During the straightline operation, the program monitors its sensors to check for deviations from its standard course. Every 0.2 seconds, it performs the PIDrelated calculations and determines if a correction is required (although the RCX clock is calibrated in 0.1second intervals, the larger 0.2second value was chosen to reduce the effects of timing errors). If the PID calculation indicates the need for a correction, the software adjusts the motor output as appropriate. Refer to the source code at peeves.html for more detail. The robot also measures distancetraveled along the straightline path to determine when it reaches the end of a corridor. Once the corridor maneuver is completed, it switches to a turning operation. The PID technique described above kept Peeves traveling in a straight line. Although I may have been able to work out an analogous PID methodology for rounding the corners, I elected to use a simpler approach. In the turning phase the course, the robot simply sets the PWM level for its outer wheel at higher value than for the inner wheel. The resulting difference in wheel speed causes the system to follow a nearly circular path as described in the equations in [7] and [9]. During the turn phase, Peeves simply monitors the angle of its orientation and breaks out of the turn when it reaches 90 degrees. Upon completion of the turn the robot immediately sets its wheels to predetermined, "gostraight" settings and then performs calculations for the next phase of its navigation. These calculations involve correcting for errors that may have accumulated during the turn. First, there is a certain amount of delay in the robot’s response when it completes the turn. It may not sample its sensors until after it has crossed the 90degree threshold and even when it sets its PWM output to "gostraight" it will continue to rotate until the motors can overcome the robot’s angular momentum. This error in orientation is typically small and can be corrected by the PID logic when the robot enters its next straightline maneuver. To do so, the program simply notes the deviation from a perfect rightangle turn and treats it as an initial condition for the algorithm to process. 
A more substantial error results when the turn radius of the robot
deviates from the expected turn radius. Refer to Figure 6. If the robot were to
execute a turn with a radius equal to half the width of the course (9 inches, 22.9 cm), it
could begin the turn right at the end of one corridor and complete the turn at the
beginning of the next. But if the actual turn exceeded the turn radius by some deviation
factor, , the robot would finish its turn outside the centerline
and forward of the beginning of the corridor. The control logic needs to account for this
deviation and apply a correction. We can compute the actual turn radius using data from the rotation sensors and equation [5] above. To compensate for how far the robot has advanced into the corridor, we simply reduce the length of the straightline maneuver it is about to execute. Unfortunately, the lateral deviation (to the left and right of the centerline) is not so easily corrected. Although we could use the PID logic to adjust for the lateral deviation (treating it as the integral error term in the PID equation), the magnitude of the error might be too large to be absorbed by the compensation logic. Instead, we defer the correction until after the next turn. We can compensate for a lateral error on the current leg of the course by simply shortening or lengthening the distance the robot travels on the next. 
Figure 6. The deviation from central path when the turn radius is larger than expected. The vehicle finishes the turn both forward and outside of its intended position. Fortunately the error can be computed accurately. A symmetrical case exists when the turn radius is smaller than expected. 
Unfortunately, after the last turn in the course – when the robot is
in the home stretch – there is no next leg to adjust. While the obvious
solution is to simply plug this lateral error into the PID logic (treating it as the
integral error component), I have not yet been able to make it work properly. For now, the
robot knows that it is off the centerline, but does not have a method to correct for the
difference. As you can see, the ability to predict the radius of the next turn is an important part of keeping the robot near the centerline of the course. For example, if we estimate that the turn radius is going to be larger than the standard 9inches (half width of the course), we instruct the robot to begin its turn before it reaches the end of the corridor. Unfortunately, the actual turn radius depends on many different factors including battery age, ambient temperature, and the kind of flooring. That last factor was relevant in the Connecticut Robotics Society event because the competition took place on a different kind of flooring (linoleum) than had been used for developing the Peeves logic (concrete). The program did not have a good turnradius estimate for the first corner and deviated substantially from the centerline. As it progressed, however, it was able to keep track of its performance on each turn and improve its estimates until it had corrected for the early deviation. In addition to the offset due to turn radius, the robot also experiences a certain amount of orientation error due to "notquite90degree" turns. Timing and control issues make it very difficult to execute a perfect rightangle turn. Fortunately, the deviation from a 90degree turn can be measured using the wheel encoders and plugged back into the PID calculations when the robot reenters its straightline phase. Any error in orientation is quickly absorbed by the PID algorithm. In recent test runs on a concrete floor, Peeves consistently finished within 3 centimeters of its starting position... typically, about 1/2 cm short of the start/finish line and 2 cm to the right of center. While such accuracy is pretty good for a Legobased robot, there is still room for improvement. Due to the suboptimal selection of PID parameters mentioned above, I have not successfully implemented a correction for the lateral error on the last turn. Nor have I been completely successful in predicting the radius for the last turn (which would reduce the need for a lateral correction). Even if I addressed these issues, Peeves' internal position tracking can only account for about half the error. The robot knows that it is not deadon the start/finish position, but consistently underestimates its error. The discrepancy is probably due to a combination of mechanical inaccuracies and effects such as acceleration that are not correctly handled by the software. An imperfect transition from the cornering phase of its operation to the straightline traversal is also a likely source of inaccuracies. I believe that the competition in next year's CRS odometry and dead reckoning contest will be far more intense than it was this June. I sincerely hope that Peeves will be beaten by another robot that benefited from the information I've presented in this article. If the success of Peeves illustrates anything, it is the value of harnessing the large body of information available to the roboticist on the web and in the library. I hope that some of the notes that I have presented in this article will lead other robot builders to the resources they need for their own projects. Acknowledgments This article would not have been possible without the assistance of many people. I would like to thank Charles Olsen, a talented and knowledgeable engineer who patiently answered my incessant questions about electronics and control theory. Thanks also go to the the members of the Connecticut Robotics Society, in particular Bill Ruehl and Jake Mendelssohn whose good sense and extensive knowledge of robotics are always an inspiration, and to John Gallichotte who organized the CRS odometry event and made important suggestions regarding this article. I'd also like to thank Kenneth Maxon whose earlier Encoder article Reaching the Next Step in Motion Control and SensorSoftware Fusion for Mobile Robots helped jump start the Peeves development effort... without the information he provided, I wouldn't have known where to begin. The author gratefully acknowledges the assistance of Sonalysts, Inc. in the production of the graphics and photographs used in this article. Sonalysts is a Connecticutbased company providing services including software engineering for systems ranging from embedded processors to corporate intranets, from rapid prototyping to lifecycle maintenance. And, finally, I would like to thank my wife, Kathy Lucas, for her assistance in the preparation of this document and longsuffering support throughout the Peeves project.
Copyright © 2001 by G.W. Lucas.
