Robot Thesis

i
2/62 OxIord Tce

Taringa Q 4068
May 23
rd
, 2003

Head oI School oI InIormation
Technology and Electrical Engineering
The University oI Queensland
St Lucia Q 4068

Dear ProI. Simon Kaplan,

As partial IulIilment oI my requirements to complete my Bachelor oI Engineering
(Electrical), I submit this thesis titled:

'Design and Optimisation oI a Control System Ior Omnidirectional Robots

All work contained in this paper, except that which is acknowledged, is my own and
has not been previously submitted in any Iorm prior to the release oI this document.

Yours sincerely

Isaac Linnett

ii

Design and Optimisation of a ControI System for
OmnidirectionaI Robots

The University oI Queensland

Department oI InIormation Technology and Electrical Engineering
Bachelor oI Electrical Engineering

Isaac Linnett

Supervisor: Dr Gordon Wyeth

Bachelor oI Engineering Thesis
23
rd
May 2003

iii
Abstract
The aim oI this thesis is to describe the soItware redesign oI the existing control
system used on an omnidirectional soccer robot. The robots used in this research
are the University oI Queensland`s RoboRoos, which compete in the
international robot soccer competition, RoboCup. The dynamic nature oI this
competition dictates that the control system must be reactive, thereby allowing
Ior rapid changes in the motion oI the robot.
The updated system is designed around a new optimal desired velocity
calculation algorithm and a supporting proportional-integral Ieedback loop to
control the motion oI the robot. The new system has increased the robot`s
maximum possible acceleration and improved the Iluidity and accuracy oI their
movements. The robot`s new control system also accounts Ior the dynamic
eIIects oI the peripheral devices, such as the kicking and ball control
mechanism.
Testing the new and old system shows that the robots are Iaster, have better
control and play the game oI soccer at a higher standard. In game conditions the
new system is able to score more goals and deIend their own goal better than the
old system.

iv
AcknowIedgements
During the course oI the completion oI this thesis many people have supported
my work, with and with out knowing. I would like to thank Jenn Ior listening to
my continuous talk about robots and all oI the things that I know bored you
senseless.
Thankyou to my supervisor Gordon, Ior allowing me to embark on this quest Ior
enlightenment and personal IulIilment, as well as letting me play with some
really great toys Ior the year.
For Dave Ior the inspiration to write parts oI this thesis and I tried as best as I
could to make them work how you wanted, but I still blame MAPS.
To the rest oI the RoboRoos crew who without there support the team would
never have moved, Chris and Jason in 2002 and Leighton, Joanna and Doug in
2003.
Thanks to all the guys and girls in the lab who made doing this thesis really
enjoyable.
Lastly thankyou to my parents Ior letting me complete my degree, it was a
wonderIul time.

v
TabIe of Contents
Abstract ........................................................................................................................ iii
Acknowledgements.......................................................................................................iv
Table oI Contents...........................................................................................................v
Chapter 1 - Introduction............................................................................................1
1.1 Research Theme.............................................................................................1
1.2 Aim................................................................................................................1
1.3 RoboCup ........................................................................................................2
1.4 The Robots.....................................................................................................2
1.5 Chapter Outline..............................................................................................4
Chapter 2 - RoboRoos System Overview.................................................................5
2.1 High Level System.........................................................................................5
2.2 Vision Recognition System (VISION) ..........................................................6
2.3 Multi-Agent Planning System (MAPS) .........................................................7
2.4 Action Execution System (AES) ...................................................................8
2.5 Navigation (NAV) .........................................................................................9
2.6 Other Sub-Systems.......................................................................................10
Chapter 3 - The Robots...........................................................................................11
3.1 Omnidrive System.......................................................................................11
3.2 Kicker Mechanism.......................................................................................12
3.3 Ball Control Device (Dribble Bar)...............................................................12
3.4 Electrical Hardware .....................................................................................14
3.5 Communications System.............................................................................14
Chapter 4 - The Control System and Related Systems ...........................................15
4.1 Bang-Bang Theory.......................................................................................15

vi
4.2 Cornell Big Red ...........................................................................................17
4.3 Motion Control oI the RoboRoos ................................................................18
4.4 Movement oI the Robot ...............................................................................19
4.4.1 Without The Ball..................................................................................19
4.4.2 With The Ball.......................................................................................20
4.5 Servo Loop...................................................................................................21
Chapter 5 - Correcting the Servo Loop...................................................................22
5.1 Correcting the FeedIoward constant ............................................................22
5.2 Correcting the PI loop..................................................................................24
5.3 Build up oI integral error .............................................................................25
5.4 Problems created..........................................................................................29
Chapter 6 - Open Loop Movement .........................................................................30
6.1 Moving in a straight line..............................................................................30
6.2 Rotational Movement...................................................................................32
6.3 Rotational and Forward Movement .............................................................34
6.4 DiIIerences between the kickplayer and other members .............................35
6.5 Accuracy ......................................................................................................36
6.6 Ramped Accelerations .................................................................................36
Chapter 7 - Motion Using Global Feedback...........................................................39
7.1 Analysis oI the old vision system................................................................39
7.2 Analysis oI the new vision system...............................................................39
7.3 Dumping Integral Error................................................................................40
7.4 Slow oscillations ..........................................................................................40
7.5 Correcting The Oscillations .........................................................................42
7.6 Further Correcting the Oscillations..............................................................44
Chapter 8 - Playing the Game.................................................................................45

vii
8.1 The Ball Control Device ..............................................................................45
8.1.1 Driving Forward with the Ball .............................................................45
8.1.2 Driving Backwards with the Ball.........................................................46
8.1.3 Rotating with the Ball ..........................................................................46
8.1.4 Translating and Rotating with the Ball ................................................46
8.2 Kicking.........................................................................................................46
8.2.1 Predict the Shoot Time.........................................................................47
8.2.2 Wait Until Stopped ..............................................................................47
8.3 Passing .........................................................................................................47
Chapter 9 - Evaluation oI the Robots......................................................................49
9.1 Simple Testing .............................................................................................49
9.2 Kicking Goals ..............................................................................................51
9.3 Kickplayer vs Goalie....................................................................................52
9.4 Playing Two on Three..................................................................................52
9.5 Playing a Match ...........................................................................................53
Chapter 10 - Conclusion .......................................................................................54
10.1 Future Work.................................................................................................54
ReIerences....................................................................................................................56
Appendix A..................................................................................................................58
Appendix B..................................................................................................................67
Appendix C..................................................................................................................80
Appendix D..................................................................................................................90

Design and Optimisation oI a Control System Ior Omnidirectional Robots
Isaac Linnett 1
Chapter 1 - Introduction
This thesis is about the design and optimisation oI a control system used on the
omnidirectional drive (or omnidrive) robots, the RoboRoos. There are two
major modules in the control system. One module creates the desired optimal
velocity trajectory and the other module uses a proportional-integral control
loop to match the actual velocity oI the robot to this trajectory. The research
undertaken in this thesis centred on creating a trajectory creation algorithm, to
better reIlect the dynamics oI the robot; and tuning the Ieedback loop Ior better
control oI the wheels.
1.1 Research Theme
A robot`s control system is responsible Ior its motion. To eIIectively control the
motion oI the robots, the system must account Ior the properties oI the
environment that it operates in. The soccer environment that this research is
based in is highly dynamic and competitive, where opponent robots are moving
at high speeds. This results in a control system that must be able to react to
constant changes in the desired motion (as commanded by higher level
intelligence modules). The perIormance oI a robot in a dynamic environment is
measured by the ability oI the robot to react to these changes at high speeds with
smooth and accurate motion. The robot`s control system must be designed and
optimised Ior high perIormance in a highly dynamic environment.
1.2 Aim
The aim oI the research undertaken is to update the control system on the
studied robots to make a system that is competitive the RoboCup competition.
To do this the system must be capable oI high accelerations, Iluid movements in
a dynamic environment, and be able to accurately control the motion. Increasing
the perIormance oI the robots motion control system will allow the system to be
competitive in the RoboCup competition |1|.
Isaac Linnett 2
1.3 RoboCup
The robots compete annually in the RoboCup F-180 league (or small sized
league) soccer competition. The goal oI the RoboCup Iederation is stated as

By the year 2050, develop a team of fully autonomous humanoid
robots that can win against the human world soccer champion
team. [1]

The F-180 league works towards this goal by developing team tactics, artiIicial
intelligence, and structuring game play methods in the domain oI robotics. The
game is very dynamic and competitive, and the robots must be Iast and agile to
keep up with the game play. For the 2003 competition, the rules have changed
to allow Ior Iully autonomous play, which means there will be no human
interaction with the robots during the twenty minutes oI game play, except Ior
the start and stop signal given by the reIeree. A Iull list oI the rules is available
through the RoboCup website |1|.

1.4 The Robots
The robots used Ior this research were the University oI Queensland`s
RoboRoos |2|. To be competitive in the competition the robots where designed
with many Ieatures, including an omnidrive system, ball control mechanism and
a high-powered kicker. Figure 1-1 shows the 1998 Roboroos playing against the
2001 RoboRoos in a match.
Isaac Linnett 3

Figure 1-1 The old (yellow markers) and the new (blue markers) RoboRoos in
a match |2|.
The existing control system used on the new robots is a modiIied version oI the
control system used on the old robots |3|. The main diIIerence is the updating
Irom the diIIerential (or wheelchair) drive system to the omnidrive system. The
system utilised on the old robots was initially used on the CuQee project, and
hence the understanding oI the existing system comes Irom examining all
previous systems.
Isaac Linnett 4
1.5 Chapter OutIine
Chapter 2 Included in this chapter are all oI the peripheral systems used by the
RoboRoos global systems to locate and determine the positions oI the robots.
Chapter 3 This chapter contains all oI the descriptions oI the robots onboard
hardware, and how it is utilised.
Chapter 4 This chapter describes the robots existing control system and how
other systems which relate to the robots
Chapter 5 This chapter explains the changes made to the IeedIoward velocity
control and the proportional-integral Ieedback loop.
Chapter 6 This chapter shows how the optimal trajectory calculations where
created without a global vision Ieedback
Chapter 7 Outlined in this chapter are the changes made to the system once
the global vision system was turned on.
Chapter 8 This chapter outlines the control and perIormance oI the peripheral
devices on the robot.
Chapter 9 Contained in this chapter is an evaluation oI perIormance oI the
new system compared to perIormance oI the old.
Chapter 10 This chapter contains a discussion oI how the goals where reached
and how well the new system behaves.

Isaac Linnett 5
Chapter 2 - RoboRoos System Overview
The topic oI this thesis is the low level control oI the robots. This Iorms a sub
optimal link in the system that has not been suIIiciently examined and resulting
in a situation where the robots are not moving to the best oI their ability. To
change this, the other RoboRoos systems are examined, as the control system is
dependent on them. Outlined in this chapter are the other parts oI the system and
how they relate to the robots control system.
2.1 High LeveI System
The higher-level code that determines the desired movements oI the robots is all
on a remote PC |2|. The PC sends this inIormation wirelessly to the robots using
a spread spectrum radio-Irequency communications module |4|. The positions
oI the robots on the Iield are determined by a global vision camera, mounted
upon a gantry above the Iield oI play |5|. This camera sends the vision signal to
the remote PC. A breakdown oI the system is illustrated in Figure 2-1.

Isaac Linnett 6

Figure 2-1 Breakdown oI the RoboRoos System.
On the global PC there are multiple systems running sequentially, these systems
include the vision recognition system (VISION), multiagent planning system
(MAPS), action execution system (AES), and the navigation system (NAV).
2.2 Vision Recognition System (VISION)
The role oI Vision is to Iind the location oI all the robots and the ball on the
Iield |2||5|. To do this, an overhead camera captures a 640x480 picture oI the
Iield and Iinds the locations oI the ball and the robots, using the yellow and blue
coloured markers located on top oI each robot. Each team is assigned either the
yellow or blue marker system prior to the beginning oI the match. The direction
oI the RoboRoos robots is determined using a line placed in Iront oI the marker.
Isaac Linnett 7
Due to the pixilisation oI the image, the robots location is Iound with an
accuracy oI Iive millimetres and its orientation is Iound with an accuracy oI two
degrees. Shown in Figure 2-2 is a captured shot oI the Iield and the robots
identiIied using the vision system.

