DRAFT

PID Closed Loop Control for robotics

By Alex Brown rbirac@homecom !"#"$$
Introd%ction
PID &Proportional' Inte(ral' Deri)ati)e* control laws will (i)e yo% more control o)er yo%r robot+s
performance ,hile the most immediate robot applications wo%ld probably be for steerin( and speed
control' the techni-%es can be applied to any .inds of motion
There are two basic types of control we can %se/ dead rec.onin(' in which we estimate the
commands necessary to achie)e a (oal s%ch as applyin( 0!1 P,2 to (o forward at 3 inches per second
and hope that the robot will tra)el at this speed4 or we can monitor the res%lts of the robot+s action and
correct the commands to ens%re that the (oal is achie)ed The more time the robot is correctin( its
performance rather than 5%st char(in( ahead' the more acc%rate performance yo%+re (oin( to (et
PID control systems pro)ide contin%o%s control 6ence' whene)er we can operate %nder PID
control' the performance will be m%ch more reliable The (oal sho%ld be to operate %nder PID type control
as m%ch as possible and to sa)e dead rec.onin( for only those times when there isn+t ade-%ate sensor
information a)ailable to do PID
Control systems
There are two basic types of control systems 7pen8loop control systems are those in which yo%r
controller tells yo%r system to do somethin(' b%t doesn+t %se the res%lt of that action to )erify the res%lts or
modify the commands to see that the 5ob is done properly Closed8loop control contin%o%sly monitors the
performance of yo%r system and chan(es the commands as necessary to stay on trac.
7pen8loop control
9xamples of open8loop control mi(ht be sendin( a P,2 si(nal to yo%r dri)e motor which yo%
.now will normally ca%se it to (o forward at 3 inches per second' b%t not meas%rin( the act%al speed and
therefore not .nowin( that the robot ran into a chair' or is climbin( a slope' and isn+t really mana(in( to (o
3 inches per second :o yo% tell the robot to (o forward for #$ seconds then t%rn ri(ht' expectin( it to (o !
feet and enter a doorway B%t it doesn+t ma.e ! feet and crashes into the wall 7r tellin( the robot to (o
strai(ht forward for a distance by settin( the steerin( position to center' and the robot slowly )eers off to
the side and crashes into the wall a(ain
The abo)e bloc. dia(ram shows the components of an open8loop control system The ;inp%t
reference< is the si(nal sent to the controller sayin(' perhaps' proceed forward at 3 inches per second The
;controller< bloc. represents the calc%lations done to control some ser)o or motor or somethin( on the
robot ;Plant< is a control systems term that means e)erythin( that is affected by the controller This may
incl%de a ser)o motor' the steerin( system of the robot' the motion of the robot in response to the steerin(
and e)en the res%ltin( position of the robot in its world The ;controlled )ariable< is whate)er part of the
plant yo%r inp%t reference is tryin( to control to It can be the ser)o position' the steerin( an(les or the
position in the world 7n a robot' the inp%t reference' the controller f%nction and parts of the plant may all
be done by yo%r microcontroller From the earlier example' the inp%t reference mi(ht be a command to (o
3 inches"second' and the controlled )ariable can be the act%al speed achie)ed which may' or may not be 3
inches"second
7pen8loop control is what dead rec.onin( is all abo%t =nfort%nately' no matter how caref%lly we
calibrate o%r motor and steerin( and other act%ation de)ices of the robot s%ch that the inp%t reference
sho%ld pro)ide the desired res%lt' there will always be errors which will ca%se the res%lt to be different than
what was planned And since errors are %s%ally c%m%lati)e o)er time' the lon(er yo% r%n in dead
rec.onin( mode' the farther yo%+ll be from what yo% planned
B%t' if open loop operations are short eno%(h &and nothin( .eeps the robot from operatin(
normally' li.e a chair*' open loop control may pro)ide ade-%ate performance for many tas.s
Closed8loop control
Closed8loop control starts o%t li.e open8loop in that yo% send an inp%t reference to the system'
b%t then feedbac. is %sed to monitor the performance res%ltin( from the command4 and if the performance
is different than desired' chan(es are made to the command to (et bac. on trac. This is done by
comparin( the inp%t reference to the feedbac. si(nal and comp%tin( an error from the reference The
controller then has e-%ations which ad5%st the commands to the plant to red%ce the error
Closed loop control is often approximated by doin( lon( dead rec.onin( actions' then stoppin(
and loo.in( aro%nd' or findin( a wall or corner in a .nown location' and %sin( that reference to fi(%re o%t
how to (et bac. on trac. Tr%e closed loop control is contin%o%sly chec.in( on performance &location'
speed' whate)er* so that the robot ne)er (ets si(nificantly off trac. For instance' to do wall followin(' the
distance to the wall wo%ld be meas%red contin%o%sly and the steerin( ad5%sted contin%o%sly to maintain
that distance 7r' in the speed control example abo)e' if the reference speed is 3 inches per second' and the
feedbac. si(nal indicates the robot is only mo)in( at ! > inches per second' the controller wo%ld see the
error si(nal and increase the power to the motor
To pic. an example we can all identify with' steerin( a real car alon( a road is li.e a wall or line
followin( exercise ?o% are fairly contin%o%sly chan(in( the steerin( to .eep the car centered in the lane
Ima(ine steerin( by aimin( the car then closin( yo%r eyes and only openin( them for a -%ic. (limpse
e)ery ! seconds or so to ma.e a correction It+s not only diffic%lt' it+s scary Beca%se yo%+re afraid yo%
mi(ht r%n into somethin( d%rin( yo%r dead rec.onin( phase :ame thin( for a robot
There are two aspects to this contin%o%s control/ one is the word contino%s' and the second is the
e-%ations to do the control I+ll tal. abo%t contin%o%s first
;Contin%o%s< really only exists in an analo( world 2ost of %s are doin( o%r robot control with
di(ital comp%ters' which can only calc%late a command after some time period B%t if yo% calc%late the
o%tp%t often eno%(h' it+s close eno%(h to contin%o%s to (i)e the same effect
For example' my c%rrent robot comp%tes its steerin( commands @$ times a second 9ffecti)ely'
it+s doin( #"@$ of a second of dead rec.onin( followed by a control command %pdate Fort%nately' it+s hard
to (et in m%ch tro%ble dead rec.onin( for #"@$ of a second
A sophisticated set of control e-%ations will ha)e many f%nctions which are dependent on the rate
at which they are calc%lated For example/ inte(rators' deri)ati)es and filters are all time dependent For
this reason' it is necessary to ha)e a software pro(ram &or at least part of the pro(ram* which exec%tes at
the desired repetition rate This ta.es some type of a real8time operatin( system This doesn+t ha)e to be
somethin( yo% b%y' or e)en somethin( complicated It can be done by pollin( &exec%tin( yo%r software
e)erytime a timer says the desired time has come*' or by a real time interr%pt which a%tomatically r%ns yo%r
software when the time comes
The interr%pt is more flexible as yo% can ha)e the software which is re-%ired to exec%te at a
specific rate r%n when the interr%pt occ%rs4 and r%n other' non8time critical software in the bac.(ro%nd
Polling software/
:tart/ InitialiAe anythin( that needs it
:et %p timer to trip at desired rate
Loop/ ,ait for timer to trip
&reset timer if necessary*
Do real time software tas.s
Inp%t sensors
Calc%late commands
7%tp%t commands
9ndloop
Interrupt driven software:
:tart InitialiAe anythin( that needs it
:et %p Interr%pt to occ%r at desired rate
Loop/ Do bac.(ro%nd tas.s' if any
9ndloop
Interr%pt/ Do real time software tas.s
Inp%t sensors
Calc%late commands
7%tp%t commands
Ret%rn
9ither of the two schemes abo)e will do the 5ob 9ffecti)ely what yo% are doin( is implementin(
a m%lti8tas.in( system 9ach time the real time software is called' a small slice of many 5obs may be
performed ?o% may be controllin( motor speed' controllin( steerin(' controllin( sonar and se)eral other
thin(s' all at @$ times a second
?o% will find that writin( software where s%bro%tines are entered repetiti)ely to do a sin(le 5ob is
a little different than %s%al For example' when a 5ob is first started' it may be necessary to initialiAe the
e-%ations B%t' not to initialiAe them on s%bse-%ent entries This may re-%ire other hi(her le)el software
to set a fla( indicatin( first pass &which the s%bro%tine can reset after doin( the intialiAation* or for the
hi(her le)el software itself to do the intialiAation I+ll try to p%t some examples in at the end to show how
this can be done
This so%nds more complicated' and it is' b%t it+s worth it
:o' what abo%t the e-%ationsB
PID e-%ations come o%t a a branch of mathematics"en(ineerin( called ;feedbac. and control
theory< The st%dy of this is hi(hly mathematical and (ets really into differential e-%ations' LaPlace
transforms' Cy-%ist' Root8Loc%s and Bode analysis and other wonderf%l thin(s All this math is necessary
to achie)e optimal performance with (ood system stability
6owe)er' I belie)e that for most simple systems &and most of o%r systems are simple*' a (ood
%nderstandin( of what the three e-%ations &P' I and D* do will allow yo% to hac. an ade-%ate sol%tion by
trial and error
PID stands for Proportional' Inte(ral' and Deri)iti)e These are three basic calc%lations that can
be %sed to control a system ?o% may find yo% can %se 5%st one' two or three to do a 5ob
Proportional control
The first and most basic part is the P' for proportional control All this means is that if yo% ha)e a
reference yo% are tryin( to control to' yo% pro)ide a control o%tp%t proportional to the error from yo%r
reference
&note/ D is commonly %sed to represent s%ch (ain factors' and is %s%ally (i)en with a s%bscript
indicatin( which (ain is meant 9( Dsteerin( *
As a standard robot example &ass%min( a front wheel steerin( robot*' if yo% are tryin( to follow a wall
%sin( a sensor that can meas%re the distance to the wall &e( sonar*' yo% wo%ld t%rn yo%r steerin( system
to ret%rn yo% to the reference If yo% are @ inches to the ri(ht' yo% mi(ht t%rn yo%r steerin( 0 de(rees to
the left If 0 inches to the ri(ht' then t%rn the steerin( E de(rees to the left That is the proportional part
A bloc. dia(ram of this operation mi(ht loo. li.e/
The drawin( indicates that the reference distance &the distance from the wall that yo% want the
robot to stay at* is s%btracted from the sonar distance This (i)es yo% an error si(nal showin( the distance
from where yo% want to be in inches &or feet or whate)er yo% want* The ;7< where the two si(nals come
to(ether is called a s%mmation point The ;F< indicates that the reference si(nal is added4 and the ;8;
means that the sonar sensor is s%btracted to (et the error si(nal That is/
error G reference distance H sonar distance
The steerin( (ain bloc. represents 5%st a m%ltiplier to (et the desired amo%nt of steerin( per inch
of error The whole thin( can be written as/
:teerin( an(le command G Dsteerin( I &reference distance H sonar distance*
:o what this e-%ation does is to steer the robot bac. toward the reference distance :o%nds li.e a
(ood idea' b%t in reality' this isn+t eno%(h ,hat will happen if we try to steer this wayB
If the robot starts o%t dead center on the reference' it will contin%e to roll strai(ht ahead Jery
(ood' except in the real world' it won+t stay exactly on center It will (et off a bit to one side or the other
,hen it does it will t%rn bac. toward the reference4 and will contin%e to .eep t%rnin( %ntil it (ets bac. to
the reference And while the amo%nt of steerin( (ets smaller as it approaches the reference' it+s still t%rnin(
in the same direction :o when it (ets bac. to the reference' its now pointed in a direction to de)iate onto
the other side As it (oes o%t the other side' it will ca%se steerin( commands to t%rn bac. a(ain' b%t it will
5%st lead to the same problem of o)ershootin( onto the ori(inal side
If yo% ma.e yo%r (ain fairly hi(h' to (et fast acc%rate performance' this simple proportional
control law will probably be %nstable ca%sin( the robot to (o bac. and forth from side to side followin( a
path similar to a sin%soid
Cow if yo% ma.e yo%r (ains low eno%(h' it may be that the robot will stay ;close< to the
reference B%t in reality the robot will be contin%o%sly ditherin( bac. and forth from one side to the other
It may be a small acceptable amo%nt of dither The tradeoff wo%ld be that with low (ains' yo%r robot will
probably be slow to ret%rn to the reference if errors occ%r' and may e)en (o %nstable with lar(e de)iations
&act%ally' as it was ditherin( bac. and forth' it ne)er was really stable anyway*
Later in this paper' there will be a comp%ter sim%lation of a robot steerin( system where yo% can
see this performance
There is a c%re for this instability It is the ;D< part of the PID4 the Deri)ati)e
Derivative control
A deri)ati)e is the rate of chan(e of a si(nal For example' if yo% are tra)elin( to a town #$
miles down the road4 and yo% are dri)in( there at 3! miles per ho%r &maybe K$ if yo% can (et away with
itL* ?o%r error is #$ miles' and yo%r deri)ati)e of that error is 3! miles"ho%r The deri)ati)e &or what I+m
probably (oin( to call rate most of the time* tells yo% how fast yo%r are approachin( or departin( from yo%r
reference
To (et bac. to the robot If yo% a tryin( to follow a wall at a distance of M3 inches' the rate si(nal
mi(ht say that the robot is approachin( the wall at @ inches per second :ince the (oal is to tra)el parallel
to the wall' we want the rate to be Aero 6ence' this rate si(nal is another type of error si(nal If the robot
is mo)in( closer to the wall' we want the steerin( to t%rn away from the wall If the robot is mo)in( away
from the wall ' we want to steer bac. towards the wall
In bloc. dia(ram form' this is e)en simpler than the proportional control abo)e
?o% don+t need a separate rate sensor to (et this si(nal' or special hardware to pro)ide it ?o% can
deri)e rate from yo%r meas%red si(nal by s%btractin( two consec%ti)e meas%rements and di)idin( by the
time between the meas%rements
If yo% are ma.in( meas%rements at a @$ hA rate'
Loop
Rate G &c%rrent meas%rement H pre)io%smeas%rement* "$!
Pre)io%s meas%rement G c%rrentmeas%rement Nsa)e for next loop
9ndloop
For example' if the robot is c%rrently M3! inches from the wall' and on the pre)io%s meas%rement
&$! seconds a(o*' it was M33 inches from the wall4 then the rate is &M3!8M33*"$! or @ inches per second
toward the wall
A bloc. dia(ram for the robot steerin( case follows4
This says that the steerin( will t%rn two de(rees for each inch per second that the robot is
de)iatin( from a path parallel to the wall Cote that the (ain is ne(ati)e This ca%ses the robot to t%rn bac.
toward a path parallel to the wall And clearly' as the rate (oes to Aero &meanin( parallel*' the steerin( will
(o to Aero also' so it will tend to stay parallel
The e-%ation for this wo%ld be/
:teerin( command G Drate I rate
The followin( fi(%re shows the effect of this control on a robot which starts o%t pointed toward a
reference line The robot t%rns to be parallel to the line &Aero rate*' b%t doesn+t care if it is on either side of
the line or e)en if it passes thro%(h the line
Another charactistic of the rate control is that it is m%ch more stable than the proportional (ain
since the robot m%st be parallel to the wall when the steerin( is Aero The proportional (ain was %nstable
since the steerin( went to Aero when the robot was the ri(ht distance from the wall' b%t there was not
(%arantee &or e)en li.elihood* that robot wo%ld be pointed in the ri(ht direction
This e-%ation pro)ides (ood (%idance to dri)e parallel to the wall b%t doesn+t really care if it is a
the desired distance
:o' now for the (ood news These two e-%ations &proportional and deri)ati)e* can be combined
to pro)ide a stable control which will trac. the wall at the desired distance
The followin( bloc. dia(ram and e-%ations show how the two are p%t to(ether/
steerin( command G Dsteerin( I &reference distance H sonar distance* F Drate I rate
,hat this e-%ation pro)ides is (%idance to ma.e an assymptotic approach to the reference Both the
distance error si(nal and the rate error si(nal are tryin( to dri)e the steerin( to ma.e themsel)es e-%al Aero
,hat will happen is that the steerin( will t%rn s%ch that a proportion is held between distance and rate The
proportional (ain is tryin( to ma.e the robot t%rn toward the reference line and the deri)ati)e (ain is tryin(
to ma.e the robot t%rn to be parallel :o if the robot is approachin( the line from the left' proportional is
tryin( to ma.e a ri(ht t%rn' and deri)ati)e is tryin( to ma.e a left t%rn At some point the two will balance'
and if this balance point is followed' the robot ends %p on the line For example in the bloc. dia(ram
abo)e' where the steerin( (ain and the rate (ain are both the same ma(nit%re' the steerin( will balance o%t
when the rate is e-%al to the distance If the robot is #@ inches to the ri(ht of the reference' and is mo)in(
to the left at #@ inchs"second' the steerin( will be Aero As the robot contin%es to the left %ntil it is 3 inches
away' the rate si(nal' now bein( proportionately lar(er than the distance will ca%se the steerin( to t%rn to
the ri(ht %ntil the rate is 5%st 3 inches per second when the steerin( command wo%ld a(ain be Aero
:imilarly' as the robot approaches to @ inches on the ri(ht of the reference' the rate will be red%ced to two
inches per second4 and as the distance error reaches Aero' the rate will also be Aero 6ence the robot will be
tra)ellin( parallel to the wall at the ri(ht distance As the control e-%ations are comp%ted many times a
second' the res%ltin( path will be smooth rather than the occassional t%rn implied abo)e
Integral control
Cow' if the proportional and deri)ati)e (ain wor.ed so (reat' as described abo)e' why do yo%
need an inte(ral term in the e-%ationB
2ainly beca%se of offsets and biases in the system For example' if yo% are steerin( with a hobby
ser)o and the di(ital Aero point yo% ha)e in software &the approximately #! millisecond point* does not
res%lt in the robot r%nnin( strai(ht ahead' the e-%ation is (oin( to ha)e to pro)ide a contin%o%s offset si(nal
to the ser)o to .eep the robot (oin( strai(ht In the pre)io%s examples' with a (ain of @ de(rees of steerin(
per inch of distance error' an offset of ! de(rees on the ser)o steerin( wo%ld re-%ire @! inches of distance
offset to cancel it o%t and (o strai(ht 2aybe for yo%r p%rpose @! inches of error is 7D B%t sometimes
yo% will want better acc%racyOthat is the system sho%ld compensate for any s%ch errors That+s what the
inte(ral term can do
Let+s say we already ha)e a proportional"deri)ati)e system which has t%rned to robot toward the
line4 b%t for whate)er reason ' the error si(nal doesn+t (o to Aero The inte(rator will create a si(nal to
cancel o%t that bias and ret%rn the error si(nal to Aero
An inte(rator is a de)ice &or e-%ation* which calc%lates the inte(ral of the inp%t o)er time The
way an inte(rator wor.s is to %se the inp%t to create an o%tp%t which will contin%e to (row %ntil the inp%t is
red%ced to Aero This is implemented on a comp%ter by addin( the inp%t to the pre)io%s o%tp%t of the
inte(rator each time the inte(rator e-%ation is performed This wo%ld be somethin( li.e/
Inte(rator7%tp%t G Inte(rator 7%tp%t F Inte(ratorInp%t
To clarify' the next fi(%re shows how an inte(rator responds to step inp%ts o)er time
The inte(rator is effecti)ely calc%latin( the offset distance &error* times the Time that that error
exists for If the inp%t stays at ! inches for @ seconds' the inte(rator o%tp%t will be #$ inch8seconds If the
error then (oes to Aero' the inte(rator will remain at #$' if the error (oes ne(ati)e' the inte(rator will start to
red%ce its o%tp%t
An inte(rator is easy to implement in software All yo% do is ta.e the c%rrent )al%e of error and
m%ltiply it by the period of each calc%lation cycle &e( if doin( control laws at @$ times per second' the
period wo%ld be $$! seconds* ?o% then .eep a r%nnin( s%mmation of these calc%lations
Loop
Inte(rator G Inte(rator F &error I $$!*
9ndloop
The only thin( yo% ha)e to do is initialiAe the inte(rator to a desired )al%e &%s%ally Aero* before
yo% start r%nnin( the loop
steerin( G Dinte(ral I inte(rator o%tp%t
Cote that this new inte(rator circ%it is %sin( the reference error as its inp%t That+s
beca%se the reference is the si(nal yo%+re really tryin( to trac. &not the rate si(nals' which is the only other
si(nal in this example :o if the error si(nal is not a)era(in( Aero' that is' it+s stayin( off to one side' the
inte(rator will slowly increase in )al%e That inte(rator o%tp%t is s%mmed into the steerin( command with
a si(n that will ca%se the steerin( to mo)e in the direction to red%ce the error 9)ent%ally' when the
inte(rator o%tp%t (ets lar(e eno%(h to cancel o%t whate)er bias was ca%sin( the error' the error will (o to
Aero and the inte(rator o%tp%t will remain constant and the robot will proceed with Aero error
Putting it all together
The followin( fi(%re shows all three terms p%t to(ether It shows that by ha)in( 5%st a sin(le
meas%red inp%t si(nal' yo% can ha)e proportional' deri)ati)e and inte(ral components to yo%r control It
becomes 5%st a matter of selectin( the ri(ht (ains for each path And we+ll (o into that more later
Using the control law simulator
Attached &or enclosed or somethin(* is a pro(ram written to r%n %nder Pbasic which I ass%me
most all of yo% ha)e on yo%r PC
This pro(ram is a simple sim%lation of a robot which is capable of doin( line followin( It has an
%ndefined sensor which pro)ides the error from the line in inches &this distance is referred to as ? in the
pro(ram*
The robot proceeds across the screen as a (raph where it is proceedin( forward at @ feet per
second and the two plots show the distance from the line &and therefore its path*' and the steerin( an(le of
the wheels which are set %p to be front wheel steerin( li.e a car And since car wheels don+t t%rn )ery far'
these steerin( wheels are limited to @$ de(rees each direction
?o% ha)e the ability to position the robot at a startin( position on or offset from the line and at an
an(le parallel to' con)er(in(' or di)er(in( from the line to the left or ri(ht
?o% can set (ains for Dsteerin(' D rate and Dint and see what the res%lts are and (et a feel for
what the (ains do
The followin( is a recommendation of experiments to do to e)al%ate each (ain path and the effect
of their combination
Brin( %p Pbasic and load the pro(ram If yo% space down to abo%t the @#
st
line yo%+ll find a
(ro%p of lines that loo. li.e/
? G $
An(le G $
Dsteerin( G $
Drate G $
Dint G $
Bias G $
These are the n%mbers yo% can chan(e &Please exc%se the %ser %n8friendliness' b%t I+m too laAy
to add a men% to chan(e parameters4 and yo% are all pro(rammers &or will be* and can handle it*
As a first sample' edit the pro(ram to ma.e ? G #@ and Dsteerin( G 8@ Then select R%nQstart
?o% sho%ld (et a co%ple plots come %p on the screen showin( the robot startin( #@ inches to the
ri(ht of the centerline' and oscillatin( bac. and forth The second plot shows the an(le of the steerin(
which also mo)es bac. and forth from left to ri(ht and hits the @$ de(ree steerin( limits at its extreme
?o%r initial set%p )al%es are also listed on the %pper part of the screen
This pro(ram implements a PID control 5%st as was explained' so lets 5%st (o thr% each (ain and
see how it wor.s
# :et ? G 3' D steerin( G 8$$# &e)erythin( else at Aero* and r%n
?o%+ll see that the robot slowly t%rns bac. toward the line
@ :et Dsteerin( G 8$# and r%n
?o%+ll see that the robot t%rns more rapidly bac. toward the line and o)ershoots to the
other side Cote that )ery little steerin( motion is (oin( on 9xample # wo%ld ha)e also
o)ershot if we (a)e it more time Cote that raisin( the (ain ca%se an increase in the
response speed And an increase in the fre-%ency of the oscillation'
M Repeat %sin( (ains of H#$ and H#$$
Cotice that with the (ain of 8#$' the response is (ettin( pretty -%ic. and the steerin( is
mo)in( abo%t #$ de(rees which is half of the maxim%m a)ailable ,hen yo% (et to the
(ain of H#$' the response is abo%t as fast as it is (oin( to (et and the steerin( is (oin(
from f%ll ri(ht to f%ll left and bac. 9)en if yo% raise the (ain more' the steerin( mi(ht
(et a bit more abr%pt' b%t the response will be abo%t the same
:o what the abo)e demonstrates is that &at least for this steerin( case*' a proportional only (ain is
always (oin( to be oscillatory4 and that raisin( the (ain increases the speed of the response
0 :o lets try the Drate (ain :et ? G 3' Dsteerin( G $ and D rate G 8# Ro r%n it
Cote the robot 5%st (oes strai(ht That+s beca%se the rate is Aero' so it+s happy
! :et the an(le to #$ That starts the robot o%t at an an(le to the line
Cotice it steers %ntil it is parallel to the line abo%t #! inches away
3 Try a(ain with an an(le of H#$
A(ain it t%rns to be parallel' b%t still doesn+t care that it+s not on the line And notice
that not a lot of steerin( is %sed so it co%ld be more responsi)e It ta.es almost #$
seconds to (et to parallel
K :o do it with a (ain of H#$ Cow it responds )ery -%ic.ly' (ettin( to parallel within #
second Probably faster than yo% need
E :o try it with a (ain of H! It %ses f%ll steerin( initially' b%t then smoothly t%rns onto a
parallel path within two seconds Cot too bad
S Cow' lets add some of the proportional (ain bac. in to see how the two wor. to(ether
:et Dsteerin( to 8#' notice how now it doesn+t -%ite (o to parallel' b%t slowly starts
(oin( bac. to the line
#$ :et Dsteerin( %p to H# Cow it (ets onto the line within #! seconds' b%t still not too
impressi)e
## :ettin( Dsteerin( to H#$' yo%+ll find that it (ets bac. to the center -%ite rapidly In fact'
too rapidly It now (oes past the line and has to t%rn bac. to (et on the line This (oin(
past the line is called ;o)ershoot< In (eneral' anytime yo%r si(nal (oes past its desired
)al%e and ret%rns' it is called o)ershoot
#@ 7)ershoot is (enerally not desireable' in this case' the Dsteerin( (ain is too hi(h :o
red%ce it to H! and see what happens Loo.s a lot better
#M T%st to see the effect' try a m%ch hi(her Dsteerin( (ain &8M$* and note that the system
oscillates m%ch as it did with only a proportional (ain' howe)er the rate (ain does ca%se
the oscillation to damp o%t e)ent%ally
#0 :o lets ta.e these (ains &Dsteerin( G 8! and Drate G 8! and see how they handle some test
conditions/
? G An(le G
3 $
#@ $
$ 8#$
$ 8@$
3 8#$
Performance of all these conditions loo.s pretty (ood since the robot ret%rns to the line
with little o)ershoot
Play aro%nd %sin( different combinations of (ains for these test cases and see the effect
on performance
#! In a steerin( system li.e this' yo% aren+t li.ely to need an inte(ral (ain' b%t let+s see what
it wo%ld do :et the offset ? and the an(le to Aero' and set bias to ! ?o%+ll see that the
steerin( starts at ! de(rees and the robot starts on the line B%t the robot mo)es o)er
abo%t an inch or so to the ri(ht which ca%ses eno%(h si(nal thr% the proportional (ain to
center the steerin( and control the robot parallel to the line If yo% want to experiment'
chan(e the Dsteerin( (ains %p and down and see how it affects the amo%nt of the standoff
&as standoff is a displacement from the reference which is held o)er time* The standoff
sho%ld be proportional to the Dsteerin( (ain
#3 Cow add some inte(ral (ain/ set Dint to H$! ?o%+ll see that while the steerin( bias
ca%ses the same initial motion to the standoff position' the inte(ral (ain slowly ca%ses the
robot to control bac. to the line
#K ?o% can experiment with increasin( the inte(ral (ain to see if yo% can (et the robot to
ret%rn to the line more -%ic.ly 7ne thin( to watch for is that inte(ral (ains' if too lar(e'
can ca%se oscillations :et Dint to H#! and note that a di)er(ent oscillation occ%rs
I2PL929CTICR A :?:T92 =:ICR T69:9 29T67D:/
Before startin(' I+d li.e to point o%t one re-%irement for this system that may be apparent' b%t I
ha)en+t stated explicitly The inp%t si(nals ha)e to pro)ide linear meas%rements Li.e they ha)e to pro)ide
the distance in inches' not the lo(ic that a distance is (reater or less than a reference )al%e Fort%nately
s%ch sensors are readily a)ailable 9( sonar' IR' tachometers' encoders etc
Another pro)iso mi(ht be that the comp%ter sim%lation yo% played with does not ha)e the same
.ind of errors' mechanical slop and noise that yo% will find in a real world system The (ains yo% can %se
in the real world probably won+t be -%ite as hi(h as yo% co%ld %se in the sim%lation B%t' now that yo%
.now what the (ains do' yo% can ad5%st them in yo%r robot to (et the best performance possible
:o' the only thin(s yo% need to (et the performance yo% want is the )al%e of the three (ains'
Dsteerin( and Drate and Dint Cow' there are entire boo.s and co%rses in control law analysis to determine
optimal (ains for best performance Fort%nately' as lon( as the system is pretty simple' a (ood hac.er can
find some (ood (ains by trial and error and maybe a little tho%(ht
First' to (et )ery fast performance in ret%rnin( to the reference' ' yo% need hi(h displacement
&distance* (ains To (et more stability' yo% want hi(h rate (ains :o%nds li.e yo% 5%st ma.e them both
hi(h and (o with itB It+s not T6AT easy ?o% saw with the sim%lation that raisin( (ains impro)es
performance for awhile' b%t too m%ch can ca%se oscillation to occ%r
:ensor noise &ca%sed by inacc%racies in the sensor or irre(%larities in the wall bein( sensed* can' if
sent thro%(h lar(e displacement and rate (ains' ca%se the steerin( to be ma.in( lar(e motions from left to
ri(ht tryin( to follow the noise )al%es ,hile this s%perfl%o%s steerin( acti)ity will probably a)era(e o%t
and follow the desired path It can loo. terrible and is wastef%l of ener(y to do all the steerin(
:o' noise is one reason to .eep (ains as low as practical while still meetin( yo%r desired
performance By the way' if sensor noise is a real problem' yo% may be able to filter o%t some of the noise'
hence .eepin( steerin( excess motion %nder control
And there are often limits as to how -%ic.ly yo%r system can respond For instance' the comp%ter
sim%lation limited the steerin( to FQ8 @$ de(rees and also limited the rate the steerin( co%ld mo)e from one
position to another This showed %p in a lot of the oscillatory cases as the steerin( 5%st (oin( bac. and forth
between the limits as fast as it co%ld This is sometimes called a ;limit cycle< since the system is 5%st
cyclin( bac. and forth between to limitin( )al%es
Lets do a wall followin( ro%tine first
First' I+m (oin( to ass%me that yo% ha)e a sonar s%bro%tine that can reliably meas%re the distance
to the wall Also' I+ll ass%me yo% ha)e a steerin( system which can direct the robot to t%rn left or ri(ht
proportionally to a command si(nal In a robot with a car type steerin( system &li.e I ha)e*' this wo%ld be
the an(le of the front wheels In a robot which steers by differential t%rnin( of the two dri)e wheels' it
mi(ht be the difference in the commanded speeds of the two wheels If yo% ha)e a robot that wal.s' yo%
fi(%re o%t what si(nal defines how m%ch it t%rns Anyway' the e-%ations can wor.
7b)io%sly we (et the proportional error si(nal by s%btractin( the sonar )al%e from the reference
B%t where does the rate come fromB ?o% can calc%late the rate by comp%tin( the difference between two
conse-%ti)e sonar readin(s and di)idin( by the time between readin(s For example' if one sonar readin(
says yo% are @0 inches from the wall' and the next one &!$ milliseconds later* says yo% are @0! inches from
the wall' then the robot is mo)in( away from the wall at #$ inches per second &@0! H@0* " $!$
Rate G &C%rrent:onarJal%e H Last:onarJal%e*"$!
Cote that this is one of those e-%ations that m%st be exec%ted at the proper repetition rate'
otherwise the rate si(nal will be wron( if the time inter)al was not $$! seconds
&Cote/ doin( a simple calc%lation li.e this for rate can res%lt in a noisy rate si(nal if the inp%t
sonar si(nal has m%ch noise on it The rate si(nal (ets partic%larly noisy since yo% are di)idin( by a short
time inter)al and effecti)ely m%ltiplyin( the noise by @$ In my own robot' the sonar si(nal is pretty (ood
and I can %se the simple e-%ation written abo)e If yo% find that yo% do ha)e so m%ch noise that it is
affectin( yo%r steerin( control too m%ch' yo% can always filter or smooth yo%r rate si(nal' maybe by
somethin( as simple as 5%st a)era(in( the last few )al%es *
:basic example/
yd = (sonardist - sonardistold)*20 'lateral rate B8 fps
sonardistold = sonardist
The code sample ass%mes only that yo% ha)e pre)io%sly r%n a sonar s%bro%tine that ret%rned the
distance as ;sonardist<' and that sonardistold was sa)ed on the pre)io%s iteration of this code Both
meas%rin( the distance and r%nnin( this code m%st occ%r at @$ times per second to ma.e the ;I@$< wor.
correctly 7ther iteration rates can be %sed by chan(in( the ;I@$< ?d is the lateral rate in feet per second
The comment of ;BE fps< is to remind me that the Binary point is E bits from the ri(ht
7D' now that yo% ha)e the two inp%t si(nals &error and rate* and ha)e a steerin( s%bro%tine that
can accept the o%tp%t' nothin( remains except to determine the (ains for the three si(nal paths/ Dsteerin('
Drate and Dinte(ral
:tart with both the Dsteerin( and Dinte(rator (ains set to Aero Then ta.e a (%ess at how m%ch the
steerin( (ain sho%ld be For instance' if yo%r robot was mo)in( forward at # foot per second' and mo)in(
toward the wall at M inches per second' how m%ch do yo% thin. yo%+d want to t%rn the steerin( to direct the
robot to a path parallel to the wallB This depends a bit on the (eometry of yo%r robot By simple math' yo%
.now that if the robot is (oin( #@ inches per second forward' and M inches per second towards the wall'
that it is approachin( the wall at abo%t a #! de(ree an(le :o p%t yo%r robot on the (ro%nd pointed #!
de(rees toward a wall and t%rn its steerin( wheels %ntil when yo% p%sh the robot forward' it ma.es a nice
t%rn %ntil it is parallel ?o% probably don+t want to %se either the f%ll steerin( capability &or else yo% can
not ma.e control corrections in both directions* or so little that the robot ta.es fore)er to t%rn There+s no
ma(ic answer T%st an estimate :o let+s say yo% (%ess the steerin( sho%ld be #$ de(rees for this sit%ation
This wo%ld (i)e a (ain of /
Drate G #$ de( steerin( " M inches per second G MMM
Cote/ those of yo% who are sayin(' ;I don+t ha)e floatin( point math in my comp%ter' how do I
do MMB<' han( in there and I+ll show yo% a way to do fractions %sin( inte(er arithmetic later
:o' pro(ram the Drate (ain of MMM into yo%r robot and then pro(ram him to (o forward at #
ft"sec Then set it down abo%t parallel to the wall and start it mo)in( It sho%ld maintain a line ro%(hly
parallel to the wall Cow try it startin( from directions pointed sli(htly toward or away from the wall The
robot sho%ld t%rn to become parallel ?o% can now increase and"or decrease the (ain %ntil the t%rnin(
performance loo.s (ood to yo% Basically' increasin( the (ain will 5%st ca%se it to t%rn more -%ic.ly to
reach parallel4 and red%cin( the (ain will ca%se it to t%rn more slowly and smoothly
Cote/ The (ain deri)ed abo)e assumed the speed was # foot"second to calc%late the #! de(ree an(le That
an(le wo%ld ob)io%sly chan(e dependin( on the robot+s speed The an(le wo%ld be smaller as speed
increased 6ence the amo%nt of correction yo% wo%ld want to do wo%ld also decrease as speed increase
7ne easy way to acco%nt for this is to ma.e the (ain' Drate' a f%nction of speed
Drate G MMM I #$ fps"act%al speed

9xperiment %sin( different (ains and an(les %ntil yo% thin. yo% ha)e a (ain that will wor. for all
the an(les yo% expect to see in real life Try some )ery lar(e an(les' 3$ de(' S$ de( #@$ de( :ee that
maybe the res%lts won+t be )ery (ood for lar(e )al%es Cote that PID control laws (enerally don+t respond
well to )ery lar(e errors In (eneral' yo% sho%ld only (et )ery lar(e errors at the transition to a new
reference' e( after a t%rn ?o% can either ens%re that yo%r t%rnin( acc%racy &which may be open loop* is
(ood eno%(h to pro)ide a close to the reference startin( point4 or re)ise the control laws to handle lar(e
errors' at least temporarily' %ntil the optimiAed e-%ations can perform ade-%ately
Cow' to (et it to trac. to the reference distance' yo% ha)e to pro(ram in a Dsteerin( (ain A(ain' yo%
can ma.e an estimate :ince the proportional si(nal and the rate si(nal wor. to(ether to (%ide the robot to
the reference' it becomes a -%estion of how fast do yo% want the robot to ret%rn to the referenceB
P%ic.lyB 7r slowly and smoothlyB For example' if the robot is 3 inches away from the reference' how
soon do yo% want him to (et bac. to the reference Probably a co%ple seconds is a (ood place to start If
so' when the distance is 3 inches' yo% want the rate to be M inches per second :o' yo% want the rate (ain to
be twice as hi(h as the proportional (ain &so that the s%m of the two terms will be Aero*
:o with the Drate (ain set to MMM' set the Dsteerin( (ain to #33 Cow p%t yo%r robot bac. down near
the wall and r%n some tests to see how these (ains wor. :ome (ood tests mi(ht be/
P%t the robot down on the reference distance pointed parallel to the wall and see if it stays there
P%t the robot down parallel to the wall b%t 3 inches from the reference on either side :ee if it
smoothly mo)es to the reference
P%t the robot down an(led sli(htly &! to #! de(rees* toward or away from the wall and see it it
strai(htens o%t and mo)es bac. to the reference
?o%+ll probably find that for small an(les and small displacements from the reference that the
robot mana(es to t%rn and (et bac. onto the reference distance ?o% mi(ht also find that for lar(er an(les
or displacements that the robot can+t ma.e the t%rns smoothly onto the reference and maybe oscillates bac.
and forth alon( the line &or crashes into the wall or other ob5ectionable' b%t normal' robot beha)ior*
?o% ha)e se)eral choices at this point :ince the an(les and displacements yo%+)e been testin( are
5%st examples of how acc%rately yo%r pre)io%s robot trac.in( commands lined the robot %p for this wall
followin(' yo% may decide that it can handle all the an(les and displacements yo% expect If so' yo% can
.eep these (ains and (o on to other problems If yo% want the robot to be able to handle lar(er startin(
errors' yo% can try chan(in( the Dsteerin( (ain' probably red%cin( it This will (i)e a slower ret%rn to the
reference' b%t yo%+ll probably find it can handle lar(er startin( errors
In a steerin( control sit%ation li.e this' yo%+ll probably find that an inte(ral (ain isn+t necessary
If yo%r robot (oes pretty strai(ht when yo% set the steerin( to Aero' the bias error from the reference
distance probably won+t e)en be noticeable
A second sample implementation Speed Control
PID control laws for a speed control system will loo. a little different from a steerin( system In
partic%lar' a speed control system &at least the one I+m (oin( to (o thro%(h* doesn+t need a rate (ain' and it
does need an inte(ral (ain ,hy is this soB
Basically' it+s beca%se the steerin( control has more le)els of inte(ration in its ;Plant< than a speed
control sytem does
A front wheel steerin( system does not directly control the distance from the wall It chan(es the steerin(
an(le of the front wheels As the robot mo)es forward o)er time' the steerin( an(le will ca%se the car to
t%rn so that it is tra)elin( toward &or away from* the wall Then' o)er time' if the car is pointed toward the
wall' it will mo)e closer to the wall 9ach of these mo)in( forwards is an inte(ration
A motor speed control sytem directly controls the speed of the motor There are no inte(ration
sta(es
The tric. is that in a closed loop control sytem' inte(rators are destabiliAin( 6ence it is harder to
stabiliAe a steerin(8to8a8wall reference system than to stabiliAe a motor speed This re-%ires %se of hi(her
order deri)ati)e terms to be come stable &proportional is the deri)ati)e of inte(ral' and rate the deri)ati)e of
proportional*
If the steerin( control system %sed the steerin( wheel an(le as the controlled si(nal in the
feedbac.' it wo%ld be as simple as the motor speed control If the steerin( system controlled the robots
an(le to the wall' it wo%ld be a little more complex than the motor control d%e to the one le)el of
inte(ration :ince it controls to the wall' after two le)els of inte(ration' steerin( is e)en more complex
2ost motor control sytems %se only the proportional and inte(ral e-%ations In fact' the system
co%ld be stable with only the inte(ral term Jery different from what yo% saw %sin( the sim%lation of the
steerin( system
Lets ima(ine what wo%ld happen if only an inte(ral term was %sed ,hen a reference speed was
applied to the controller A lar(e error wo%ld exist so the inte(rator wo%ld be(in to b%ild %p in a direction
to ma.e the motor r%n 9)ent%ally' the motor wo%ld start r%nnin( and slowly accelerate as the inte(rator
contin%ed to b%ild %p As the motor approached the reference speed' the error to the inte(rator wo%ld (o to
Aero and the inte(rator wo%ld remain constant pro)idin( 5%st eno%(h command to .eep the motor on the
reference speed If the load on the motor chan(ed' ca%sin( its speed to chan(e' the inte(rator wo%ld a(ain
see an error and chan(e its o%tp%t %ntil the motor speed was correct a(ain
:o' an inte(ral term alone can control a motors speed' b%t it doesn+t do a )ery fast 5ob of it It is
slow beca%se the inte(rator (ain %s%ally has to be fairly low to remain stable If the (ain is so hi(h that it
exceeds the motors ability to respond' the command will o)ershoot the needed )al%e for steady state
operation and the loop will oscillate
Addin( a proportional (ain will pro)ide m%ch faster response since there is no time delay for it to
b%ild %p It immediately pro)ides a command proportional to the error and can (et the motor immediately
mo)in( 6owe)er' as the motor starts to approach the reference speed' the error (ets smaller and hence the
proportional term (ets smaller :o' in the end' the inte(rator will still ha)e to b%ild %p to hold the whole
command so that the proportional term can be Aero when the error is Aero In fact' the motor will (et %p to
speed e)en more slowly than the inte(ral term alone since the motor starts r%nnin( sooner and the
inte(rator sees less error to ma.e it b%ild %p
There m%st be a better way And there is/ a new term' often called ;feed forward< which is an
estimate of how m%ch command is re-%ired to r%n the motor at the desired speed The feed forward term is
an open8loop command to (et the motor close to speed' then the proportional and inte(ral terms (et it ri(ht
on and also compensate for load ind%ced speed chan(es
:o to determine the (ains for a motor speed control sytem' yo% first determine a feed forward (ain
which will dri)e the motor to approximately the desired speed This feed forward term may not necessarily
be a simple (ain For my P,2 controlled motor' it ta.es abo%t 0$1 P,2 5%st to (et the motor started' so
my feed forward term is/ 0$1 F Dff I :peed Reference ?o% can determine the feed forward term by
trial and error T%st find somethin( that will dri)e the motor to abo%t the ri(ht speed
Cext yo% want to impro)e the motors response to loadin( chan(es ,ith 5%st feed forward' if yo%
load the motor more hea)ily' or li(htly' the speed will chan(e and stay at a new )al%e ?o% can now add a
porportional (ain which will immediately respond in a direction to dri)e the motor toward the reference
speed B%t the proportional (ain can+t (et the motor all the way bac. on speed beca%se as the speed
approaches the reference speed' the error tends toward Aero and the proportional term starts to (o away
,hat this means is that the proportional term can red%ce errors b%t not eliminate them :o to implement a
proportional (ain' r%n the motor and chan(e the loadin( &maybe 5%st slow the wheels down by hand*
Increase the proportional (ain as far as yo% can before %ndesirable effects &li.e oscillation or 5er.iness* start
to appear
Then to ta.e care of the steady state additional command to handle the new loadin(' increase the
inte(ral (ain The inte(ral (ain can be similarly increased %ntil bad effects are seen
Cow why was steerin( command different in the way we set it %p ,e saw that speed control had
no inte(rators in the plant and steerin( had two ,e saw that the inte(ral term co%ld pro)ide stable control
for speed b%t not for steerin( Basically' it ta.es deri)ati)e terms to stabiliAe each of the inte(rators in the
steerin( plant The proportional term is %sed to stabiliAe one inte(rator' and the rate term is %sed to
stabiliAe the next %pstream inte(rator
?o% ha)e to ha)e a stable system to experiment with the (ains' so for the motor speed control yo%
can start with either the proportional or the inte(ral B%t for the steerin( yo% m%st start with the rate term
5%st to (et a stable system
Appendix A
Fractions %sin( inte(er arithmetic
A fraction can be represented in an inte(er word by assi(nin( a ;binary point< to a position in the
word which yo% ta.e care of in yo%r code and calc%lations The comp%ter ne)er .nows it+s there
For example' I+m %sin( an 2otorola 3E6CS#@BM@ comp%ter with #3 bit inte(er words coded in
:basic I ha)e defined many of my )ariable to ha)e a binary point between the first and last E bits of the
word A binary point is similar to a decimal point in that all the n%mbers to the left of the binary point are
%nit )al%e' and the n%mber to the left are fractional This means that the ran(e of my word is now F"8 #@K
with fractional part down to #"#@E of a %nit For example' ! feet is represented as U$!$$ $! for the feet
and $$ for the fraction ! > feet wo%ld be V$!E$
Cow' ob)io%sly' this red%ces the ran(e of the word considerably' b%t it does allow fractions And
yo% can p%t the binary point at different places if it s%its yo%r p%rposes
:o addin( and s%btractin( fractions is easy4 5%st add or s%btract the #3 bit n%mbers and yo% will
(et the ri(ht fractional res%lt
The problem arises when yo% want to m%ltiply or di)ide 2%ltiplyin( two #3 bit n%mbers (i)es a
M@ bit res%lt 2%ltiplyin( two n%mbers with E bit fractional components means the inte(er all ends in the
top #3 bits' and the bottom #3 bits is all fraction ,hat yo% can do with this sit%ation depends on what
lan(%a(e yo% are codin( in If yo% ha)e access to all M@ bits' yo% can %se bits @M thr% E to constr%ct a #3
bit word with the normal E bit fraction If yo% don+t ha)e access to the top #3 bits &as in :basic*' yo% can
5%st m%ltiply by small eno%(h n%mbers so as not to exceed the max )al%e of F"8 #@K
For instance' in wall followin(' I wo%ld ne)er expect an error of more than @ feet' so I+d feel
comfortable m%ltiplyin( by anythin( %p to 3$
Cow I don+t %s%ally want to m%ltiply by lar(e n%mbers' b%t I mi(ht want to m%ltiply by small
n%mbers with a fraction The easy way is by m%ltiplyin( by one inte(er and di)idin( by another
For example' to m%ltiply by @K!' m%ltiply by ## then di)ide by 0 ?es' this may be a bit
aw.ward' b%t it wor.s T%st ma.e s%re yo%r first m%ltiplier doesn+t ca%se an o)erflow
P: If yo%+re %sin( :basic' one thin( yo%+ll soon find is that it doesn+t pro)ide the si(ned
m%ltiply and di)ide necessary to ma.e this scheme wor. with positi)e and ne(ati)e n%mbers It assembles
to only %nsi(ned m%ltiply and di)ide &em%l and idi) on the 6C#@* ?o% can wor. aro%nd this by %sin( the
find and replace f%nction of yo%r editor to chan(e all em%l to em%ls and idi) to idi)s in the asm file before
assemblin( &and if I e)er (et aro%nd to it I+m (oin( to write a batch file or somethin( to a%tomate thisO
b%t it+s 5%st easy eno%(h that I don+t seem to (et aro%nd to it*
Appendix B
Code for Pbasic :teerin( control law sim%lation
'Control system simulation program for small robots.
' by Ale Bro!n rbira"#$ome."om %&2'&00
'(emonstrates t$e effe"ts of gain "$anges in a )*( "ontrol system using
a
'simple simulated robot steering system.
' +$e robot is assumed to be follo!ing a straig$t line and "an steer to
t$e
'left and rig$t.
' +$e starting point of t$e robot "an be set by "$anging t$e ,alues in
t$e
'program (no time for user friendly on-line "$anges).
' +$e "$angable ,alues are-
' . distan"e from t$e line in in"$es (/ = left of line)
' Angle of robot to line in degrees (/ = pointed left)
' 0steering- t$e 1proportional1 gain
' 0rate- t$e 1deri,ati,e1gain
' 0int- t$e 1integral1 gain
' Bias- adds an offset to t$e steering system
' 2$en 13un&4tart1 is sele"ted5 t$e robot mo,es for!ard at 26 in"$es
per
'se"ond for 27 se"onds..and t$e results are plotted s$o!ing . and
steering.
' 8imits steering !$eels turn angle to 20 degrees
y = 0 'in"$es (/ is rig$t of line5 do!n on
grap$)
angle = 0 'degrees (/ is pointed to rig$t5 do!n)
Bias = 0 'front steering off "enter bias
0steering = 0 'degrees steering&in"$
0rate = 0 'degrees steering&in"$&se"
0int = 0 'degrees steering& in"$-se"ond
4peed = 26 'in"$es per se"ond of for!ard motion
= 0 'in"$es starting point tra,eling for!ard
steering = 0 'steering starts pointed straig$t a$ead
rate = 4peed * 4*9(angle & 7:.%) ' "al"ulate .rate from angle and speed
4C3;;9 ('') '<60680 grap$i"s
8=CA+; '5 '- )3*9+ 1initial offset . = 1> y
8=CA+; 25 '- )3*9+ 1initial angle = 1> angle
8=CA+; %5 '- )3*9+ 1Bias = 1> Bias
8=CA+; 65 '- )3*9+ 10steering = 1> 0steering
8=CA+; 75 '- )3*9+ 10rate = 1> 0rate
8=CA+; <5 '- )3*9+ 10int = 1> 0int
8*9; (805 '06)-(805 2?<) ',erti"al start of position grap$
8*9; (05 200)-(<005 200) 'position @ero line
8*9; (:05 '06)-(805 '06) ''2 in"$es to left line
8*9; (:05 2?<)-(805 2?<) ''2 in"$es to rig$t line
8=CA+; 85 '- )3*9+ 1'2 in"$1
8=CA+; ?5 '- )3*9+ 18eft1
8=CA+; ':5 '- )3*9+ 1'2 in"$1
8=CA+; '85 '- )3*9+ 13ig$t1
8*9; (805 %'0)-(805 %?0) ',erti"al start of steering grap$
8*9; (05 %70)-(<005 %70) 'steering @ero line
8*9; (:05 %'0)-(805 %'0) '/20 degrees steering
8*9; (:05 %?0)-(805 %?0) '-20 degrees steering
8=CA+; 205 '- )3*9+ 120 deg1
8=CA+; 2'5 '- )3*9+ 18eft1
8=CA+; 265 '- )3*9+ 120 deg1
8=CA+; 275 '- )3*9+ 13ig$t1
A=3 i = 0 += 27
8*9; (80 / i * 205 600)-(80 / i * 205 620)
9;B+
8=CA+; 285 '- )3*9+ 1 0 '0 se"onds
201
A=3 i = 0 += 700 'for a 27 se"ond run
*nteg = *nteg / y & 20
steeringtmp = (0steering * y / 0rate * rate / 0int * *nteg) / Bias
*A AB4(steeringtmp - steering) C 6 +D;9
steering = steering / 6 * 4E9(steeringtmp - steering)
;84;
steering = steeringtmp
;9( *A
*A AB4(steering) C 20 +D;9 steering = 20 * 4E9(steering)
angle = angle / steering & 20
rate = 4peed * 4*9(angle & 7:.%)
= / 4peed & 20
y = y / rate & 20
)4;+ (i / 805 y * 8 / 200) 'dra! position lo"ation
)4;+ (i / 805 2 * steering / %70) 'dra! steering angle
9;B+