Figure 2-2 Picture captured Irom global camera |2|.
2.3 MuIti-Agent PIanning System (MAPS)
MAPS is the highest level planning system used in the system |2||6|. The goal
oI MAPS is to examine the current state oI the Iield and assign an action to
every robot that will best IulIil the overall goal oI the team. To best implement
this, the system only evaluates the current state oI the Iield and determines the
best actions Ior every Irame. This allows Ior Iast responses because an
individual robot does not have to complete a prior action beIore starting a new
one, but instead will always have an action that has the greatest beneIit Ior the
team.
The two main goals Ior the team are to get the ball into the opponents` goal box
(score a goal), and stop the ball Irom getting into our goal box (stop the
Isaac Linnett 8
opponents Irom scoring). To determine the correct action Ior each robot, the
system looks at each robots position and velocity, and determines which robot is
best suited Ior each available action. (The goalie robot always remains the
same.) The Iour primary actions currently in use are the
Kickplayer the robot whose role is to acquire the ball, dribble to a position
and shoot.
Screenplayer used in an oIIensive position to create scoring opportunities.
Coverhigh & Coverlow these are the deIenders and used in conjunction with
the goalie robot to block our goal.
Once the robots have been assigned an action the MAPS system examines the
Iield oI play and using a series oI potential Iields determines the optimal
location Ior each robot to either move to or in the case oI the kick player kick
the ball to.
2.4 Action Execution System (AES)
The AES system determines how to best complete the actions Ior each robot
|2||6|. To do this it examines each action and breaks it down into a series oI
tasks that must be under taken beIore the action can be completed. Examples oI
these tasks may be move to a location or kick the ball.
To best describe the role oI the AES, an example oI the kickplayer robot
shooting a goal is described.
1. Move close to the ball
2. Acquire the ball
3. Dribble to location where you can shoot at the goal
4. Kick the ball
Because oI the reactive nature oI the system, each task does not have to be
completed beIore the next is Iinished. Examples are iI the robot acquires the ball
while moving towards it, skip step two; or iI you can shoot at the goal as soon
as you acquire the ball, skip step three. Ultimately a goal location and
orientation is determined Ior each robot by the AES.
Isaac Linnett 9
2.5 Navigation (NAV)
The NAV system determines how to reach the goal location oI the task |6|. Due
to obstacles between the goal location and the robot this path may not be a
straight line.
To determine the best path to a goal location the NAV system assigns a goal
location, and overlays an obstacle map, thus determining the way to reach the
goal location. The resultant direction is the shortest route to the goal location
without colliding with any obstacles. The resultant output Irom the NAV system
is a Iour dimensional vector which is sent to the robot using the communications
module. The Iour parts oI the vector are
1. Delta length (Deltal) this represents the distance oI travel in a straight line
the robot must travel.
2. Delta heading (Deltah) the direction oI travel the robot must move in.
3. Delta Iacing (DeltaI) the amount oI rotation the robot must go through to
Iace the correct direction.
4. Radius oI curvature (radius) the maximum radius oI curvature the robot can
have when moving.
The Iirst three vectors are regarded as the three primary vectors as they relate to
the Iinal desired position oI the robot, and the radius is regarded as a secondary
vector since it only describes how the robot shall reach the goal location. These
Iour vectors are shown in Figure 2-3.
Isaac Linnett 10

Figure 2-3 Break down oI Iour movement vectors. The red line represents
Deltal, green is Deltah, magenta is DeltaI and the aqua is radius. (NB in this
diagram the movement is in a straight line so the radius is inIinite).
The goalie NAV system disregards all obstacles on the Iield, so the resultant
primary vectors are always a straight line to the Iinal destination with inIinite
radius.
2.6 Other Sub-Systems
The two other sub-systems embedded in the higher-level code which have an
impact on the control system, are the tracking and predict code. The tracking
code keeps a record oI the previous positions oI all the robots on the Iield and
the ball, thereIore can calculate the velocities oI all the objects on the Iield.
These velocities are then passed through a Kalman Filter that takes the very
dynamic incoming data and creates a realistic Iiltered signal. The current
position and the Iiltered velocity is then passed to the predict code which
predicts how Iar the objects will travel in the Iuture and can thereIore predict
where the objects will be.
Isaac Linnett 11
Chapter 3 - The Robots
The robots studied where designed in 2001 by Cusack |7|, Ball |6| and Wyeth
|8| to be Iast, robust and powerIul. The three main mechanical Ieatures oI the
robots are the omnidrive system, the high-speed kicker and the ball control
device (dribble bar). The electronic hardware designed to support the
mechanical system centres around the Motorolla MC68332 |9| and the spread
spectrum communications module |4|.
3.1 Omnidrive System
The drive system on the robots consists oI six castor wheels, each mounted in
pairs with one drive direction and one Iree direction |2||10|, as shown in Figure
3-1. The three pairs are mounted 120
o
apart creating a system that can move in
any direction on a Ilat plane. Each pair oI motors is driven through a gearbox
using a MiniMotor 6V 2224 series motor |11|. These motors where chosen Ior
their size, eIIiciency and driving voltage. Each motor has a 512 count magnetic
encoder mounted |11|.

Figure 3-1 The omnidrive system |2|.

Isaac Linnett 12
3.2 Kicker Mechanism
The RoboRoos kicker is based on the crossbow principle |7|, as shown in
Figure 3-2. The strike plate is retracted using a rack and pinion device, and held
in the ready position with a pin locking into a matching slot on the rack. To
release the kicker, the pin is removed Irom the slot, and the rack, connected to
the strike plate, Iires Iorward. Only one motor is needed to control the device as
it retracts the striker plate when driven Iorward, and releases the pin when
reversed.

Figure 3-2 Pictorial view oI the kicker mechanism |2|.
There are two slots located on the rack, designed Ior 1) a soIt kick Ior passing
and 2) a hard kick Ior shooting goals. The system can lock the mechanism into
either, and move Irom soIt to hard but not hard to soIt.
3.3 BaII ControI Device (DribbIe Bar)
AIter the 2001 competition a dribble bar was designed and constructed Ior the
Iront oI the robots |2|. The aim oI the device was to place backspin on the ball
and drive it back towards the robot. The original design had a helical screw
located on the outside, intended to move the ball towards the centre oI the bar,
which is the best position Ior kicking accuracy. A revised dribbler was designed
Ior the 2003 competition, which was simpler and had no helical screw. This new
dribbler gives a smoother movement when the robot is in contact with the ball,
Isaac Linnett 13
and thereIore has better ball control. Shown in Figure 3-3 and Figure 3-4 are the
designs oI the new and old dribble bars.

Figure 3-3 Old dribble bar, with the helical screw |2|.

Figure 3-4 New dribble bar, with a smooth outer surIace.
Isaac Linnett 14
3.4 EIectricaI Hardware
The electrical hardware used in the robots is designed around the Motorolla
MC68332 microcontroller |12|. One oI the main Ieatures oI this microcontroller
is the embedded Time Processor Unit (TPU)|13| that allows Ior easy motor
PWM control and decoding oI the signals Irom the wheel encoders |11|. The
processor also required external ram, Ilash and MOSFET drivers. To drive the
wheel motors and kicker motor, a standard MOSFET H-bridge design is used,
and the dribbler motor is driven by a single MOSFET |6|.
3.5 Communications System
To communicate between the global computer and the robots, a high-speed
spread spectrum communications system |4| was developed in 2002 to increase
the speed and decrease the error rate oI the previous system |14|. The new
system interIaces to the Motorolla using a PIC16F873 microcontroller, with a
transmission speed oI 250Kbps, and a latency oI less than 8ms. The other
advantage oI the system is that multiple users can operate the same system
simultaneously, and there is little eIIect Irom noise. During the 2002
competition the system had an error rate oI less than 5.
Isaac Linnett 15
Chapter 4 - The ControI System and ReIated Systems
When the RoboRoos system was changed Irom diIIerential drive to
omnidirectional, Wyeth, Ball, and Miljkovic |2||6||10||8| Iirst implemented the
motion control schema to determine the path oI the robot Irom the three primary
vectors. The system was based around the existing control system used on the
diIIerential drive robots |3|. The new system allowed Ior simpliIication oI the
desired motion control algorithm, allowing Ior the rotational and translational
vectors to be computed as separate vectors. The motion control system used was
bases on the Bang-Bang theory.
4.1 Bang-Bang Theory
Mujtaba, in Discussion oI Trajectory Calculation Methods |15| Iirst discusses
the idea oI the bang-bang theory. The idea oI the motion is to move a robot arm
a predeIined distance, given as x, in a predeIined route. The bang-bang theory
suggests that the arm accelerates at the maximum Ior halI the distance, and then
decelerates Ior the remainder. The basis oI this motion is Irom equation E 4-1.
E 4-1 ce dis on accelerati coitv InitialJel itv FinalJelco tan 2
2 2

Using this Iormula and assuming that the rate oI acceleration, a, is equal in
magnitude but opposite in direction to the deceleration, the Iinal velocity oI the
Iirst stage (assuming initial is zero) can be calculated as shown in equation E
4-2.
E 4-2 ) 2 / ( 2 x a itv FinalJeloc
Given this, and the Iinal velocity at the end oI the cycle Ior the system is equal
to zero, and the distance traveled is known, a relationship is Iormed as shown in
equation E 4-3
E 4-3 0 ) 2 / ( 2 ) (
2 2
x a x a itv FinalJelco
This leads to a situation as pictured in Figure 4-3, Figure 4-2 and Figure 4-1.
Isaac Linnett 16

Figure 4-1 This graph depicts the acceleration oI the robot over the set time
period. The zero line is shown.

Figure 4-2 This graph depicts the velocity oI the robot arm over a set time
period.
Isaac Linnett 17

Figure 4-3 This graph depicts the distance travelled by the robot arm over a set
time period.
The greatest advantage oI this type oI motion is that the point oI change Irom
acceleration to deceleration is easily calculated. The serious downside is that
there is an instant oI inIinite acceleration diIIerential (jerk) at the beginning,
middle and end. Other trajectory calculation methods are discussed in |8| but
they rely on pre-programmed paths, and the path cannot be changed during the
motion, which is not suitable Ior the types oI motion that are used on the robots.
4.2 CorneII Big Red
The Cornell Big Red |16| team is the current world champions and thereIore the
designed system must at least outperIorm their 2002 system, in order to be
competitive Ior the 2003 competition. The robots used by the Cornell team have
an omnidirectional drive system with Iour driving wheels. This design reduces
the eIIect oI weight transIer between the wheels under high accelerations. The
2003 robots are estimated to have a top translational acceleration oI 4.5 ms
-2
and
a top translational velocity oI 1.9ms
-1
.
The greatest diIIerence between the Cornell motion system and the RoboRoos
system is the calculation oI velocities. The Cornell team calculates the desired
Isaac Linnett 18
wheel velocities on the global computer system, and relays the path to the robots
as three separate wheel velocities. The robots then match the wheel velocities
using local Ieedback Irom the wheel encoders. The 2003 robots also have
accelerometers and a gyro onboard to help with localised motion control |19|.
The path generation oI the robots uses a modiIied bang-bang theory |17|, where
the acceleration is given by
Jelcoitv ration ZeroAccele on Accelerati E 4-4
The constants u and are dependant on the motors, voltages and mechanical
characteristics oI the robot. The zero acceleration is the instantaneous
acceleration oI the robot Irom a zero velocity.
4.3 Motion ControI of the RoboRoos
From the initial vectors, and the current velocities, the robot determines whether
to accelerate, decelerate or neither. The system does this Ior the rotational vector
(Deltah) and the straight-line vector (Deltal). The robot then calculates the
distance travelled in one time Irame (running at 1kHz 1ms) and updates the
two displacement vectors. The direction oI travel is calculated Irom the DeltaI
constant, but to avoid situations where the direction oI travel rapidly changes
while travelling at speed, the direction oI travel is increased or decreased over a
period oI time to DeltaI.
Isaac Linnett 19
4.4 Movement of the Robot
The movement oI the robot is calculated Irom the three primary vectors and the
secondary vector. The movement oI the robot is reliant on wether or not the
robot has the ball.
4.4.1 Without The BaII
Once the direction oI travel, desired rotational and translational velocities have
been calculated, the wheel velocities are calculated using the equations E 4-5, E
4-6 and E 4-7. This set oI calculations assumes that the point oI rotation oI robot
is the central axis. The origin and description oI these Iormulas is Irom |10|.
rotvel desired rotational velocity
vel desired translational velocity
dir direction oI travel
w1 wheel 1 desired velocity

E 4-5 rotvel vel dir dir w )) cos( 3 ) sin( 5 . 0 ( 1
E 4-6 rotvel vel dir dir w )) cos( 3 ) sin( 5 . 0 ( 2
E 4-7 rotvel vel dir w ) sin( 3
Isaac Linnett 20
4.4.2 With The BaII
II the robot has the ball, the center oI rotation is changed Irom being the center
oI the robot, to being the ball. This was done such that when the robot rotates
with the ball, it does not lose the ball due to sideways movement oI the ball on
the dribble bar. To change the motion the updated calculations are shown in E
4-8, E 4-9and E 4-10.

w1* desired velocity oI wheel one with the ball
w2* desired velocity oI wheel two with the ball
w3* desired velocity oI wheel three with the ball

E 4-8
2 40
1 * 1

ocpd
ocpm pr rotvel
w w
E 4-9
2 40
2 * 2

ocpd
ocpm pr rotvel
w w
E 4-10
2 40
3 * 3

ocpd
ocpm pr rotvel
w w

pr pivot radius, the distance Irom the center oI the ball to the center oI the
robot
ocpm omni count per mm, the number oI encoder counts per mm oI travel
ocpd omni count per degree, the number oI encoder counts per degree oI
travel

When tested this code works well and the robot repetitively revolves around the
ball irrespective oI the Deltah, Deltal or DeltaI.
Isaac Linnett 21
4.5 Servo Loop
Once the desired velocities are set, the servo loop takes the desired velocity,
reads the actual velocity and then attempts to match them. The robot is naturally
a very under dampened system |18| and thereIore requires a Ieedback loop to be
placed around the wheels/encoders and desired velocity to create a critically
dampened system. The pre-existing system on the robots had a
proportional/integral (PI) Ieedback loop with unity gain and a unity multiplier
on the Ieed-Iorward component.

Isaac Linnett 22
Chapter 5 - Correcting the Servo Loop
The base oI the control system revolves around the servo code, which contains
the core oI the control system. The code receives the desired wheel velocities
Irom the control code and sets the wheel PWM to the desired level to correct the
wheel velocities. The complete servo.c Iile is included as appendix C.
5.1 Correcting the Feedfoward constant
The system contains a Ieed Iorward controller and a proportional-integral
Ieedback loop, as illustrated in Figure 5-1.

Figure 5-1 Low level wheel control system.
The old system assumed that the velocity was equal to the PWM, ie a PWM oI
50 (125) would equal a velocity oI 125 counts per ms. Under testing
conditions this assumption was Iound to be Ialse, and the relationship was Iound
to be closer to
E 5-1 velcoitv PWM 5 . 1
Setting a constant PWM across the motors and measuring the velocity that the
wheel reaches calculated this relationship. Figure 5-2 shows a wheel
accelerating to a Iinal velocity Ior PWM 50, and Figure 5-3 depicts the
relationship Ior multiple trials.
Isaac Linnett 23

Figure 5-2 Plot oI Velocity over time Ior a constant PWM input oI wheel 2

Figure 5-3 Plot oI velocity oI wheels against the applied PWM
Isaac Linnett 24
5.2 Correcting the PI Ioop
The system created without a Ieedback loop is underdampened |18|, as depicted
in Figure 5-4.

Figure 5-4 Plot oI desired wheel velocity and actual wheel velocity without PI
Ieedback loop.

To correct this a PI Ieedback loop is created that samples the wheels velocity
and Ieedbacks to the velocity ~ PWM controller. Equations E 5-2, E 5-3 & E
5-4 represent the new system.

Perr proportional error.
Ierr Integral error
Dvel desired wheel velocity
Vel actual wheel velocity
E 5-2 Jel Dvel Perr
E 5-3
Perr Ierr
Isaac Linnett 25
E 5-4 Ierr Perr Dvel PWM 5 . 1

To calculate the constant u, is set to zero and u is increased until the system
becomes unstable. This is recognised by erratic movement oI the wheels when
no desired velocity is placed on the system. Once u is calculated is increased
until instability is reached, then the system is considered to be critically
dampened.
5.3 BuiId up of integraI error
Integral (int) error in the system corrects the placement oI the robot, making
sure it does not get lost. Simply this means that iI the system is under running,
the int error builds up, then when it can it will over run such that it travels the
same distance as it was called Ior as shown in Figure 5-5.

Figure 5-5 The red area represents the build up oI integral error, while the
purple area where the integral area is released. Built up integral error when
accelerating is released when decelerating, resulting in the integral oI the
desired velocity being the same as the actual velocity. Over the course oI the
one metre sprint the total sum oI the integral error equals zero.
Isaac Linnett 26
Due to the old system being very underdampened, the control loop required a
large build up oI integral error to create any movement in the wheels. This was
not very beneIicial, as it required time to increase or decrease the integral error
to an amount that would power the motors at the correct velocity. Also the large
build up oI integral error would overIlow, causing the wheel to get lost and the
robot to travel a shorter distance then required, as shown in Figure 5-6 and
Figure 5-7.

Figure 5-6 Integral error oI one wheel during a one-meter sprint. The build up
oI integral error is caused by the system being overdriven and thereIore
overIlows the system.
Isaac Linnett 27

Figure 5-7 Distance travelled by one wheel during a one-meter sprint. Because
oI the overIlow oI integral error the wheel travels less distance than that
required to make the sprint.
With the system being critically damped, the system does not require the
integral error to build up, and hence does not overIlow, as shown in Figure 5-8.
This results in a system where the actual movement oI the robot is very similar
to the desired movement oI the robot as shown in Figure 5-9.
Isaac Linnett 28

Figure 5-8 Integral error oI a one-meter sprint when the system is critically
dampened.

Figure 5-9 The distance travelled by one wheel when during a one-meter
sprint when the system is critically dampened. The two lines overlap so well
that the red line is hard to see.
Isaac Linnett 29
5.4 ProbIems created
The tests used above are not indicative oI the actual movement oI the robot.
Very rarely is the robot required to move with a constant velocity, but rather the
velocity is dynamic, always accelerating or decelerating. This means the robot
has to be tested with a changing velocity. To do this we can run the robot over a
distance oI one meter in a straight line, with uniIorm acceleration, and equal but
opposite deceleration. Error! Reference source not found. shows the readout
oI one wheel Ior this trial.

Figure 5-10 Velocity proIile oI one wheel over a 2 meter sprint.
As shown the system works well Ior small velocities (0.5 ms
-1
) but as the
speed increases the system Ialls apart. This can be contributed to a lack oI
power to the wheels. To compensate Ior this, the system must be changed so at
higher velocities the acceleration is reduced.

Isaac Linnett 30
Chapter 6 - Open Loop Movement
The majority oI the movement oI the robots can be simpliIied into a series oI
unidirectional stepped movements. These movements are deIined using the Iour
constants, Deltah, DeltaI, Deltal, and radius |6|. Deltah is the amount oI rotation
required by the robot; Deltal is the length oI travel required in the direction oI
DeltaI. Radius is the minimum amount oI curvature allowed when travelling
with a Deltah. The radius variable was not studied in this thesis. The entire path
planning movement is contained in the Iiles control.c and robot.c, which are
included in appendix B and A.
6.1 Moving in a straight Iine
II the Deltah constant is set to zero the desired movement oI the robot is in a
straight line, with a direction dependant on the DeltaI constant. The original
motion oI the robot was created using a modiIied Band-Bang method |15|,
meaning the robot accelerates at a given acceleration, testing whether it can stop
in the remaining distance or iI it can continue accelerating. The system also tests
iI the robot is exceeding the maximum velocity, and iI it is, limits the robots
velocity. A breakdown oI this is shown in Figure 6-1.

Isaac Linnett 31

Figure 6-1 Breakdown oI Iorward motion control system.
The major problem with this system is the Iailure to take into consideration the
relationship between acceleration and velocity. As the robot travels Iaster, it
requires more power to keep it at that velocity, so less power is available Ior
acceleration. This gives rise to a relationship in equation E 6-1.
V
cur
the current velocity
A
av
the maximum available acceleration at V
cur
V
max
the maximum velocity
A
max
the maximum acceleration
Isaac Linnett 32
E 6-1
max
max
max
J
A J
A A
cur
av

To simpliIy this we can recognise that A
max
/V
max
is a constant, so the equation
simpliIies to equation E 6-2.
E 6-2 r J C A A
cu av

max

C a constant
In the current system using an A
max
oI 4.5ms
-2
and V
max
oI 1.5ms
-1
the C
constant equates to approximately 3, creating an easily calculated equation. To
change the motion control system the acceleration used is shown in equation E
6-3.
E 6-3 Jelcoitv eleration MaximumAcc on Accelerati 3
6.2 RotationaI Movement
The rotational movement oI the system is the same as the straight-line
movement but there is generally less distance to travel, and more power
available. This means the acceleration oI the system can be approximated as a
constant Ior the small distances travelled. When measured the acceleration oI
the rotational system at zero velocity can be as high as 6ms
-2
.
The other main diIIerence between the Iorward and rotational systems is the
ability to have negative velocities. This leads to a system where the motion
control must recognise the diIIerence between the robots spinning with a
positive or negative velocity.
Inaccuracies in the vision system when Iinding the robot create inaccuracies in
the rotational orientation oI the robot as high as two degrees |5|. These
inaccuracies mean that when a robot moves to the desired position it may not be
where it is meant to be, within the accuracy oI vision. To compensate Ior this
the robots Iinal position is clipped to within two degrees, as long as the velocity
is small enough.
The take into consideration the diIIerences between the two systems the
rotational motion control system is modiIied as shown in Figure 6-2.

Isaac Linnett 33

Figure 6-2 Breakdown oI rotational motion control system.
Isaac Linnett 34
6.3 RotationaI and Forward Movement
Under ideal situations the rotational and Iorward movement systems would be
discrete and independent, but on the robots they are very closely dependent
upon each other. While the Iorward movement system is accelerating, there is
less power Ior the rotational system, but to make it even more complex the only
way to relate the systems is on a wheel basis. To explain this, the model in
Figure 6-3 shows an example oI how the Iorces relate when we wish to undergo
rotational and translational movements simultaneously.

Figure 6-3 This shows a simpliIied map oI the Iorces present on the wheels
(numbered 1,2 and 3) oI the robots when undergoing translational and
rotational movements simultaneously.
Isaac Linnett 35
As shown in the Iorce map the rotational and translational movements are
related diIIerently Ior each wheel. Assuming both systems are setup such that at
all times the maximum amount oI Iorce is present in the system, the Iorces on
wheel three are negated, there is no translational Iorce on wheel one so there is
only the Iorce available is due to rotational, but on wheel one both Iorces are
acting in the same direction, resulting in the wheel being overdriven, which will
mean the robot will react badly, and the resulting motion being unpredictable.
To Iix this problem we go back to looking at the rotational system and how Iast
it is rotating. II we use the Iull available acceleration, the resulting angular
acceleration oI the robot is calculated to be 4591 deg*sec
-1
. This is very high
and the time taken to rotate through 180 degrees is calculated at 400 ms.
II the acceleration is reduced to 1ms
-2
or 765 deg*sec
-1
, this results in a 180
degree spin time oI just less than a second. This is still very Iast and the results
mean that there is still a lot oI Iorce still available Ior the translational
movement.
6.4 Differences between the kickpIayer and other members
The aims oI the kickplayer`s movements are to acquire the ball, dribble to a
location and shoot at the goal. This means that rotational movements are very
important to the movement oI the player and hence should be carried out Iast
and smoothly. Other team members however are simply required to get in the
road oI the ball or the opposition. This means they should translate, as Iast as
possible and the rotational movement is less oI a priority. To accommodate Ior
this, the kickplayer is programmed with a smaller than maximum translational
acceleration, and the other players are programmed with the maximum
acceleration.
When tested with this system the kickplayer would rotate and translate at the
same time, reaching the goal location with the correct heading. The other
players would move to the goal location with a greater speed but would not have
the correct heading, thereIore would have to rotate to the correct heading when
they reach the Iinal translational position.
Isaac Linnett 36
6.5 Accuracy
The accuracy oI the robot is oI great concern when pinpoint movement oI the
robot is desired. To measure the translational movement oI the robot a simple
test setting the Deltal to one meter and measuring the distance travelled without
vision updates. The system should also be checked to make sure that the integral
error does not overIlow, leading to a loss in accuracy.
To Iind the amount oI inaccuracies in the system a test was repeated many times
with a Deltah oI one meter, and with diIIerent accelerations and maximum
velocities. The robot repeatedly moved ninety percent oI the required distance.
The trial was then repeated Ior two meters and the accuracy was Iound again to
be ninety percent.
To measure the accuracy in rotational movement the robot was set to rotate
1800 degrees (5 revolutions) and the angle between the start and Iinishing point
measured. This trial was conducted multiple times with diIIerent accelerations
and maximum velocities, and it was always Iound that the robot returned to the
start point with an accuracy oI greater than ninety-nine percent.
The diIIerence in accuracy can be attributed to slip in the wheels. When the
robot translates the resistive Iorce is normal to the weight, resulting in a shiIt oI
the mass distribution over the wheels. When the robot accelerates with a zero
Deltah, the weight oI the robot is shiIted to the back wheels, and with less
weight on the Iront wheels so they can easily slip. This results in the robot
losing its position and the robot moving incorrectly. The reason when the robot
does not lose position when rotating is that the mass distribution over the wheels
does not change and the robot does not slip, thereIore there is no loss in
position.
6.6 Ramped AcceIerations
Currently the control system uses a modiIied Bang-Bang method to control the
movements oI the robot, leading to a system with an inIinite acceleration
diIIerential, or jerk. To improve the motion oI the robots a system where the
accelerations are not on or oII could be created, instead the robot should ramp
up to its maximum acceleration with a predeIined jerk. This would create a
Isaac Linnett 37
smoother movement oI the robot and may lead to a greater accuracy in the
movement.
Figure 6-4 shows how the velocity oI the robot would look iI the motion was
created with a Iinite jerk. Labelled are the key points in Table 6-1.

Figure 6-4 Rotational velocity proIile using non-inIinite jerk.

Time Point Description
t
1
1 Zero point, velocity is zero, acceleration is zero
t
2
2 Acceleration reaches maximum
t
3
3 Start oI braking, acceleration is decreased
t
4
4 Deceleration reaches maximum
t
5
5 Acceleration is increased to bring the robot to a halt
t
6
6 End point, velocity is zero, acceleration is zero
Table 6-1 Description oI non-inIinite jerk motion control.

Isaac Linnett 38
The main problem with a system like this is calculating the points where the jerk
is positive, negative or zero. At point 1, the system starts moving; thereIore it
can simply start increasing the acceleration. At points 2 and 4 the robot reaches
maximum acceleration or deceleration so the system stops increasing (or
decreasing) the acceleration. At point 6 the system should simply come to rest.
The two points that have to be calculated mathematically are point 3 and point
5. The calculations Ior these points rely on the instantaneous velocity at point 3,
the maximum acceleration and the jerk. Shown in equation E 6-4 and E 6-5 are
the distances travelled Irom point 3 to the end and point 5 to the end. (For a
complete break down oI all the equations oI motion governing the movement oI
the robots reIer to appendix D).

D
36
distance travelled Irom point 3 to point 6
D
56
distance travelled Irom point 5 to point 6
V
3
velocity at point 3
m
acc
maximum acceleration
J acceleration diIIerential (jerk)

E 6-4
J
m J
J
m
m
J
D
acc acc
acc

3
2
3 2
3
36
2
4
3
2

E 6-5
2
3
6
56
J
m
D
acc

As shown the calculation required to know at what point the robot should start
decreasing the acceleration is quite complex and requires a lot oI processing
power. Also the robot being in a very dynamic situation where it is required to
constantly change trajectories requires the system to be very stable and mobile.
This trajectory generation method is very delicate and requires large amounts oI
processor time thereIore could not be used on the robots.
Isaac Linnett 39
Chapter 7 - Motion Using GIobaI Feedback
The movement oI the robots is governed by the vision and AI system, and such
the robots perIormance should be oI the highest quality when the vision is
turned on. Measures oI the eIIectiveness oI the control systems include the
ability to quickly react to a change in the system, accuracy oI the robots and the
speed oI the robots. The ideal system would be Iast, as accurate as the vision
system and be able to react to changes in the system as Iast as they occur.
7.1 AnaIysis of the oId vision system
The previous vision system was created with each robots navigation on-board
allowing Ior the robots to have as sense oI location on the Iield. Each robot
received the coordinates oI all the robots on the Iield and the ball, and Irom that
the robot decided the appropriate action. Using this global reIerence the robot
could path integrate using Ieedback Irom the wheel encoders and thereIore
always know where it was. One draw back oI this system is the amount oI time
used in processing the data, leaving less time Ior the control loop; meaning only
simple control systems could be used.
The old control system worked well in the loop, but with the navigation being
moved oII-board the useIulness oI the path integrate system was questioned.
This lead to the conclusion that it was no longer needed in the system so the
new control system must work harder to keep the robot on track with very little
local Ieedback on the robot. The only Ieedback on the robot now being the
integral error contained in the PI Ieedback loop oI the core control system.
7.2 AnaIysis of the new vision system
The new system was designed such that a greater amount oI AI could be
perIormed and the robots could develop a higher level oI teamwork. This lead to
the problem oI what inIormation should be sent to the robots. Many ideas were
considered but the Iinal choice was made between sending the robot wheel
velocities and, sending robot displacements. The Iinal decision was chosen to be
displacements; this was made mainly Ior ease oI implementation, among others.
Isaac Linnett 40
The main problem common to both strategies is that the robot has no global
sense oI location so it does no know where it is on the Iield and thereIore cannot
know where it is going.
Now that the core control system works very well and has a low Iailure rate, the
system`s robustness means that each robot should perIorm any task sent by the
AI system, within reason. To test this, the robots perIormance is recorded during
simulations oI the match conditions and the perIormance oI robot noted.
7.3 Dumping IntegraI Error
Analysis oI the system shows that under most game conditions the Integral error
in the PI Ieedback loop remains small, so to Iurther improve the reactive nature
oI the robot, the integral error is cleared every time the system acquires a
communications packet. This creates a system that Iorgets all knowledge oI
where it has been and the motion is determinate only on where it has to go. This
system works well under trials and can react to large changes in the
displacements rapidly.
7.4 SIow osciIIations
When Iirst tested it was noted that the new higher-level intelligence system
created a slow oscillation in the robots, at approximately Iive to ten hertz. The
cause oI these oscillations was Iound to be the predict code and Kahlman Iilter
not properly calculating the predicted movement oI the robot. This predict code
works by looking at the last two Irames Irom vision, taking the distance the
robot has moved to be its velocity and adding that to the newest Irame. It does
not account Ior the Iuture accelerations and decelerations oI the robot. Shown
below in Figure 7-1 is graphical representation oI the rotational velocity oI the
robot over an approximate two second time period. It shows how the robot
oscillates, and the amount oI oscillations.
Isaac Linnett 41

Figure 7-1 A graphical representation oI the slow oscillations present when the
robot is controlled using vision. The area inside the box is shown iI Figure 7-2.

Figure 7-2 One oscillation oI the robot under vision. This shows how the
velocity and Deltah are out oI phase by 100 ms. The positions oI where one
image was grabbed Irom the camera and where the robot received the
commands Irom that Irame are shown.
Isaac Linnett 42

Figure 7-2 shows that Irom the time that vision grabs a Irame to when the robot
receives the communications, the velocity oI the robot can change by up to 30
deg/sec, which can represent a large percentage oI the velocity while the robot is
approaching a Iinal destination. The resultant is a system that greatly over
predicts the position oI the robot when it is approaching its Iinal heading.
7.5 Correcting The OsciIIations
To reduce the amount oI spin available on the robot the maximum velocity is
reduced when the robot receives a small Deltah. The result oI this is a robot that
cannot move Iar enough in the small time window oI the oscillations and instead
becomes a dampened system, instead oI an undampened system, as shown in
Figure 7-3.

Figure 7-3 This shows how the rotational velocity is clipped when Deltah is
small.
The accelerations used in the rotational system are a small percentage oI the
available accelerations. To take advantage oI this, we can change the system
such that iI we know we cannot stop in time using the current acceleration, we
Isaac Linnett 43
can increase the acceleration to a value that will allow us to stop in time. To do
this we introduce an algorithm that compares the current acceleration, velocity
and distance leIt to travel and iI we cannot stop in time, increase the
acceleration. A readout oI the resultant system is shown in Figure 7-4.

Figure 7-4 Increased rotational acceleration Ior a rotation oI 180 degrees. The
black line shows how the acceleration increases at the end oI the turn so the
robots velocity is zero when the Deltah is zero.
Isaac Linnett 44
7.6 Further Correcting the OsciIIations
Since the oscillations are created Irom a higher source, any solution used that is
on the robot can only compensate Ior oscillations, not correct them. To Iix the
oscillations the predict code in the vision system must be Iixed to take into
account the movement oI the robots aIter the communications packet has been
sent to the robot. To do this a mirror oI the control system must be created in the
vision code to interpret the how the robot will react to the sent communications
packet, ie how Iar and in what direction the robot will move depending on what
displacements are sent. This is time consuming and means the same calculations
will be repeated on and oII the robot.
To make this easier the system could be changed such that the desired velocities
could be sent to the robot, and these Ied directly back to the predict system, thus
a better predict code which should remove the oscillations.
Isaac Linnett 45
Chapter 8 - PIaying the Game
The RoboRoos where designed as soccer playing robots. To enable the robots to
eIIectively play soccer the peripheral Ieatures oI the robot must be controlled to
the best oI their ability in order to play the game well. The other Ieatures oI the
robot that enable them to play the game oI soccer are the dribble bar and kicker
mechanism. The dribble bar allows the robot to acquire the ball and control it,
while the kicker mechanism allows the robot to take shots at the goal and pass
the ball.
8.1 The BaII ControI Device
The use oI the dribble bar is to acquire the ball and control it. It does this by
placing backspin onto the ball, thereIore the ball will always move towards the
robot. The device should not have a back eIIect on the motion oI the robot, and
the ball should always remain in control. This presents a range oI criteria that
must be Iilled since the robot`s motion varies greatly when it is dribbling the
ball. The movement oI the robot can be broken down into Iour main
movements.
Driving Iorward with the ball.
Driving backwards with the ball.
Rotating with the ball.
Translating and rotating with the ball.
To eIIectively control the rotation oI the dribble bar, the power to the dribble
bar is controlled using a percentage oI the total PWM available. An example is
iI you want the dribble bar to run at halI power, the PWM is set at 50.
8.1.1 Driving Forward with the BaII
This is the simplest motion available to the robot. When the robot is accelerating
Iorward with the ball, the dribble bar can be turned oII, as the ball will always
remain in the centre grove while accelerating. When the robot starts to
decelerate the dribbler must turn on such that there is enough backspin on the
Isaac Linnett 46
ball to hold its position against the robot. The back Iorce also helps to quickly
stop the robot when straight-line movement is desired.
8.1.2 Driving Backwards with the BaII
When the robot is travelling with straight-line motion in a direction other than
directly ahead, the ball moves oII the centre oI the dribble bar and such the
power on the dribble bar must be increased to the maximum. With the power at
the maximum, the maximum reverse acceleration the robot can handle is 1.5ms
-
2
, so the higher control code is modiIied such that when the robot has the ball,
and the direction oI motion is in the reverse direction, the maximum
translational acceleration is 1.5ms
-2
.
8.1.3 Rotating with the BaII
When the robot rotates with the ball, the point oI rotation is moved Irom the
centre oI the robot to the centre oI the ball. While this is happening, only a small
amount oI backspin needs to be applied to the ball so the amount oI power
applied to the dribble bar is minimal.
8.1.4 TransIating and Rotating with the BaII
This is the most complex motion the robot Iaces when controlling the ball, and
is the most oIten used. The way the system deals with the motion is when the
amount oI rotation is large; the power applied is at a maximum, but as the angle
oI rotation decreases the amount oI applied power decreases to zero when the
rotation is zero, and straight-line movement is achieved. To increase the
perIormance oI this movement, the higher-level code is altered such that the
robot is always driving into the ball, making it easier to control the ball as the
Iorce oI the dribble bar is towards the ball.
8.2 Kicking
To react to a situation where there is an open shot at the goal, the robot has a
high-speed kicker that can propel the ball at up to 5ms
-1
. The only disadvantage
with the mechanism is that there is a 150 ms delay Irom the time the kicker is
released and the point where the ball moves away Irom the robot. The main
concern with this that in 150 ms the robot can rotate more than ten degrees, this
Isaac Linnett 47
can be the diIIerence between putting the ball in the back oI the goal and
missing completely. To prevent this Irom happening, two measures have been
tested to prevent miss kicks.
8.2.1 Predict the Shoot Time
This code watches the velocity oI the robot, and the distance leIt until the robot
rotates to the open shot at goal. When the goal is 150ms away, it releases the
kicker and the robot should Iinish the rotation and end up Iacing the goal when
the ball leaves the Iace oI the robot. This solution is riddled with problems, the
main one being the time to kick can be Irom 100ms to 200ms, so the kicker
could be released early or late. Also the robot must take into consideration the
acceleration oI the robot, which can increase greatly as the robot approaches a
Iinal destination, meaning the robot will also miss the shot at goal.
8.2.2 Wait UntiI Stopped
This is the simplest situation, and relies on the robots rotational acceleration to
be zero, or very close to zero, beIore it takes the shot at goal. This may mean the
robot waits longer beIore the shot on goal but it will have a greater accuracy,
which is oI greater concern when limited opportunities are available.
8.3 Passing
To take Iull advantage oI the Iield oI play the robots must be able to pass to one
another. This is also important in the passing challenge. When the ball is kicked
at the Iull 5ms
-1
at a Iriendly robot, the ball simply bounces oII the receivers
dribble bar and thereIore cannot accept the ball. To enable passing the robots
have a soIt kick option on the kicking mechanism, which enables the kicker to
kick the ball at an approximate 2ms
-1
.
The advantage oI kicking the ball over dribbling is that when kicked the ball
instantaneously reaches a speed oI 2ms
-1
, thereIore can travel one meter in halI a
second. When the robot dribbles the ball over a meter, it can take over a second.
The disadvantage oI kicking is that the robot on the receiving end must take
time to accept the ball, and there is the chance the ball is not accepted properly.
Isaac Linnett 48
When the robot wishes to use the soIt kick option, the kicker mechanism is
locked into the soIt kick position, then when it wishes to make a hard kick,
moves the kicker into the hard kick position and Iires, which can take time. Also
once the robot has locked the kicker into hard kick, it cannot be reset into soIt
kick until the kicker has been Iired, which Iurther complicates the passing
process.
Isaac Linnett 49
Chapter 9 - EvaIuation of the Robots
The goal oI this thesis was to improve the existing control system oI the
RoboRoos robots. To test the eIIectiveness oI the code, the robots where placed
through many exercises that where indicative oI real game conditions. The
robots where also tested against one another in a two on three competition to
test the simple game plans and the movement oI the robots in game conditions.
To give a true representation oI the eIIectiveness oI the new system the robots
competed in a Iriendly game against the University oI Melbourne soccer team,
the RooBots.
9.1 SimpIe Testing
The easiest and simplest test oI the robots is the straight-line motion test. This is
a task where the robots are required to travel halI a meter Iorward and stop. One
robot was loaded with the old system used in the 2002 world cup competition
and given the task oI moving Iorward one meter. The readout oI the how the
wheel attempted to match the actual velocity to the desired velocity is shown in
Figure 9-1.
Isaac Linnett 50

Figure 9-1 A map oI how the old control system matched the actual velocity to
the desired velocity.

Figure 9-2 The readout oI wheel one Ior a halI-meter sprint using the new
control system.
Isaac Linnett 51
To compare this to the new control code the same robot was given the new
control system and task oI moving Iorward one meter. The readout oI wheel one
is shown in Figure 9-2.
The Ieatures that are noticeably diIIerent between the two systems are the time
diIIerence to complete the task, and the large spikes in the old code when the
robot reaches a high velocity. The new code can control the actual velocity to
within the accuracy oI the encoders, but the old code Irequently loses control oI
the actual velocity at high speeds, indicated by the large spikes in the actual
velocity.
This test was repeated Ior distances greater than halI a meter, and was Iound
that Ior distances less than one meter the new system was Iaster than the old
system, but Ior distances larger than one meter the old code was quicker. The
problem with the old system is that the integral error in the old code would build
up during the acceleration and then during the deceleration would remain at the
maximum velocity Ior longer, and then brake at decelerations oI up to 7ms
-2
.
This would liIt the robot onto the Iront wheels and oscillate when it reached the
Iinal destination, which is a situation that is not desirable in the game.
The simple testing indicates that the system is Iaster Ior shorter distances, with
the same amount oI control, and is slower Ior longer distances, but has superior
control.
9.2 Kicking GoaIs
To have a good idea oI the overall eIIectiveness oI the robots control on the ball,
one robot was placed on the Iield with the ball and allowed to kick as many
goals in a ten-minute period as possible. When the old code was loaded the
robot kicked 20 goals, and the robot with the new system kicked 59 goals. The
old system mainly struggled with slow oscillations making it diIIicult to acquire
the ball, but once acquired it would accurately kick the ball into the goal halI oI
the time. The new code could eIIectively acquire the ball most oI the time, and
only miss the shots at goal approximately one third oI the time. The main
problem with both codes is that when the kicker is released the robot would
rotate slightly oII the goal direction, leading to a situation where the ball would
miss the goal.
Isaac Linnett 52
9.3 KickpIayer vs GoaIie
To test the eIIectiveness oI the Iast moving code, the goalie robot was trailed
against a kickplayer robot that was loaded with the new code. The match was
run until twenty accurate shots where made at the goal. The goalie with the new
code saved sixteen oI the goals, and the old goalie saved Iourteen. The main
diIIerence is that the old code has low speed oscillations and a sluggish
behaviour, while the new code tracks the ball better and is more reactive to the
motion oI the ball.
9.4 PIaying Two on Three
With the introduction oI the new MAPS and AES system, the robots could be
played against each other in game conditions. The game is setup such that the
robots are each assigned one oI the Iive available positions, and teams are
designated as the Kickplayer and Screenplayer played oII against the two
deIenders and the goalie. This system is mainly used to test the ball handling
skills oI the robot in an oIIensive situation, and how Iast the deIending robots
can move to a deIensive position.
The Kickplayer robot in this situation is able to hold onto the ball and pace` in
Iront oI the goalmouth without losing control. The pacing reIers to the ability to
move Irom one side oI the goal to the other while looking Ior and creating
openings in the deIence oI the deIending side. The only diIIiculty that arrises
Irom this situation is that the robot cannot always spin Iast enough to shoot at
small openings.
The deIending robots work well as a team and move well in unison to not allow
any shots at goal. The only time there are opportunities Ior scoring is when the
Screenplayer robot eIIectively jams the deIending robots and creates openings
in the deIence.
When played Ior extended times the oIIensive robots can only score at about
one goal every Iour to Iive minutes.
Isaac Linnett 53
9.5 PIaying a Match
The deIinitive test Ior the control system is how it reacts in a match. The new
systems Iirst trial match took place against the RooBots, with an incomplete
new control system. The result oI the game was six/nil to the RoboRoos. The
success oI the game can be attributed to the many improvements made to the
system during the year, including an updated MAPS, AES and NAV system.
The new control system worked well and was able to integrate well and keep up
with the new Iaster and tighter system. The major problem Iaced during the
match was the ball handling skills oI the robots, and multiple shots at the goal
where lost when the kickplayer robot lost control oI the ball while dribbling to a
kick location.
Isaac Linnett 54
Chapter 10 - ConcIusion
The Iocus oI this thesis was to design a control system that could perIorm to the
high standards required in the RoboCup competition. The new system was to be
as dynamic and reactive as the newly implemented higher intelligence system
controlling the robots.
Testing in all areas oI the game, and the game itselI show the new system
delivers a responsive controlled robot that can outperIorm the old robots in any
task. The new system is Iaster, more reactive and the robots play the game oI
soccer better than they previously did using the old system.
10.1 Future Work
To remain competitive beyond the 2003 competition new robots must be
designed which have more and better Ieatures than the current robots.
Improvements to the mechanical design oI the robots include using a Iour-wheel
drive system; better wheel design; power, pass, chip and swerve kicker
mechanisms; and a redesign oI the ball control mechanism.
To improve the overall control system, a Ieed back loop must be created in the
higher system to take into consideration the sent vectors such that the Iuture
positions oI the robots are better predicted, and choices oI Iuture movements are
better calculated.
The greatest improvement Ior the motion control system is to intertwine the
rotational and translational movements, creating a motion oI the robot that is
more eIIicient and dependant on both vectors working together. Also
introducing a system where the acceleration diIIerential is never inIinite will
improve the motion oI the robot.
To improve the eIIectiveness oI the proportional-integral Ieedback loop on the
robot, the encoders could be changed to 1024 count encoders, or higher.
Increasing the encoder count will mean the TPU on the microprocessor will
have to be clocked at a higher rate, or a new supporting electrical system will
have to be introduced to enable the robot to count all oI the encoder steps. II the
encoder count is increased, the servo loop could be run at a higher clock rate,
this would allow Ior a better control over the wheel velocity.
Isaac Linnett 55
Increasing the voltage across the wheels will increase the power available to the
robot, but this will decrease the liIe oI the motors. To avoid burn out oI the
motors, when they are run at higher voltages, the motor PWM can be clipped
during most oI the game play such that the motor only receives voltages within
its operating range. The higher voltage could be used when bursts oI power are
needed to give a competitive edge.
The code which controls the turn and shoot motion must be updated to allow Ior
taking Iaster shots at the goal. The current system does not allow Ior rapid shots
taken on goal, but iI the system where created such that the robot could spin at a
Iaster rate while it still controls the ball, the robots would be able to take
advantage oI open shots at goal, and narrow shooting lanes.

Isaac Linnett 56
References
|1| The RoboCup Federation, 2003, RoboCup OIIicial Site`,
http://www.robocup.org/, |18 May, 2003|
|2| Ball D.M., 2003 The RoboRoos: UQ`s Robot Soccer Team`
http://www.itee.uq.edu.au/~dball/roboroos/ |18 May, 2003|
|3| Ford M., 1998, Operating System and Motor Control SoItware Ior a Soccer
Playing Robot`, Bachelor oI Engineering Thesis, The University oI Queensland.
|4| Hirsch J., 2002, Robust High Speed Spread Spectrum Communication`
Bachelor oI Engineering Thesis, The University oI Queensland.
|5| Nolan C., 2002, Colour-Based Object IdentiIication and Location in a
Controlled Environment`, Bachelor oI Engineering Thesis, The University oI
Queensland.
|6| Ball D.M., 2001, Intelligence System Ior the 2001RoboRoos Team`,
Bachelor oI Engineering Thesis, The University oI Queensland.
|7| Cusack D., 2001, Mechanical Redesign oI the RoboRoos (Robot Soccer
Team)`, Bachelor oI Engineering Thesis, The University oI Queensland.
|8| Wyeth G.F., Ball D., Cusack D. and Ratnapala A., 2002, UQ RoboRoos:
Achieving Power and Agility in a Small Small Size Robot`. RoboCup-2001:
Robot Soccer World Cup V. Lecture Notes in ArtiIicial Intelligence 2377.
Springer Verlag, Berlin, 2002, pp. 603.
|10| Miljkovic, R., 2001, Control System Design Ior the RoboRoos`, Bachelor
oI Engineering Thesis, The University oI Queensland.
|11| Faulhaber, MINIMOTOR SA ~~~ Minature Drive System`,
http://www.minimotor.ch/uk, |18 May, 2003|
|12| Motorola Inc., 1990, MC68332 User`s Manual`, Databook
|13| Motorola Inc., 1996, TPU Time Processor Unit ReIerence Manual`,
Databook
|14| Kliese R., 2001 Wireless Communications Ior the Robot Soccer` Bachelor
oI Engineering Thesis, The University oI Queensland.
Isaac Linnett 57
|15| Mujtaba, M.S., 1977, Discussion oI Trajectory Calculation Methods`,
Exploratory Study oI Computer Integrated Assembly Systems, StanIord
University.
|16| DÀndrea R., 2003, RoboCup Lab RoboCup Page`,
http://robocup.mae.cornell.edu/RoboCup.html, |18 May, 2003|
|17| Kalmar-Nagy T., DÀndrea R., GangulyP., 2002, Near-Optimal Dynamic
Trajectory Generation and Control oI an Omnidirectional Vehicle`,
http://www.mae.cornell.edu/raII/Papers/nagJRR02/nagJRR02.pdI |18 May,
2003|
|18| Nise N.S., Control Systems Engineering 3
rd
Edition` John Wiley &
Sons, INC.
|19| Raghunathan P., Purwin O., Woo Lee J., DÀndrea R., The Cornell
BigRed Team - Foci Ior RoboCup 2003`,
http://robocup.mae.cornell.edu/2003long.pdI, |18 May, 2003|

Isaac Linnett 58
Appendix A
Robot.C

/*
RoboRoos 2003 - UQ's Robot Soccer Team
Copyright (C) 2003 David Ball and Dr. Gordon Wyeth
dballitee.uq.edu.au & wyethitee.uq.edu.au

This program is Iree soItware; you can redistribute it and/or modiIy
it under the terms oI the GNU General Public License as published by
the Free SoItware Foundation; either version 2 oI the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useIul,
but WITHOUT ANY WARRANTY; without even the implied warranty oI
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License Ior more details.

You should have received a copy oI the GNU General Public License
along with this program (LICENSE.txt); iI not, write to the Free SoItware
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
Also try: http://www.gnu.org.
*/
#include "robot.h"
#include "control.h"
#include "lookup.h"
#include "..\comms\unpackRF.h"
#include "..\board\robotio.h"

#iIdeI SIMUL
#include stdlib.h~
#include stdio.h~
#endiI

#iIndeI SIMUL
#include "..\board\intr.h"
#include "..\board\servo.h"
void ResetServo();
#endiI

/*
* 1.0 * COUNTPERMM 4.98 deg/ms
*
Isaac Linnett 59
*
*Speeds - acc, tacc, vel, tvel
*/
Speeds Iastspeeds (int) ((4.5 * OCOUNTPERMM) / 1000.0),
(int) ((1.0 * OCOUNTPERMM) / 1000.0),
(int) (2.0 * OCOUNTPERMM),
(int) (2.0 * OCOUNTPERMM)};

Speeds dribblespeeds (int) ((3.0 * OCOUNTPERMM) / 1000.0),
(int) ((0.7 * OCOUNTPERMM) / 1000.0),
(int) (2.0 * OCOUNTPERMM),
(int) (1.0 * OCOUNTPERMM)};

Speeds setupspeeds (int) ((2.0 * OCOUNTPERMM) / 1000.0),
(int) ((0.2 * OCOUNTPERMM) / 1000.0),
(int) (0.5I * (Iloat) OCOUNTPERMM),
(int) (0.05I * (Iloat) OCOUNTPERMM)};

Speeds haltspeeds (int) ((0.1 * OCOUNTPERMM) / 1000.0),
(int) ((0.2 * OCOUNTPERMM) / 1000.0),
(int) (0.5I * (Iloat) OCOUNTPERMM),
(int) (0.05I * (Iloat) OCOUNTPERMM)};

const int SLOWMOVEMM 50;
const int SLOWTURNDEG 20;

void SetHalt(Move *mv);
void SetMove(Move *mv, int dl, int dh, int dI, int rad);
void SetKick(Move *mv, int dl, int dh, int dI, int dk, int rad, int kicktol);
void SetSpeeds(Move *move, Speeds *spds);
int Filter(int num1, int num2, int num3, int num4);

/*
* SetMoveType -
*
* This Iunction sets the move interrupt Iunction which is in essence
* the motion control schema that will get executed once every ms until
* it is changed
*/
void SetMoveType(Move *move, int mtype);

/*DMB's
* robotInitRobot() -
*
*/
Isaac Linnett 60
int robotInitRobot(Move *move)
InitMove(move);
SetHalt(move);
SetSpeeds(move, &Iastspeeds);
return TRUE;
}

int robotRFComms2Move(commspacketstruct comms, Move *move)
short dummy;
iI (commsGetCommand(commsHALT, comms.player|move-~id|.command, &dummy))
#iIndeI SIMUL
LED2OFF();
ResetServo();
#endiI

SetHalt(move);
move-~dribbleron FALSE;
move-~kickerarm FALSE;
move-~kickerhard commsGetCommand(commsKICKHARD, comms.player|move-~id|.command, &dummy);
} else
#iIndeI SIMUL
LED2ON();
ResetServo();
#endiI

iI ( (move-~id comms.kickplayerid) && (move-~haveball) )
SetSpeeds(move, &dribblespeeds);
iI(ABS(comms.kickplayer.deltahdeg) ~ 90) // iI he has the ball reduce his max acc in the reverse
direction
move-~acc move-~acc~~1;

/*
* II the robots are setting up (eg Ior kickoII) then do it slow.
*/
iI (commsGetCommand(commsSETUPSPEEDS, comms.player|move-~id|.command, &dummy))
SetSpeeds(move, &setupspeeds);
SetKick(move, comms.kickplayer.deltalmm,
comms.kickplayer.deltaIdeg,
comms.kickplayer.deltahdeg,
comms.kickplayer.deltakdeg,
comms.kickplayer.radiusmm,
commsGetCommand(commsKICKENABLE, comms.kickplayer.command,
&dummy)); // kicktol
move-~kickerhard commsGetCommand(commsKICKHARD,
comms.kickplayer.command, &dummy);

} else
Isaac Linnett 61
iI (move-~id ! comms.kickplayerid)
iI ( (move-~id 4 /*GOALIEID*/) && (comms.player|move-~id|.deltalmm
(SLOWMOVEMM - 20)) )
move-~maxv (int) (0.1I * OCOUNTPERMM);
else iI (comms.player|move-~id|.deltalmm SLOWMOVEMM)
move-~maxv (int) (0.1I * OCOUNTPERMM);
}
iI (ABS(comms.player|move-~id|.deltaIdeg) SLOWTURNDEG)
move-~maxrotv (int) (0.02I * (Iloat) OCOUNTPERMM);
//else iI (ABS(comms.player|move-~id|.deltaIdeg) (SLOWTURNDEG * 2))
// move-~maxrotv (int) (0.05I * (Iloat) OCOUNTPERMM);

// move-~tacc (int) (0.1I * (Iloat) OCOUNTPERMM/1000);
/*
* II the robots are setting up (eg Ior kickoII) then do it slow.
*/
iI (commsGetCommand(commsSETUPSPEEDS, comms.player|move-
~id|.command, &dummy))
SetSpeeds(move, &setupspeeds);

SetMove(move, comms.player|move-~id|.deltalmm,
comms.player|move-~id|.deltaIdeg,
comms.player|move-~id|.deltahdeg,
comms.player|move-~id|.radiusmm);
move-~kickerhard commsGetCommand(commsKICKHARD,
comms.player|move-~id|.command, &dummy);
}

move-~dribbleron commsGetCommand(commsDRIBBLERON,
comms.player|move-~id|.command, &dummy);
}

// move-~kickerhard TRUE;

return TRUE;
}

#iIndeI SIMUL
commspacketstruct RFComms;

int robotMainLoop(Move *move)
move-~dribbleron TRUE;
move-~kickerhard TRUE;
iI (!InitLookup())
return FALSE;
}
Isaac Linnett 62
move-~lookupsinitialised TRUE;
move-~dribbleron FALSE;

#iI 0
// First run this
move-~dribblerstrengthpercent 0;
SetMove(move, 2000, 0, 0 ,0);
while (1);
#endiI

while (1)
iI (unpackRFGetPacket(&RFComms) COMMSOK)
LEDON();
robotRFComms2Move(RFComms, move);
LEDOFF();
}
}

// we should never exit out.
while (1)
;
}
#endiI

void SetHalt(Move *mv)

#iIndeI SIMUL
LogState(FALSE);
disable();
#endiI
mv-~mctrlschema omnihaltIn;
#iIndeI SIMUL
enable();
#endiI
}

/*
*
*
* Rad in mm
*
*/
void SetMove(Move *mv, int dl, int dh, int dI, int rad)
Isaac Linnett 63

#iIndeI SIMUL
LogState(TRUE);
disable();
#endiI
// in here is the gordo deIinition oI deltah and deltaI
mv-~Ivel mysqrt (mv-~Iacc * rad) * sqrtocountpermm;
mv-~mctrlschema omnimoveIn;
mv-~ballcentred FALSE;
mv-~deltal dl * OCOUNTPERMM;
mv-~kicktol 0;
mv-~deltah dh * OCOUNTPERDEG; // assume already appropriate clipped iI desired
mv-~deltaI (dI * OCOUNTPERDEG) - mv-~Iacing;
while (mv-~deltaI ~ ANG180COUNTS)
mv-~deltaI - ANG360COUNTS;
while (mv-~deltaI -ANG180COUNTS)
mv-~deltaI ANG360COUNTS;
#iIndeI SIMUL
enable();
//LogData(time, mv-~deltaI, mv-~deltah, mv-~deltal, mv-~deltak, 0, 0, 0, 0, 0);
#endiI
}
void SetKick(Move *mv, int dl, int dh, int dI, int dk, int rad, int kicktol)

#iIndeI SIMUL
LogState(TRUE);
disable();
#endiI
mv-~Ivel mysqrt (mv-~Iacc * rad) * sqrtocountpermm;
// mv-~Ivel (int) sqrt(mv-~Iacc * rad) * sqrtocountpermm;
mv-~kicktol kicktol * OCOUNTPERDEG;
// mv-~kicktol 40;
mv-~mctrlschema omnimoveIn;
mv-~ballcentred TRUE;
mv-~deltal dl * OCOUNTPERMM;
mv-~deltah dh * OCOUNTPERDEG; // assume already appropriate clipped iI desired
mv-~deltak dk * OCOUNTPERDEG;
mv-~deltaI (dI * OCOUNTPERDEG) - mv-~Iacing;
while (mv-~deltaI ~ ANG180COUNTS)
mv-~deltaI - ANG360COUNTS;
while (mv-~deltaI -ANG180COUNTS)
mv-~deltaI ANG360COUNTS;

#iIndeI SIMUL
enable();
#endiI
Isaac Linnett 64
}

/*
* SetSpeeds -
*
* This Iunction sets the speed limits oI movement Ior the motion control
* schemas.
*/
void SetSpeeds(Move *move, Speeds *spds)

move-~acc spds-~acc;
move-~tacc spds-~tacc;
move-~Iacc (spds-~acc * 181) ~~ 8; /* Muliplies by cos(45) */
move-~dIacc (move-~Iacc) * OCOUNTPERGORDO;
move-~maxv spds-~maxv;
move-~maxvdiII spds-~maxvdiII;
move-~maxrotv spds-~maxvdiII;
}

/*
* InitMove -
*
* This Iunction gets called beIore the system becomes operational in both
* the simulator and the real thing. It initialises the move structure
* and sets up its variables. It allocates the data structure into memory.
*
* RETURN VALUE: a pointer to the allocated memory
*/
int InitMove(Move *move)

/* Clear this Ilag so we don't use lookups beIore they are set up */
move-~lookupsinitialised FALSE;

move-~acc 0;
move-~tacc 0;
move-~maxv 0;
move-~maxvdiII 0;
move-~vel 0;
move-~veldiII 0;
move-~Iacing 0;
move-~mctrlschema omnihaltIn;
move-~time 0;
move-~hdg 0;
move-~x 0;
Isaac Linnett 65
move-~y 0;
move-~vd 0;
move-~stuck 0;
move-~vIinalsq 0;
move-~rvIinalsq 0;
move-~vdIinalsq 0;
move-~Iacc 0;
move-~deaccelerating FALSE;

move-~dl1 0;
move-~dl2 0;
move-~dl3 0;
move-~dl4 0;

move-~dh1 0;
move-~dh2 0;
move-~dh3 0;
move-~dh4 0;

move-~dI1 0;
move-~dI2 0;
move-~dI3 0;
move-~dI4 0;

#deIine KICKTOLERANCE 40 ///- mm
move-~kicktol KICKTOLERANCE;
#iIdeI SIMUL
iI (!InitLookup())
return FALSE;
}
move-~lookupsinitialised TRUE;
#endiI
return TRUE;
}
#iIndeI SIMUL
/*
* MoveDelay -
*
* This Iunction delays Ior the given number oI ms. Negatives result
* in no delay time.
*/
void MoveDelay(Move *mv, int delay)

// delay mv-~time;
// while (mv-~time delay);
Isaac Linnett 66
//NOP();
}#endiI
Isaac Linnett 67
Appendix B
Control.C
/*



*/

#include stdlib.h~
#include "rtypes.h"
#include "control.h"

#iIdeI SIMUL
#include stdio.h~
#endiI

#include "robot.h"
#include "motor.h"
#include "lookup.h"

#iIndeI SIMUL
#include "..\board\mc6833x.h"
#include "..\board\servo.h"
#endiI

/*
* Compiler Switches
* ENCFBACK - This switches the encoder Ieedback Ior the NPI on
Isaac Linnett 68
*/
#deIine ENCFBACK
#deIine OCOUNTPERGORDO4 OCOUNTPERGORDO*4

/**************************** CONSTANTS ****************************/
#deIine K256ONROOT3 ((int)((256.0 / 1.73205) 0.5))
#deIine K256ON3 ((int)((256.0 / 3.0) 0.5))
#deIine K1024ON3 ((int)((1024.0 / 3.0) 0.5))
#deIine K65536ON3 ((int)((65536.0 / 3.0) 0.5))

/* table lookups Ior trig Iunctions */
#deIine COSCOUNTS(v) COS(ClipHdg360((v) / OCOUNTPERGORDO))
#deIine SINCOUNTS(v) SIN(ClipHdg360((v) / OCOUNTPERGORDO))

#deIine SPINKICKSPEED ((int)1.0*OCOUNTPERMM)

const int onedegree ((int)2.0*OCOUNTPERDEG);
const int Iivedegperms ((int)5.0*OCOUNTPERDEG/1000);

/**************************** GLOBALS ******************************/
Move *m;

/*
* Stuck Ilags - these are located in servo.c but are not present
* in simulation
*/
#iIndeI SIMUL
extern int stuck;
extern int stuckdir;
extern int time;
#else
//extern int currenttime;
#endiI

int taccbrake;

const int wheel1|SINTABLESIZE|
111,112,114,115,117,118,119,120,121,122,123,124,125,125,126,127,127,127,128,128,128,128,128,128,128,127,127,127,126,1
25,125,124,123,122,121,120,119,118,116,115,114,112,111,109,107,105,104,102,100,98,96,93,91,89,87,84,82,79,77,74,71,69,66
,63,61,58,55,52,49,46,43,40,37,34,31,28,24,21,18,15,12,9,5,2,-1,-4,-7,-11,-14,-17,-20,-23,-27,-30,-33,-36,-39,-42,-45,-48,-51,-
54,-57,-60,-62,-65,-68,-71,-73,-76,-78,-81,-83,-86,-88,-90,-93,-95,-97,-99,-101,-103,-105,-107,-108,-110,-112,-113,-115,-116,-
117,-119,-120,-121,-122,-123,-124,-124,-125,-126,-126,-127,-127,-128,-128,-128,-128,-128,-128,-128,-127,-127,-127,-126,-
126,-125,-124,-124,-123,-122,-121,-120,-118,-117,-116,-114,-113,-111,-110,-108,-106,-105,-103,-101,-99,-97,-94,-92,-90,-88,-
85,-83,-80,-78,-75,-73,-70,-67,-65,-62,-59,-56,-53,-50,-47,-44,-41,-38,-35,-32,-29,-26,-23,-20,-17,-13,-10,-7,-4,-
1,3,6,9,12,15,19,22,25,28,31,34,37,40,43,46,49,52,55,58,61,64,67,69,72,75,77,80,82,85,87,89,92,94,96,98,100,102,104,106,108,
109,111,112,114,115,117,118,119,120,121,122};
Isaac Linnett 69

const int wheel2|SINTABLESIZE| -111,-109,-108,-106,-104,-102,-100,-98,-96,-94,-92,-89,-87,-85,-82,-80,-77,-75,-72,-
69,-67,-64,-61,-58,-55,-52,-49,-46,-43,-40,-37,-34,-31,-28,-25,-22,-19,-15,-12,-9,-6,-
3,1,4,7,10,13,17,20,23,26,29,32,35,38,41,44,47,50,53,56,59,62,65,67,70,73,75,78,80,83,85,88,90,92,94,97,99,101,103,105,106,1
08,110,111,113,114,116,117,118,120,121,122,123,124,124,125,126,126,127,127,127,128,128,128,128,128,128,128,127,127,126
,126,125,124,124,123,122,121,120,119,117,116,115,113,112,110,108,107,105,103,101,99,97,95,93,90,88,86,83,81,78,76,73,71,
68,65,62,60,57,54,51,48,45,42,39,36,33,30,27,23,20,17,14,11,7,4,1,-2,-5,-9,-12,-15,-18,-21,-24,-28,-31,-34,-37,-40,-43,-46,-49,-
52,-55,-58,-61,-63,-66,-69,-71,-74,-77,-79,-82,-84,-87,-89,-91,-93,-96,-98,-100,-102,-104,-105,-107,-109,-111,-112,-114,-115,-
116,-118,-119,-120,-121,-122,-123,-124,-125,-125,-126,-127,-127,-127,-128,-128,-128,-128,-128,-128,-128,-127,-127,-127,-
126,-125,-125,-124,-123,-122,-121,-120,-119,-118,-117,-115,-114,-112,-111,-109,-108,-106,-104,-102,-100,-98,-96,-94};

const int wheel3|SINTABLESIZE| 0,-3,-6,-10,-13,-16,-19,-22,-25,-29,-32,-35,-38,-41,-44,-47,-50,-53,-56,-59,-61,-64,-67,-
70,-72,-75,-78,-80,-83,-85,-87,-90,-92,-94,-96,-98,-100,-102,-104,-106,-108,-109,-111,-113,-114,-116,-117,-118,-119,-120,-122,-
123,-123,-124,-125,-126,-126,-127,-127,-127,-128,-128,-128,-128,-128,-128,-128,-127,-127,-126,-126,-125,-125,-124,-123,-
122,-121,-120,-119,-118,-116,-115,-113,-112,-110,-109,-107,-105,-103,-101,-99,-97,-95,-93,-91,-89,-86,-84,-81,-79,-76,-74,-71,-
68,-66,-63,-60,-57,-54,-51,-48,-45,-42,-39,-36,-33,-30,-27,-24,-21,-18,-14,-11,-8,-5,-
2,2,5,8,11,14,18,21,24,27,30,33,36,39,42,45,48,51,54,57,60,63,66,68,71,74,76,79,81,84,86,89,91,93,95,97,99,101,103,105,107,1
09,110,112,113,115,116,118,119,120,121,122,123,124,125,125,126,126,127,127,128,128,128,128,128,128,128,127,127,127,126
,126,125,124,123,123,122,120,119,118,117,116,114,113,111,109,108,106,104,102,100,98,96,94,92,90,87,85,83,80,78,75,72,70,
67,64,61,59,56,53,50,47,44,41,38,35,32,29,25,22,19,16,13,10,6,3,0,-3,-6,-10,-13,-16,-19,-22,-25,-29};

/*
* SetSpeeds -
*
* This Iunction sets the speed limits oI movement Ior the motion control
* schemas.
*/
void SetSpeeds(Move *move, Speeds *spds);

/**************************** CODE *********************************/
/*
* control -
*
* This Iunction gets called once every ms by the servo loop on the robots
* or by the simulator. It is responsible Ior setting the wheel velocities
* Ior the next servo iteration and Ior perIorming the path integration
* process. To set the wheel velocities it branches to the appropriate
* motion control schema as set by the higher navigation processes.
*/
void Control(moveptr move)

int dir;

m move;

/* Update the time variable */
#iIndeI SIMUL
m-~time time;
#endiI

iI (!move-~lookupsinitialised)
return;
Isaac Linnett 70

/* Call the motion control schema that is
currently being used */
m-~mctrlschema();

iI (m-~Iacing ~ ANG360COUNTS)
m-~Iacing - ANG360COUNTS;
else iI (m-~Iacing 0)
m-~Iacing ANG360COUNTS;

/* Calculate the heading used when calculating the wheel velocities*/
dir (m-~Iacing OCGON2) / OCOUNTPERGORDO;

/* Calculate the wheel velocities based on the direction oI travel rotation velocity*/
m-~vel1 (ROUNDSHIFTSIN(m-~vel * wheel1|dir|)) m-~rotvel;

iI (m-~ballcentred)
m-~vel1 - (m-~rotvel * INVPIVOTVELRATIO) ~~ (PIVOTSHIFT 1); // For Iacing 90 wheel 1
is halI vel
m-~vel2 - (m-~rotvel * INVPIVOTVELRATIO) ~~ (PIVOTSHIFT 1); // For Iacing 90 wheel 2
is halI vel
m-~vel3 (m-~rotvel * INVPIVOTVELRATIO) ~~ PIVOTSHIFT; // For Iacing 90 wheel 3
is -vel
}

// Isaac's dribbler control code
/*
* II we have the ball we need to account Ior all the system paramaters
* Note 1*OCOUNTPERDEG 31142
* 1*OCOUNTPERMM 27487
*/
#iI 1
iI (move-~haveball)

/* II we are heading directly Ioward and not rotating (dl dh ~ 0) and accelerating,
* turn the dribbler oII. II directly Ioward and not rotating but deaccelerating, turn the
* dribbler on enough to keep the ball in the robots grasp.
*/
iI((move-~deaccelerating) && (move-~deltal~1))

// when we are moving in a straight line, not in a Ioward dirrection and not rotating keep the speed up
else iI((ABS(move-~Iacing) ~ 2802780)&&(ABS(move-~Iacing) 8408340)) // 90deg-~270deg

Isaac Linnett 71
// on a spin and shoot we only just need the dribbler on to keep the ball in grasp
//else iI(move-~deltal 137435) // 5mm
// move-~dribblerstrengthpercent 10;

// on a turn and move we set the percentage varing dependant on the dh
else
iI(ABS(move-~deltah) ~ 2802780) // 90deg
else iI(ABS(move-~deltah) ~ 1557100) // 50 deg
else iI(ABS(move-~deltah) ~ 788550) // 25 deg
//else iI(ABS(move-~deltah) ~ 155710) // 5 deg
// move-~dribblerstrengthpercent 10;
else
}

} else
/*
* II we dont have the ball then max it out.
*/
}
#endiI

BallControlInterIace(move-~kickerarm, move-~kickerhard, move-~dribbleron, move-
~dribblerstrengthpercent);
BallControlFeedback(&move-~haveball, &move-~currentkickerstrength);

/* Set the actual wheel velocities */
OmniMotor(m-~vel1, m-~vel2, m-~vel3);
}

/*
* MOTOR CONTROL SCHEMAS
*/

/*
* omnistopIast -
*
* This schema immediately brings the robot to a halt by setting
* the platIorm velocity to 0. Never gets used, just here Ior historical reasons.
*/
void omnistopIast(void)

Isaac Linnett 72
m-~vel 0;
m-~rotvel 0;
}

/*
* omnihaltIn -
*
* This schema brings the robot to a halt graceIully by ramping down
* the robots velocity until 0 at the maximum acceleration
*/

void omnihaltIn(void)

/* Halts translational velocity */
iI (m-~vel ~ m-~acc)
m-~vel - m-~acc;
else
m-~vel 0;
/* Halts the rotational velocity */
iI (ABS(m-~rotvel) ~ m-~tacc)
iI (m-~rotvel ~ 0)
m-~rotvel - m-~tacc;
else
m-~rotvel m-~tacc;
} else
m-~rotvel 0;
}
}

/*
* This Iunction is responsible Ior the general motion oI the robot,
* controlling the accelration, deceleration, heading and Iacing
* direction oI the robot. The distance to travel, the change in heading
* and the change in Iacing are all set in the Iunction setomnimove() in
* Onschema.c
*/
void omnimoveIn(void)

int newv;
int newvsq;
int deltaIacing;
#iIndeI SIMUL
LogData(time, m-~Iacing, m-~deltah, taccbrake, m-~deltal, m-~rotvel, m-~vel, m-~acc, m-
~dribblerstrengthpercent, m-~haveball);
#endiI
Isaac Linnett 73
/*
* Reduce deltaI to zero smoothly. For capped acceleration we need to account
* the velocity oI the robot, a (v`2)/r. II you then work out the change in
* Iacing Ior the speciIied radius you get deltaIacing a * delt / v.
*/

//iI (0)
iI (m-~deltaI ! 0)
iI (m-~vel 0)
deltaIacing m-~deltaI;
} else
deltaIacing IRAD2HDG (SGN(m-~deltaI) * m-~dIacc) / m-~vel;
iI (ABS(deltaIacing) ~ ABS(m-~deltaI))
deltaIacing m-~deltaI;
}
}
m-~deltaI - deltaIacing;
m-~Iacing deltaIacing;
}

/*
* We want to reach a goal location as speciIied by the length parameter.
* At the same time the maximum speed must remain below vx so we ramp up or
* down to reach the linear velocity at amax so as to minimize the time to
* reduce length but still stay within bounds
*/

/*
* First we assume that we always want to be accelerating unless...
*/
iI (m-~deltal ~ 0)
/*
* the Iaster we go the less our acceleration so acc maxacc - maxacc * vel / maxvel
* and since maxacc / maxvel 1/300 we use acc maxacc - vel/300
*/

newv m-~vel (m-~acc - (m-~vel/300)); //new code
//newv m-~vel m-~acc; //old code

/*
* We need to divide by 16 Ior a scaling Iactor to prevent int overIlow, and by
* 2 again to compare acc*distanceleIt v`2/2 on the next line, this is where
* we decide where we should be deccelerating
*/
newvsq ((newv ~~ 2) * (newv ~~ 3));
iI ((m-~vIinalsq (m-~acc * (m-~deltal ~~ 4))) newvsq)
Isaac Linnett 74
m-~deaccelerating TRUE;
newv m-~vel - m-~acc;
} else
m-~deaccelerating FALSE;
}
/*
* this is to do with the maximum spin available in a circle, iI the direction we
* are heading is not the Ioward direction then limit the speed somehow
*/
iI ((m-~deltaI ! 0) && (m-~vel ! 0))
iI (m-~vel ~ m-~Ivel)
newv m-~vel - m-~Iacc;
else
newv m-~vel;
}

/*
* next we check iI we are in the bounds oI the predIined max velocity
*/
iI (newv ~ m-~maxv)
newv m-~maxv;

/*
* then we check Ior negative numbers which we dont want
*/
else iI (newv 0)
newv 0;
m-~deltal - newv;
iI (m-~deltal 0)
m-~deltal 0;
}
// else stop
} else
newv 0;
m-~deltal 0;
}

m-~vel newv;
//m-~vel 0;

/*
* We want to reach a desired heading as speciIied by deltah. The rotational velocity
* is ramped up to minimise the time taken to reach the desired heading
*/

Isaac Linnett 75
iI ((ABS(m-~rotvel) Iivedegperms) && (ABS(m-~deltah) onedegree))
m-~rotvel 0;
m-~deltah 0;
}

iI ((m-~rotvel ! 0) ,, (m-~deltah ! 0))

/* Attempt to align the robot with the given heading direction
* Accelerate
*/
iI (m-~deltah ~ 0)
newv m-~rotvel m-~tacc; // * ( 1 - ABS(m-~rotvel)/m-~maxrotv));
else iI (m-~deltah 0)
newv m-~rotvel - m-~tacc; //* ( 1 - ABS(m-~rotvel)/m-~maxrotv));
else
newv m-~rotvel;

/*
* We need to divide by 16 Ior a scaling Iactor to prevent int overIlow, and by
* 2 again to compare ax v`2/2 on the next line
*/
//////////////////////////////////////////////////////////////////////////////
#iI 0 // Gordo's braking code
newvsq (ABS(newv) * newv ~~ 1);
iI ((m-~tacc * m-~deltah) newvsq)
newv m-~rotvel - m-~tacc;
else
newv m-~rotvel m-~tacc;
#endiI
//////////////////////////////////////////////////////////////////////////
#iI 0 // Isaac's braking Code
newvsq ((newv * newv)~~1);
iI (ABS(m-~tacc * m-~deltah) newvsq) // iI we cannot stop using tacc, start braking

// This determines which direction to brake in
iI (m-~deltah ~ 0)
newv m-~rotvel - m-~tacc;
newv m-~rotvel m-~tacc;
else
newv m-~rotvel;
}
else // iI we can stop using tacc, dont brake and reset taccbrake to tacc
taccbrake m-~tacc;
#endiI
///////////////////////////////////////////////////////////////////////
Isaac Linnett 76
#iI 1 // Isaac's braking Code mark II
newvsq (ABS(newv * newv) ~~ 1);

iI(((m-~rotvel ~ 0) && (m-~deltah 0)) ,, ((m-~rotvel 0) && (m-~deltah ~ 0)))
// iI the rotvel and deltah have opposite signs, come down as hard as possible
taccbrake (m-~tacc~~2);

// limit the taccbrake to prediIined limits
iI (taccbrake ~ (m-~tacc2))
taccbrake (m-~tacc2);
else iI (taccbrake m-~tacc)
taccbrake m-~tacc;

iI (m-~rotvel ~ 0)
newv m-~rotvel - taccbrake;
else iI (m-~rotvel 0)
newv m-~rotvel taccbrake;
else
newv m-~rotvel;

}

else iI (ABS(m-~tacc * m-~deltah) newvsq) // iI we need to brake, brake
iI (ABS(taccbrake * m-~deltah) newvsq) // test to see iI we have to brake harder
taccbrake (m-~tacc~~2);
else
taccbrake - (m-~tacc~~2);

// limit the taccbrake to prediIined limits
iI (taccbrake ~ (m-~tacc2))
taccbrake (m-~tacc2);
else iI (taccbrake m-~tacc)
taccbrake m-~tacc;

iI (m-~deltah ~ 0)
newv m-~rotvel - taccbrake;
newv m-~rotvel taccbrake;
else
newv m-~rotvel;

}

else // iI we can stop using tacc dont brake and reset taccbrake to tacc
Isaac Linnett 77
taccbrake m-~tacc;

#endiI

// This limits the velocity oI the robot
iI (newv ~ m-~maxrotv)
newv (m-~rotvel - taccbrake);
else iI (newv -m-~maxrotv)
newv (m-~rotvel taccbrake);
}

else
newv 0;
m-~deltah 0;
}

m-~deltah - newv;
m-~Iacing - newv;
m-~rotvel newv;
m-~deltak - m-~rotvel;

// printI("m-~deltah d\n", m-~deltah);

// iI ((ABS(m-~deltak) m-~kicktol) && (m-~armkicker ! RELEASEKICKERNOW)) dmb changed 20/07/02
// IL changed 20/01/03
// iI ((ABS(m-~deltak) m-~kicktol) ,, (m-~kickerarm RELEASEKICKERNOW))
// iI (((ABS(m-~deltak)~~7) (ABS(m-~rotvel) - (m-~tacc6))) ,, (ABS(m-~deltak) m-~kicktol) ,, (m-~kickerarm
RELEASEKICKERNOW))
iI (((ABS(m-~rotvel) SPINKICKSPEED) && (ABS(m-~deltak) m-~kicktol)) ,, (m-~kickerarm
RELEASEKICKERNOW))
m-~kickerarm ARMKICKER;
// ResetServo();
} else
m-~kickerarm NOTARMKICKER;
}

}

/*
* SetMoveType -
*
* This Iunction sets the move interrupt Iunction which is in essence
Isaac Linnett 78
* the motion control schema that will get executed once every ms until
* it is changed
* SetMoveType never actually gets called, instead DMB changed it such that
* iI you want to halt you call onmihalt, and iI you want to go you write it
* directly, pretty silly (IL)
*/
void SetMoveType(Move *move, int mtype)

switch (mtype)

case OMNIHALT:
break;

case OMNIMOVE:
move-~mctrlschema omnimoveIn;
break;

deIault:
break;

}

}

/*
* ClipHdg180 -
*
* This Iunction limits delta to the angle range -ANG180 detla ANG180
*/
int ClipHdg180(int delta)

while (delta ~ ANG180)
delta - ANG360;
while (delta -ANG180)
delta ANG360;
return (delta);
}

/*
* ClipHdg90 -
Isaac Linnett 79
*
* This Iunction limits delta to the angle range -ANG90 detla ANG90
* so robot can use both Iront and back Iaces.
*/


delta - ANG180;
while (delta -ANG90)
delta ANG180;
return (delta);
}

/*
* ClipHdg360 -
*
* This Iunction limits delta to the angle range 0 detla ANG360
*/

while (delta 0)
delta ANG360;
delta - ANG360;
return (delta);
}
Isaac Linnett 80
Appendix C
Servo.C
/*
*
* TITLE: Servo.c
*
* PURPOSE: Program Ior proportional integral control oI the robot
* Adapted Irom code written by Gordon Wyeth
*
* WRITTEN BY: Russell Boyd
* Michael Stevens
* Miles Ford
* Brett Browning (BB)
* Gordon Wyeth (GW)
* David Ball (DMB)
* Isaac Linnett (IL)
*
* REVISION HISTORY:
* 13/5/98 - Rewritten Ior code integration
*
* MAJOR OMNI REVISION
* 01/08/01 - Written to operate the omnidrive -DB
*
* 2002-2002 - Rewritten to better handle the control - IL
*/
/*



Isaac Linnett 81
*/
#include "mc6833x.h"
#include "..\common\types.h"
#include "tpu.h"
#include "servo.h"
#include "sci.h"
#include "Orobot.h"

#include "robotio.h"
//#include "speaker.h"
#include "kicker.h"

#include stdlib.h~ /* Only needed Ior logging data */

/* Needed Ior robot encoder width numbers */
#include "..\common\types.h"

/* Include simulator Iiles */
#include "..\robot\rtypes.h"
#include "..\robot\control.h"

/* PI Control Constants */
#deIine FKP 1
#deIine TKP 1
#deIine FKI 1
#deIine TKI 1
#deIine FFWD 1
#deIine TFWD 1

/* Integral error saturation points. Units are mm and headings respectively */
#deIine FSTUCK (100 * OCOUNTPERMM) // ~ 27500000
#deIine FUNSTUCK (5 * OCOUNTPERMM) // ~ 5500000
#deIine TSTUCK 0x3FFFF

/* Number oI counts per ms beIore TPU might stuII up */
#deIine FASTTHRESHOLD 50

/* Biggest value oI vel beIore we assume an overIlow. */
#deIine WOVERFLOW 0x800

/**************************** GLOBALS ******************************/
long w1int, w2int, w3int;
int w1, w2, w3;
int w1pwm, w2pwm, w3pwm;
int w1vel, w2vel, w3vel;
int w1err, w2err, w3err;
Isaac Linnett 82
int time 0, Ilag 0, stuck 0;
int w1dvel 0, w2dvel 0, w3dvel 0;
int oldw1 0, oldw2 0, oldw3 0;

int stuckdir 1;

int Iorwardenable TRUE;
int rotationenable TRUE;

int Ireq 200;

/*
* PITintr -
*
* This interrupt receives the desired wheel velocities, calculates
* wheel velocity error and then calculates the required duty cycle
* to decrease the error using proportional integral control
*/

void interrupt PITintr(void)

LED2ON();

/* Clear the watchdog by writing 0x55 and 0xAA in order to soItware
* service register.
*/
SIMREGS.SWSR SIMSWSRRESET1;
SIMREGS.SWSR SIMSWSRRESET2;

time;

// Ior latency test
// iI (time 15000)
// LEDON();

/* Read encoder position count */
w1 ((int) QDECread(W1QDECPRI));

KickerController();

Isaac Linnett 83
DribblerController();

/* Call the control routine */
Control(&move);

/* Compute change in position */
w1vel w1 - oldw1;
w2vel w2 - oldw2;
w3vel w3 - oldw3;

/* II we are going to Iast, switch to Iast quad decode */
iI (w1vel ~ FASTTHRESHOLD)
FQDECsetIast(W1QDECPRI);
else iI (w1vel -FASTTHRESHOLD)
else
FQDECsetnormal(W1QDECPRI);

else

else

/*
* Test Ior overIlow. Assume max change between
* interrupts is one turn or 256.
* GFW 26/7 Changed this to 2048 and implemented a
* #deIine OVERFLOW
*/

iI (w1vel ~ WOVERFLOW)
w1vel - 0xFFFF;
else iI (w1vel -WOVERFLOW)
w1vel 0xFFFF;

Isaac Linnett 84
w2vel - 0xFFFF;
w2vel 0xFFFF;

w3vel - 0xFFFF;
w3vel 0xFFFF;

/* Convert velocities to 1/256th oI encoder count */
w1vel 8;
w2vel 8;
w3vel 8;

/* Save current position Ior next time */
oldw1 w1;
oldw2 w2;
oldw3 w3;

/* Compute wheel velocity proportional error */
w1err w1dvel - w1vel;

/* Compute wheel velocity integral error */
w1int w1err;
w2int w2err;
w3int w3err;

/* Saturate the integral error to prevent the servo windup
* This used to be important when run at 7 Irames a second because it used to be a collision
* Iail saIe, iI the robot collided with an object beIore the vision updated, it would reduce
* the intergral error so the motors did not burn out. Now that vision runs at 60 Irames a second,
* the robots should not collide as oIten and the int error has been Iixed so it should not overIlow
* as much so this should not be necessay any more. (IL)
*/

/*
stuck FALSE;
iI (ABS(w1int) ~ FSTUCK)
w1int -SGN(w1int) * FUNSTUCK;
stuck TRUE;
}
Isaac Linnett 85
stuck TRUE;
}
stuck TRUE;
}
*/

/*
* Compute virtual motor PWM values
* pwm (prop error * pint int error * kint) kprop desired velocity in counts
* Changed because wpwm , wdvel, but wpwm ~ 1.5 * wdvel (on the UQ Iield it is actually 1.6 but 1.5 is easier to
implement).
* This change reduces the int error continuously overIlowing which is a cause oI error in movement. Once
* this change was made the simple movement oI the robot was more accurate by about 10 or 20 (IL)
*/
#iI 1
w1pwm (w1err*10 w1int*10 6*w1dvel);

/* Now convert pwm values back to single encoder count scale (multiply by 256), and a Iurther 4
* Irom the because 6/4 1.5 (IL)
*/
w1pwm ~~ 10;
w2pwm ~~ 10;
w3pwm ~~ 10;

#endiI
#iI 0 //old code
w1pwm (w1err w1int w1dvel);

w1pwm ~~ 8;
w2pwm ~~ 8;
w3pwm ~~ 8;
#endiI

#iI 0
// set wheels to a predeIined value - used in testing
iI (w1dvel ~ 0)

w1pwm 0;
w2pwm 0;
Isaac Linnett 86
w3pwm 0;
}
else iI (w1dvel 0)

w1pwm 0;
w2pwm 0;
w3pwm 0;
}
else

w1pwm 0;
w2pwm 0;
w3pwm 0;
}
#endiI

/* Constrain duty cycle to limits */
w1pwm MAX(MIN(w1pwm, PWMPERIOD), -PWMPERIOD);

// LogData(time, w1vel, w1dvel, w1pwm, w2vel, w2dvel, w2pwm, w3vel, w3dvel, w3pwm);
// LogData(time, w1vel, w1dvel, w1pwm, w1dvel, w1err, w1int, w3vel, w3dvel, w3pwm);

/* Output PWM in desired direction (dir oI motor) and duty cycle */
iI (w1pwm ~ 0)
PWMset(W1PWMCHA, (ushort) (PWMPERIOD - w1pwm));
PWMset(W1PWMCHB, PWMPERIOD);
} else
PWMset(W1PWMCHA, PWMPERIOD);
PWMset(W1PWMCHB, (ushort) (PWMPERIOD w1pwm));
}
iI (w2pwm ~ 0)
} else
}
iI (w3pwm ~ 0)
} else
Isaac Linnett 87
}

LED2OFF();

}

/*
* OmniMotor -
*
* The desired velocities Irom control.c are received
*/

void OmniMotor(int vel1, int vel2, int vel3)

w1dvel vel1;
w2dvel vel2;
w3dvel vel3;
}

/*
* OmniEncoder -
*
* Sets the wheel velocities to equal that read Irom the encoders
* this used to be called in omniintergrate but is no longer called
* since it no longer exists - so just ignore this (IL)
*/
void OmniEncoder(int *vel1, int *vel2, int *vel3)

//#deIine USETRUEVEL
#iIdeI USETRUEVEL
*vel1 w1vel;
*vel2 w2vel;
*vel3 w3vel;
#else
*vel1 w1dvel;
*vel2 w2dvel;
*vel3 w3dvel;
#endiI
}

/*
* ResetServo -
*
* This Iunction resets the integral errors oI the servo loop back
Isaac Linnett 88
* to zero.
*/
void ResetServo()

w1int 0;
w2int 0;
w3int 0;
}

#deIine NUMLOGDATA 2500
int logptr;
struct logstruct
int time, w1v, w1dv, w1pwm, w2v, w2dv, w2pwm, w3v, w3dv, w3pwm;
} ;
struct logstruct *logdata;

void InitLogData()
logdata malloc (NUMLOGDATA * sizeoI (struct logstruct));
logptr 0;
}

int lognow FALSE;

void LogState(int lognowstate)

lognow lognowstate;

}

void LogData(int time, int w1v, int w1dv, int w1pwm, int w2v, int w2dv, int w2pwm, int w3v, int w3dv, int w3pwm)

iI (logptr ~ NUMLOGDATA)
//return; //one oII 2.5 seconds
logptr 0; // loop recording

iI (lognow FALSE)
return;

logdata|logptr|.time time;
logdata|logptr|.w1v w1v;
logdata|logptr|.w1dv w1dv;
logdata|logptr|.w1pwm w1pwm;
Isaac Linnett 89

logptr;

}

// does nothing
void DebugTxt(char *Imt, ...)
}
Isaac Linnett 90
Appendix D
Description oI modiIied bang-bang method using Iinite acceleration diIIerential.

Acceleration at t
1
0
Velocity at t
1
0
Displacement at t
1
0

From t
1
to t
2
jerk is positive so

Acceleration at t
2
jerk*( t
1
- t
2
) max acceleration
Velocity at t
2
jerk*( t
1
- t
2
)
2
/2
Displacement Irom t
1
to t
2
jerk*( t
1
- t
2
)
3
/6

From t
2
to t
3
jerk is zero so

Acceleration at t
3
max acc
Velocity at t
3
V
2
max acc*(t
3
- t
2
)
Displacement Irom t
2
to t
3
V
2
*(t
3
- t
2
) max acc*(t
3
- t
2
)
2
/2

From t
3
to t
4
jerk is negative so

Acceleration at t
4
max acceleration jerk(t
4
t
3
) -max acc
Velocity at t
4
V
3
max acc*( t
4
t
3
) jerk(t
4
t
3
)
2
/2
Displacement Irom t
3
to t
4
V
3
*( t
4
t
3
) max acc*( t
4
t
3
)
2
/2 jerk(t
4
t
3
)
3
/6

From t
4
to t
5
jerk is zero so

Acceleration at t
5
-max acc
Velocity at t
5
V
4
max acc*(t
5
t
4
)
Displacement Irom t
4
to t
5
V
4
*(t
5
t
4
) max acc*(t
5
t
4
)
2
/2

From t
5
to t
6
jerk is positive so

Acceleration at t6 -max acc jerk*(t
6
t
5
) zero
Isaac Linnett 91
Velocity at t6 V5 - max acc*(t
6
t
5
) jerk*(t
6
t
5
)
2
/2 zero
Displacement Irom t5 to t6 V5*(t
6
t
5
) - max acc*(t
6
t
5
)
2
/2 jerk*(t
6
t
5
)
3
/6

Robot Thesis

Hochgeladen von

Dokumentinformationen

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Robot Thesis

Hochgeladen von

Copyright:

Verfügbare Formate

i

2/62 OxIord Tce

Das könnte Ihnen auch gefallen