Control System Design for the RoboRoos

Robert Miljkovic

19th October 2001

Acknowledgements

This project could not have been the success that it was without the assistance of a number
of people.

Firstly I would like to thank my family for putting up with me throughout the year
and especially my parents for assisting me to attend RoboCup in Seattle, USA.

Special thanks must go to Gordon Wyeth, for providing guidance and assistance
throughout the year.

To my RoboRoos team members David Cusack, David Ball, Russell Kliese and
Adrian Ratnapala many thanks for your combined efforts, your efforts made every-
thing as successful as it was.

Special thanks is extended to the Electrical Mechanical workshop team of Keith,

Colin and Wayne for their brilliant construction of the robots. Their expertise, time
and dedication to the project produced a team of robots whose construction was the
envy of other teams at RoboCup.

Helen Lakidis for her unfaltering dedication when it came to sourcing components
even when we made last minute orders.

Gregg Buskey and Mark Chang for being an avenue for discussion on topics related
to the thesis.

i
 57-61 Edgewater Drive

Chambers Flat, QLD 4133

Tel (07) 3802 1140
miljkovic@ozemail.com.au

October 17, 2001

Prof. Simon Kaplan
School of Information Technology

and Electrical Engineering

University of Queensland
St Lucia, QLD 4072

Dear Professor Kaplan

In accordance with the requirements of the Bachelor of Engineering (Honours), I present

my thesis entitled “ Control System Design for the RoboRoos”. This work was performed
under the supervision of Dr Gordon Wyeth.

I declare that the work submitted in this thesis is my own work, except as acknowledged in
the text, and has not previously been submitted for a degree at the University of Queens-
land or any other institution.

Yours Sincerely

Robert Miljkovic

ii
 Abstract

This thesis describes the design and development of the control system for the University
of Queensland RoboRoos 2001 robot soccer team. This year, the robots underwent a
complete mechanical redesign. The redesign saw the introduction of a omni-directional
drive system. In order to operate the drive system in a controlled manner, a new control
system was required.
As mobile robots, such as the RoboRoos, interact with the real world, many non-ideal
characteristics and factors must be taken into account. When dealing with robot motion,
these factors include friction, stiction, hardware delays, component limitations and tol-
erances. All these factors effect the way the overall system behaves. The motor control
software was developed to improve the system response, bringing the system output closer
to the desired state.
The motor control system provides the vital link between the complex navigation software
and the low-level hardware. The role of the control system is to provide the means of
enabling the high-level intelligent software to control the hardware of the robot.
The motor control software receives the desired distance and direction in which the robot
must travel from the high-level navigation system. From these commands the motor con-
trol software calculates the required wheel velocities, outputs these velocities to the motor
and adjusts for error within the system through the implementation of proportional inte-
gral control.
RoboCup 2001 provided the perfect testing platform for our robots. The system was
placed under the most demanding of situations, international competition. The omni-
directional drive system and its control system proved capable of handling the highly
demanding competitive rigours of this level of competition.
Development of the control system is still necessary. Through testing further refinements
to the motor control code constants used in the control calculations can be made, although
the currently used constants operate satisfactorily. With the addition of a ball control
mechanism (dribbling bar), control software will be required for its operation. The ad-
dition of the dribbling bar will mean a new schema will have to be written allowing the
robots to pivot around the ball while dribbling.
Contents

1 Introduction 1
1.1 Thesis Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 RoboCup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Overview of who the RoboRoos are . . . . . . . . . . . . . . . . . . . . 4
1.4 Work completed on this thesis . . . . . . . . . . . . . . . . . . . . . . . 6
1.5 Document Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2 Literature Review and Existing Technology 9

2.1 RoboRoos 2000 Review . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1.1 Operational Overview . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2 Drive System . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.1.3 Control System . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.4 Interfaces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.2 Cornell 2000 Team . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.3 Drive Assemblies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
2.3.1 Omni-Directional Drive . . . . . . . . . . . . . . . . . . . . . . 19
2.4 Low Level Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.4.1 Open and closed loop control systems . . . . . . . . . . . . . . . 27
2.4.2 Controllers and their response . . . . . . . . . . . . . . . . . . . 28

3 Specifications 31

i
4 Drive System 35

4.1 Principles of omni-directional drive system . . . . . . . . . . . . . . . . 35

4.1.1 Omni-directional drive configuration . . . . . . . . . . . . . . . . 35
4.2 Kinematics of omni-directional drive system . . . . . . . . . . . . . . . . 38
4.3 Component Selection . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.3.1 Motors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.3.2 Encoders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
4.3.3 Gear Train . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

5 Robot Control Software 44

5.1 Reactive Navigation Schemas . . . . . . . . . . . . . . . . . . . . . . . . 44
5.2 Reactive Control Loop . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
5.3 Motion Control Routine . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
5.3.1 Motor Control Schemas . . . . . . . . . . . . . . . . . . . . . . 48

5.3.2 Path Integration . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

6 Results & Discussion 51

6.1 Further Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

7 Conclusion 54

A Complete System 58

B Mechanical Drawing of RoboRoos 2001 Drive System 60

C Reactive Control System Source Code 62

D Motion Control Routine Source Code 66

E Small Size League Rules 70

ii
List of Figures

1.1 The RoboRoos system in overview . . . . . . . . . . . . . . . . . . . . . 5

1.2 RoboRoos in action in the quarter finals of RoboCup 2001, Seattle . . . . 5

2.1 RoboRoos 2000 differential drive system[13] . . . . . . . . . . . . . . . 13

2.2 RoboRoos 2000 control model [11] . . . . . . . . . . . . . . . . . . . . 13
2.3 Reactive control loop code flow chart . . . . . . . . . . . . . . . . . . . 15
2.4 Hierarchical decomposition implemented by Cornell RoboCup team[7] . 18
2.5 Block diagram representation of trajectory generation [7] . . . . . . . . . 19
2.6 Example of a “universal wheel” . . . . . . . . . . . . . . . . . . . . . . 20
2.7 Basic orthogonal wheels operating principle . . . . . . . . . . . . . . . . 21
2.8 Longitudinal orthogonal wheel assembly . . . . . . . . . . . . . . . . . . 22
2.9 Lateral orthogonal wheel assembly . . . . . . . . . . . . . . . . . . . . . 23
2.10 Omni-directional poly roller wheel[14] . . . . . . . . . . . . . . . . . . . 26
2.11 Generic open loop system . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.12 Generic closed loop system . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.13 Response of a system to a step input . . . . . . . . . . . . . . . . . . . . 30

4.1 Delta configuration omni-directional drive . . . . . . . . . . . . . . . . . 36

4.2 Lateral orthogonal wheel assembly drive system in “Y” configuration . . 37
4.3 Kinematics of an individual wheel assembly . . . . . . . . . . . . . . . . 38
4.4 Kinematic representation of the forces generated by the omni-directional
drive system implemented in the RoboRoos 2001 robots. . . . . . . . . . 39

iii
4.5 Force provided by each wheel assembly relative to heading . . . . . . . . 40

4.6 Quadrature decoding [18] . . . . . . . . . . . . . . . . . . . . . . . . . . 42

4.7 Single stage spur gearbox on each drive assembly . . . . . . . . . . . . . 42

5.1 Example of the OM and GD integration process to produce the best head-
ing for the robot [19]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

5.2 Control model implemented on the RoboRoos 2001 robots . . . . . . . . 46

5.3 Flowchart of the control loop implemented on the RoboRoos 2001 robots 47
5.4 Flowchart of the path integration function implemented on the RoboRoos
2001 robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

A.1 Overview of the complete RoboRoos system . . . . . . . . . . . . . . . . 59

B.1 Mechanical drawing of the drive system and components on the underside
of the RoboRoos 2001 robots . . . . . . . . . . . . . . . . . . . . . . . . 61

iv
List of Tables

2.1 Mechanical capabilities of the RoboRoos 2000 robots . . . . . . . . . . . 10

3.1 Desired mechanical capabilities of the RoboRoos 2001 robots . . . . . . 33

4.1 Actual mechanical capabilities of the RoboRoos 2001 robots . . . . . . . 40

4.2 Faulhaber 2224 06 SR drive motor characteristics . . . . . . . . . . . . . 41

v
Chapter 1

Introduction

1.1 Thesis Overview

This thesis details the development of the motor control software of the University of
Queensland RoboRoos 2001 robot soccer team. The control system forms the means by
which the high-level intelligent navigation code controls the low level hardware of the
robot.

The redesign of the drive system from a differential drive configuration to a six-wheel
lateral orthogonal omni-directional drive system required a new control system. The new
mechanical capabilities of the drive system had to be realised within the software running
both on the robots and on the commanding computer.
The motor control software receives the desired distance and direction in which the robot
must travel from the high-level navigation system. From these commands the motor con-
trol software calculates the required wheel velocities, outputs these velocities to the motor
and adjusts for error within the system through the implementation of proportional inte-
gral control.

1
1.2 RoboCup

RoboCup is an annual international competition aimed at promoting the research and

development of artificial intelligence and robotic systems. The competition focuses on
the development of robotics in the areas of

Multi Agent robot planning and coordination

Pattern Recognition and real time control

Sensing Technology

Vision Systems (both global and local cameras)

Mechanical design and construction

The long-term goal of RoboCup is:

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

This statement might seem extravagant by today’s technological standards and will re-
quire an immense amount of research before it is possible. Despite the enormity of the
task and the lack of current suitable technology we must consider that it only took 50
years of development to take man from their first powered flight to landing on the moon.
Coupled with the phenomenal progress of computing technology such an outstanding goal
may be indeed achievable within this time frame.
The RoboCup World Championships consists of seven different events
i. F180, Small-Sized Soccer Robot League:

Teams of 5 small autonomous mobile robots (180mm in diameter) playing soccer on a

green-carpeted field the size of a ping-pong table. A golf ball is used as a substitute for a
soccer ball. A camera is located on top of the field and and provides the input to the robot’s
global vision system. Local cameras can be mounted on the robots. Communications
between the autonomous commanding PC and the robots is via a wireless FM radio link.

2
ii. F2000, Medium-Sized Soccer Robot League:




Teams of up to four robots with an area of no more than compete on a five
metre by nine metre field. The field is carpeted and has markings similar to those on a
real soccer field. The robots are fully autonomous with local vision processing. Wireless
communications is permitted between the robots only. The F2000 robots play with a FIFA
approved bright orange indoor soccer ball.
iii. Sony Legged Robot League:
Teams of up to three Sony entertainment (AIBO) robots play each other on a green-
carpeted field with sloping sides. The corners of the fields are marked with different
coloured cylinders so the robots know where they are on the field. The robots play using
a bright orange plastic ball.
iv. Soccer Simulation League:
The soccer simulation event consists of a series of scientific evaluations, competitions, and
research demonstrations, of artificial intelligence (AI) and multi-agent systems (MAS)
technology. All events utilise the RoboCup Soccer Server as the domain in which agents
are to operate, in essence agreeing on a standard test-bed for experimentation. [2] Soccer
Server consists of 2 programs, ‘soccerserver’ and ‘soccermonitor’. ‘soccerserver’ is a
field simulator that calculates movements of players and a ball. ‘soccermonitor’ is a
window interface that displays a window of the field on an X-window. [3]
v. RoboCup Rescue Simulation:
A distributed computer simulation environment of earthquake disasters including maps
has been prepared by the organiser, where buildings collapse, streets are blocked, fire
spreads, and traffic conditions are affected by a seismic reaction. Intelligent action brigades
(agents) of software try to minimise the disaster damage in this virtual space. Rescue par-
ties save victims from the destroyed buildings, fire-fighter companies extinguish the fire,
and police parties open the blocked roads.
vi. RoboCup Rescue Robot League:
The league consists of robots that are either tele-operated, share autonomy or are fully
autonomous that search for human bodies within the remains of a building efficiently and
accurately. The field consists of a real sized destroyed building of three layers with each
layer of the building focusing on different situations that might be encountered in the
field including traversing obstacles, negotiating rubble and overcoming difficult sensory
situations.

3
vii. RoboCup Junior League:

RoboCup junior is a league set up for primary and high school students. It consists of
3 separate challenges within the league: soccer, rescue and dance. The soccer challenge
consists of robots constructed from Lego that play a 2-on-2 soccer game on a field which
is colour coded in shades of gray. The theme of this years rescue challenge was that the
robots were required to rescue humans from the roof of a burning building. To find the
victims, the robots had to follow a black line on the playing field until they reached their
targets. The dance challenge consists of one or more robots that wear costumes and move
creatively to music.

1.3 Overview of who the RoboRoos are

The University of Queensland RoboRoos team consists of five highly specialised soccer-
playing robots. This team of robots were designed with the primary objective of compet-
ing in and winning RoboCup 2001.
Our team consists of mobile robots designed to be highly agile and powerful both in
defence and attack. The redesigned robots are a mechanically superior robot featuring
a six-wheel orthogonal omni-directional drive system, consisting of three independently
controlled drive units. Each drive unit consists of a bi-directional DC motor driving the
wheels through a simple gearbox. Built into each of the drive units are high-resolution
magnetic encoders to provide motion feedback to the onboard Motorola 68332-controller
board.

The controller performs the motor control and plans the path taken by the robots based
on the information received from the central controlling PC via the 418/433 MHz radio
link. The central controlling PC, using a frame grabber card, collects images from the
overhead camera and processes the images to facilitate the generation of team strategies.
Figure 1.1 gives an overview of the entire system. For a photo of the actual system the
system that constitutes the RoboRoos 2001 robot soccer team refer to Appendix A.
The RoboRoos team competed in the F-180 small size league of RoboCup 2001 held in
Seattle, USA in August. The small sized league consists of teams of 4 field robots and
a goalkeeper playing soccer on a field the size of a ping-pong table. The small robots
use an orange golf ball as a substitute for a soccer ball. The rules of the league stipulate
that the robot must fit within a cylinder of 180mm diameter and be of a height no more

4
 Off − Board Computer
Overhead
Camera
MAPS
Vision Comms
Planner

FM Radio Link

Robot Navigation
Comms
H/W Module

On−Board Robots (x5)

Figure 1.1: The RoboRoos system in overview

than 150 mm. Aside from this size restriction the mobile robots play by rules very similar
to those stipulated by FIFA. A full listing of the small size league rules are included in
Appendix E. The photograph included in figure 1.2 shows the RoboRoos team in action
in the quarter finals of RoboCup 2001, Seattle USA.

Figure 1.2: RoboRoos in action in the quarter finals of RoboCup 2001, Seattle

5
1.4 Work completed on this thesis

The work completed within this thesis has included mechanical design work, component
selection and the writing the control software that bring all the physical aspects of the
robots drive system into operational harmony.

Drive System Design and Motor Selection

The mechanical redesign of the robots was undertaken to greatly improve the mechanical
performance characteristics of the robots, in particular in terms of their agility and ma-
noeuvrability. In order to be agile, the mechanical design of the robot required the centre
of mass of the robot to be as low as possible. As most of the robots weight was contained
within the drive system and the main chassis, the intention was to design the robots with
profiles as low as possible.
With the intention of designing low profile, agile, manoeuvrable robots we set about se-
lecting a drive system. Once the design had been finalised as being the six-wheel lateral
omni-directional drive system we set about selecting components with which to construct
this drive system design. The components, which controlled the drive systems perfor-
mance, included the motors, gear train and encoders. The motors, gear train and encoders
used in the final design of the drive system were selected such to maximise the continuous
acceleration and continuous velocity of the robots. When designing for agility and ma-
noeuvrability it was decided that these two factors were the most important to maximise.

Kinematic Calculations

With the drive system designed, the next task was to develop a system of controlling this
new drive system. To enable control of the drive system a series of mathematical equations
describing the drive’s operation were required. The equations described the kinematic
relationship between the robot’s wheel velocities and the overall platform velocity. The
equations were first derived such that with a robot platform velocity, the wheel velocities
could be calculated. The inverse kinematic equations were then derived to allow the robot
platform velocity to be calculated from individual wheel velocities.

6
Design of the Reactive Control System

The robust operation of the reactive control loop implemented in the RoboRoos 2000
team convinced us to modify the existing control loop to control the new omni-directional
drive. In the differential drive configuration the control process was performed on both the
forward translational velocity and the turning velocity. An inherent property of the omni-
directional drive is that both the translational and rotational components of the robots
motion are fully decoupled and are derived from the relationship between the wheel ve-
locities. This means that we have to perform control on the individual wheel velocities
only and resolve the issue of translation and rotation control higher in the navigation
software at a schema level. Therefore the major changes within the control code were
to change the control loop to operate on the wheel velocities and to remove control of
forward and turning velocities.

Motor Control Schemas

With the relationship between the robot’s wheel velocity and the platform velocity de-
scribed by the kinematic equations, the task of developing code that would produce robot
motion was undertaken. This code connects the navigational software with the mechan-
ical structure of the robot, effectively providing the means of communication between
the intelligent software and the mechanical hardware. The first task was to establish the
parameters that would be required from the navigation code in order to control the robot’s
motion in the desired manner. Once parameters that the navigation code would pass had
been established the kinematic equations could be used to calculate the desired wheel
velocities to achieve the desired translational and rotational velocities. Once the desired
wheel velocities had been calculated the wheel velocities are passed to the reactive control
system.

7
1.5 Document Overview

Chapter 2 reviews the existing RoboRoos 2000 robot soccer team concentrating in par-
ticular on the drive system and the control software implemented in the robots. A review
of the winning team of RoboCup 2000, Cornell, is also presented. The potential drive
systems are examined with a detailed explanation of the operation of the drive system that
was selected. This is followed by a review of control theory illustrating what concepts
could have been implemented in the new control system.
Chapter 3 discusses the aims of the thesis. The chapter details the specifications of the
desired design.
Chapter 4 details the drive system that was implemented in the RoboRoos 2001 robot
soccer team. The kinematics and the dynamics of the system are described in detail. The
kinematic equations that formed a critical part of the control code are also derived. The
selection criteria used in selecting the drive motors, gears and encoders are also detailed.
Chapter 5 details the control code that was implemented aboard the robots. Firstly an
overview of the navigation code is given. The path planning process of the navigation
is discussed. The reactive control loop is described detailing the control model used, the
control calculations performed and the form of compensation implemented. The motion
control system is then explained in detail, describing the motor control schemas, path
integrations and the generation of the wheel velocities.
Chapter 6 discusses the performance of the RoboRoos team in the RoboCup world cup.
The overall performance of the RoboRoos 2001 drive system and control system are com-
pared to the specifications that we set out to achieve. Further work that needs to be com-
pleted on this thesis is also detailed.
Chapter 7 concludes the thesis by reviewing what has been achieved with the design of
the control system.

8
Chapter 2

Literature Review and Existing

Technology

Before we can begin creating a new design for the RoboRoos 2001 we must first anal-
yse the RoboRoos 2000 team and the team that won the competition last year to de-
termine where our redesign efforts should focus. From the RoboRoos 2000 review we
must identify which aspects of the design should be retained and what should undergo
redesigned. In addition to reviewing last year’s team we must analyse last years winner,
Cornell RoboCup team, to identify the standard that must be exceeded. Once the stan-
dards have been established a review of the possible technology that we can implement is
also outlined.

2.1 RoboRoos 2000 Review

To gain an insight into the operation of the RoboRoos system, a brief overview of each of
the system components will be made. Figure 1.1 illustrates how each of the components
interact to form the complete system that is the RoboRoos. The components most relevant
to this thesis are discussed detailing the platform from which the RoboRoos 2001 control
system was created.

9
2.1.1 Operational Overview

Mechanical Design

The mechanical design of the field robots in the RoboRoos 2000 team was optimised
for speed, acceleration and cornering ability, while maintaining as broad a front face
as possible for contact with the ball. The drive system for the robot was arranged in a
differential drive configuration. The two wheels were mounted on the sides of the robots
with the motors directly coupled to the wheels via spur gearboxes. Teflon skids were
placed at the front and rear of the robot to provide the third point of contact. The tyres
were made of soft rubber providing a high coefficient of friction to minimise slip during
acceleration. The table below outlines the mechanical capabilities of the robots.

  )

Continous Peak


Acceleration ( 1.5 3.6
Speed ( 2 3.4
Time over half field (s) 1.33 0.86
Table 2.1: Mechanical capabilities of the RoboRoos 2000 robots

Electronic Design

The electronics of the robots are housed on two boards. The main board contains the
CPU, associated memory and the power electronics for the control of the motors. The RF
communication electronics of the robots are housed on a separate shielded board.
The robots are controlled by a Motorola 68332 microcontroller, featuring 16 x 16 bit timer
channels supporting PWM and quadrature capture for the motor drive. Optical encoders
are mounted on the wheel shafts to provide feedback on wheel velocities.
Each robot is power by custom arranged Nickel Metal Hydride battery packs, which have
the capacity to power the robots for at least 20 minutes under strenuous playing condi-
tions. The motor are driven by H-bridges, with gate drivers providing the level shifting
required by the MOSFETs, which act as switches within the H-bridge. The MOSFETs
are complimentary N and P channel devices selected such that the system is rated for
continuous operation under motor stall conditions ensuring reliable motor control in all
situations.

10
On-Board Software

The software that runs on-board the robots consists of two threads - a cognitive level
thread and a schema level thread. The schema level thread is triggered every millisecond
and is responsible for the moment to moment navigation of the robot dealing with prob-
lem, such a achieving smooth acceleration. The processes at this level are called schemas.
The cognitive level of the software is responsible for selecting the appropriate schema at
any given time. The cognitive level maintains the planning resources such as path to goal
locations.
Schemas

The primary schemas are traverse and align, which are responsible for the translational
and rotational control of the robots. Each schema has a length parameter associated with
it to ensure that if communications fail the robot will halt once the current move is com-
pleted. The length parameter of a move is determined by encoder feedback. In the case
of new information arriving during an operation the length parameter can be modified or
the schema changed in a smooth transition.
The schema level of operations is also responsible for keeping track of the motion of
the robots to provide a current estimate of position. This process of path integration
provides the robot with a current and accurate measure of its position and heading. The
inherent delay in the system between the event and the receipt of that event on the robot
is minimised by updating beyond the delay using information from the path integrator.

Cognitive Level
The cognitive level spends most of its time calculating how to get to the goal locations
that are specified in communications received from the planning system. The decisions
made at the cognitive level are tactical commands based on a grid representation of the
field. The field is divided into 90 mm grid units, which roughly divides the field into a
30 x 16 grid of rectangles. The data received from the PC planning system details the
obstacles that are added to the grid to represent the current playing conditions. Using this
representation of the current playing conditions the software determines the path the robot
will take to move it the goal location.

11
Vision System

The vision system receives input from the colour CCD camera mounted overhead. The
camera interfaces with a vision frame grabber residing in a Pentium III 450MHz PC
that performs all the vision processing as well as generating the multi-agent strategy for
the team. The primary task of the vision system is to locate, identify and track each of
the objects of the field. Coloured balls and pattern markings on the robots are used to
determine which team the robots were on and the heading of the robots.

MAPS Planner

The planner provides strategic commands to each of the robots based on an assessment
of the current situation. The two commands that are sent to the robots by the planner are
GOTO and KICK. Each of the commands contain parameters that indicate to the robot
the location to which the KICK or GOTO command should be executed.

2.1.2 Drive System

The RoboRoos 2000 chassis that contains the drive system is of an elongated design
chosen such to provide a large surface area on both the front and back kicking surfaces
of the robots[11]. The shape and placement of the wheels has a major effect on the
turning torque that is generated by the robot. To maximise the turning torque of the
robots, the turning diameter should be as large as possible. This is achieved by placing
the motors along the longest axis in a differential drive configuration. Figure 2.1 outlines
the mechanical design of the drive system.
The drive was designed with the centre of gravity as low as possible to maximise the
amount of traction between the motors and the surface of the field allowing for higher
acceleration while maintaining complete control over the robot. The wheels used on the
drive system were small drag racing wheel, 30mm in diameter, and made of soft rubber to
ensure a high coefficient of friction for maximum traction. The motors used in the drive
system were type 2230 6V DC-Micromotors from Minimotor. The gearheads attached to
the motors were type 22/2K spur gearheads giving a 3.1:1 reduction ratio.

12
 Encoder
Motor/Gearbox

Wheel Motor/Gearbox

Encoder

Wheel

Figure 2.1: RoboRoos 2000 differential drive system[13]

2.1.3 Control System

The robot control system implemented in the RoboRoos 2000 team utilises proportional
integral control as well as feed forward compensation. The code used for the PI control
was originally written by Gordon Wyeth for use on CUQEE III, the world champion
micro mouse. This code was altered to suit the hardware that drives the RoboRoos. The
fundamental characteristics of the code and the control system remain the same in both
applications. Figure 2.2 illustrates the control model that is implemented in the robot
control code.
ldvel + fdvel + ferr + lvel
Σ Σ F(s) Σ
+ − −
MOTORS
− +
tdvel + terr rvel
rdvel + Σ Σ T(s) + Σ
−
Desired Values

Σ +
tvel rvel
−
+ SENSORS
Σ +
fvel lvel

Actual values

Figure 2.2: RoboRoos 2000 control model [11]

13
Figure 2.3 illustrates how the control system model of figure 2.1 is implemented as a
reactive control loop within the software that resides aboard the robots. The reactive
control loop is initiated every millisecond by the Periodic Timer Interrupt (PIT) and is
responsible for the generation of wheel velocities and the control of the wheel velocities.

14
 PIT Increment
Interrupt Timer

Read encoder
position count

Call control
routine

Calculate
change in
position

Test for yes Correct for

overflow overflow

no

Calculate desired
forward and turn
velocites

Calculate forward
and turn
proportional error

Calculate
forward and turn
integral error

check if stuck Stuck flag

yes true

no
Calculate
forward and turn
PWM values

Calculate right
and left wheel
PWM values

Check duty Set PWM value

cycle limits yes to maximum

no

Output PWM
values for each
channel

Figure 2.3: Reactive control loop code flow chart

15
The calculations used to determine the output of the control system will now be investi-
gated. In order to calculate the proportional and integral errors the right and left wheel
velocities are coupled and converted to forward and turn velocities. The forward and turn
velocities are given by:

Forward Velocity = right velocity + left velocity

Turn Velocity = right velocity - left velocity

The velocity proportional errors are both calculated as shown:

Forward Error = forward desired velocity - forward actual velocity

Turn Error = turn desired velocity - turn actual velocity

The forward and turn velocity integral error calculations are:

Forward Integral Error = forward proportional error

Turn Integral Error = turn proportional error

Using the above, the desired forward and turn motor Pulse Width Modulation (PWM)
values that we wish to output are calculated. The PWM values are calculated as below.

PWM = (proportional error + (integral error x integral constant)) x propor-

tional constant + (desired velocity x feedforward constant)

From the forward and turn PWM values the right and left PWM values are calculated as
below.

Right PWM = forward PWM + turn PWM

Left PWM = forward PWM - turn PWM

These PWM values are then passed to the motors via the appropriate TPU channels.

16
2.1.4 Interfaces

The main control loop on the robots operates within the main thread (navigate) and deter-
mines what the robot should be doing based on the data frames received, via the wireless
RF link, which contain environment and command updates. A set of interacting navi-
gation schemas calculate the command specific strategies that control the robot’s motion.
These navigation schemas control and implement the path planning of the robot by calling
motor control schemas.
Every millisecond a Timer Processor Unit (TPU) interrupt suspends the reactive naviga-
tion schemas and enables the current motor schema. The schemas use encoder feedback
to ensure that the robot is performing its required task.
The motor schemas include:

Halt - brings the robot to a halt

Align - rotates the robot by a heading of length mm

Traverse - moves the robot forward by length mm

Curve - given a required velocity and a heading the robot traverses a distance of
length mm. Hence, this results in the robot travelling a curved path and arriving at
the provided polar location as quickly as possible within the given constraints.[10]

The speeds that are set in the motor control schemas are received by the reactive control
system. This system runs within the same thread as the motor control schemas and drives
the motors using Pulse Width Modulation (PWM). The reactive control system ensures
that the motors actually drive at the desired speed using feedback from the encoders.

2.2 Cornell 2000 Team

In order to be competitive in this year’s competition a review of the technology utilised

by last years winner is necessary. The winner of RoboCup 2000 F180 league was Cor-
nell RoboCup team. This team brought to the competition two new electro-mechanical
innovations, these being the omni-directional drive system and a dribbling mechanism. It
was evident from reviews of their performance that the omni-directional drive system gave
Cornell a superior advantage in manoeuvrability. The arrangement of the omni-directional

17
drive system allows the robot to control its translational and rotational velocities indepen-
dently by controlling the drive assembly velocities.
With the mechanical advantages of the drive determined it was a case of devising control
strategies that would control the drive system in such a way that the team could max-
imise the omni-directional drive’s advantage. The key system concept behind the control
strategy employed by the Cornell RoboCup team was that of hierarchical decomposition.
Figure 2.4 illustrates the main idea of the concept.

DESIRED FINAL POSITION AND

VELOCITIES, TIME TO TARGET DESIRED VELOCITIES

TRAJECTORY LOCAL
STRATEGY
GENERATION CONTROL
FEASIBILITY OF REQUESTS

Figure 2.4: Hierarchical decomposition implemented by Cornell RoboCup team[7]

The Strategy block of the control system is responsible for the implementation of strategy
algorithms. The strategy block deals with prediction, including obstacle avoidance and
implements the play-based strategy that the team has adopted.

The Trajectory Generation block of the control system is responsible for calculating the
future robot velocity profile required to reach the final position with the desired final
velocity in either the shortest amount of time or with minimal control. A similar process
is used with the robot orientation. This data is then used to calculate the wheel velocities,
which are sent to the robot via wireless communication.
The only aspect of the control strategies that are executed on the robot are local control
loops designed to regulate the robot’s wheel velocities to the desired wheel velocities.
Figure 2.5 illustrates in more detail the overall trajectory and control scheme for one
robot adopted by the Cornell RoboCup team. The overall system feedback is achieved by
solving the optimal control problem for every robot at every frame.

2.3 Drive Assemblies

One of the primary objectives of the mechanical redesign of the RoboRoos was to design
robots that were more manoeuvrable and agile than the RoboRoos 2000 team. In order
to do this we had to investigate the drive systems that were available for incorporation

18
 Actual position and
ROBOT orientation of robot VISION
Desired wheel velocities

Processed position and

\orientation of robot
Voltage Velocity
of motors
LOCAL
Desired final
CONTROL
position, velocity

Predicted
position,
CONVERSION TO Desired
velocity TRAJECTORY orientation
DESIRED WHEEL PREDICTION
VELOCITIES GENERATION

Desired angular velocity ORIENTATION

CONTROL

Desired Orientation

Figure 2.5: Block diagram representation of trajectory generation [7]

into our new design. Each of the drive system configurations that were investigated are
described below with particular reference made to the agility and manoeuvrability of the
design.

2.3.1 Omni-Directional Drive

The superior manoeuvrability gained by the Cornell RoboCup team through the introduc-
tion of the omni-directional drive prompted us to investigate the merits of the drive sys-
tem. The omni-directional drive system gave birth to a new family holonomic wheeled
platforms that feature full omni directionality with simultaneous and independently con-
trolled translational and rotational motion capabilities.
A variety of drive mechanisms have been inspired by the "universal wheel concept", il-
lustrated in figure 2.6. The "universal wheel" assembly provides a combination of con-
strained and unconstrained motions while turning, the combination of which provides
omni directionality. The "universal wheel" involves a large wheel with many small rollers
mounted on the rim. As the drive shaft of the wheel is powered the wheel is driven in
a direction perpendicular to the axis of the drive shaft, i.e. the constrained direction of
motion is in the x-axis. At the same time the rollers allow the wheel to move freely in
a direction parallel to the drive shaft, i.e. the unconstrained direction of motion is in the
z-axis.

19
 Y

X
Z

Figure 2.6: Example of a “universal wheel”

The disadvantages with this setup is that the wheel needs to be large in order to accom-
modate the rollers and suffers greatly from successive shock as each roller makes contact
with the ground. A variation of this design is where the rollers are positioned at 45 from
the axis and are tapered to remedy some of the roller shock experienced.

Orthogonal Wheels

The omni directional drive system used by Cornell is considered an "orthogonal wheels"
concept with provides normal traction in the direction perpendicular to the drive axis and
free wheels in the axis parallel to the motor axis. This behaviour is identical to that of
the universal wheel. There are two possible wheel assembly type based on the orthogonal
wheel concept, the longitudinal orthogonal wheel assembly and the lateral orthogonal
wheel assembly.
Before we discuss the specifics of each of the drive assemblies we will discuss the basic
principle of operation of the orthogonal wheel concept. The most critical component to
the operation of the drive system is the wheels. The wheels of the drive system are two
spheres of equal diameter, which have been sliced to resemble wide rounded tyres. The

20
axle of each of the wheels run perpendicular to the sliced surface through ball bearings
allowing the wheel to rotate freely about the axle. A rounded bracket at the extremities of
the axle is attached to an axle through which the wheel is rotated on the rounded surface
of the spheres, perpendicular to the motor axis.
When two of these wheel assemblies are parallel, at a constant distance and their rotations
about this motor axis synchronised, contact with the ground is assured by at least one
wheel at all times. Figure 2.7 illustrates the simplest configurations which makes use of
this operating principle.
rotate





































































































































































































































































































































































Y

Axle
X
Z

Figure 2.7: Basic orthogonal wheels operating principle

When the wheels are rotated in a synchronised fashion at the same speed they are driven
in a direction perpendicular to the motor axle, i.e. in the x direction. While being driven
in this direction, the wheel in contact with the ground is free to roll in a direction parallel
with the bracket axle, i.e. in the z-axis. This free rolling action allows the entire wheel
assembly to move freely in that direction.
Longitudinal Orthogonal Wheel Assembly
The longitudinal orthogonal wheel assembly is illustrated in the figure 2.8. When the main
shaft turns the wheels provide traction in a direction perpendicular to the main shaft, while
freewheeling in the direction parallel to the main shaft. In the direction perpendicular to
the main shaft the wheel assembly has a constrained motion controlled directly by the
rotation of the main shaft, while the motion component that is parallel to the shaft is
unconstrained.
The main drawback with this design which eliminated its possible use within our design
was the inability for the longitudinal orthogonal wheel assembly to rotate around a vertical
axis without allowing slip to occur.
During game play the robots are often required to rotate on the spot not only to turn but
also to push the ball away from the walls. In addition to this inability to rotate about a
vertical axis, control of this drive system would require some form of sensing of which
wheel is in contact with the ground as the distance the contact wheel is away from the

21
 ψ

Xref
3 θ
V
w1
w3

1
Yref

w2
2 y

Figure 2.8: Longitudinal orthogonal wheel assembly

centre of the robot is necessary when calculating the kinematics and dynamics of the
robot. All forms of sensing would overly complicate the design of the wheel assemblies
and the drive system and be very susceptible to damage during the course of a soccer
game.

Lateral Orthogonal Wheel Assembly

The Lateral Orthogonal Wheel assembly is a slightly more complicated design, as it re-
quires a gear or belt type transmission. Figure 2.9 gives a simple illustration of the drive
layout. Each wheel is held in a bracket, which is coupled to a drive shaft located, per-
pendicular to the axis of the wheel axle. The two drive shafts are parallel and coupled
together through gears, which allow them to be driven at the same velocity by a common
motor. The assembly thus has a constrained and controllable direction of motion perpen-
dicular to the drive shafts and a unconstrained freewheeling direction of motion parallel
to the drive shafts.
The lateral arrangement of the wheel assembly allows the platform to rotate about a ver-
tical axis with no slippage and without discontinuity of the motor speed. Rotation about
a vertical axis is possible due to both wheels being the same distance from the centre of
the platform.
With the lateral arrangement of the wheel assembly, no discontinuous change in the motor
speed is necessary when ground contact changes from one wheel to the other. This is due

22
 ψ

3 Xref
θ
V
w1

w3
1

Yref

w2
2

Figure 2.9: Lateral orthogonal wheel assembly

to both wheels turning with the same speed in their constrained direction; this also ensures
that no slip occurs during wheel transitions.
Now that the operation of the wheel assemblies have been discussed it is important to
detail how the assemblies can form a drive system for a platform. The only requirements
are that the layout needs to provide enough directions of constrained motion to allow
both omni-directional translation and rotation, and that the stability of the platform is
independent of the assembly’s configuration.
In order to produce a platform with three full degrees of freedom and no kinematic redun-
dancies the simplest arrangement is that of three wheel assemblies as shown in figure 2.8
and 2.9.
The three assemblies are located at the apex of a triangle to ensure the stability of the plat-
form, while the 120 orientation relationship between the constrained motion directions
of the assemblies provide excellent directional control capabilities.
Kinematic Relationships
For both of orthogonal wheel arrangements, the constrained motion of each assembly is
indicated by the arrows labelled 1, 2 and 3. For each of the layouts let represent the
angular velocity (in rads/sec) of the internal reference frame of the platform (Xref, Yref)
with respect to the absolute reference frame (x, y). The magnitude of the translational

23
velocity of the platform (in m/s) is denoted by 
 and the direction with respect to the in-
ternal reference frame of the platform is denoted by  [0, 2 ]. Using these conventions
the wheel shaft velocities, 
can be calculated (in rad/sec) for either layout as:

 



     

!
#"

 (2.1)
#"

$

%
   &  (2.2)


' 


    (  

!
#"

'
(2.3)

"
where is the radius of the spherical wheels and represents the distance from the
centre of the platform to the centre of the wheel assembly ) that is contact with the ground.
The first terms of 2.1-2.3 represents the projections of the translational velocity 
* on
the constrained motion directions of each assembly, while the rotational velocity of the
platform is represented by the last terms [5].
When performing odometry calculations the kinematic equations are used in a slightly
different form, which clearly illustrates the one to one relationship between the Cartesian
and wheel velocities. If we set

,+  -
*.   (2.4)

,/  -
* &  (2.5)

Equations 2.1-2.3 can be rewritten as:

     
'

10
32 
4+
,/

10
(2.6)

with 67
7
7

:9

'

 "

;=<
<
<

"

 (2.7)
2 5 
8 5 >
9 ' "
  '

24
 "

"
 and
"


Since are always positive quantities, is invertible and its inverse matrix
' 2

2 is:

67
7
7
 
 
9
 
9
 
 
 
9
;=<
<
<


' ' '


" " " "
2  " " "   (2.8)
 ' 8
 '
>

5 5 5

so that


4+
4/
 0
2
      
'
 0
(2.9)

where
 is relative to the robots internal reference frame,
 is relative to the robots
internal reference frame, is the rotational velocity of the robot about the vertical axis,
 is the velocity of wheel assembly 1,  is the velocity of wheel assembly 2 and   is
5

the velocity of wheel assembly 3 [5].

It is clear from the relationships described above that the rotational and translational com-
ponents of the platform’s motion are fully decoupled and can be controlled independently
and simultaneously. It is also evident that the motion of the robot can be controlled by
altering the values of
4+
,/ and , with the wheel velocities calculated using 2.6. The
actual velocity of the platform can also be determined via encoder feedback on the wheel
and is calculated using 2.9.
Using this drive design the platform can access the three degrees of freedom of planar
rigid body motion with no constraints or compatibility conditions on the three independent
controls, a capability not achieved by any conventional wheeled system.

Omni-directional poly roller wheel

The availability of this form of omni-directional wheel was only discovered at the RoboCup
competition this year. A number of team had implemented omni-directional drive systems
using this 4 cm omni-directional poly roller wheel. Figure 2.10 is a photograph of the
omni-directional poly roller wheel.

The drive system is arranged in the same triangular shape as the orthogonal omni-directional
drives with the orthogonal wheel pair being replaced with one roller wheel. The drive sys-
tem exhibits all the characteristics of the orthogonal omni-directional drive but without the

25
 Figure 2.10: Omni-directional poly roller wheel[14]

complexity to the orthogonal wheels. The roller omni-directional wheels provide a simple
cost effective means of implementing an omni-directional drive.
The disadvantages of the wheels that are evident are the size of the wheels. The wheels are
4 cm in diameter making the design of a compact drive system very difficult. The second
disadvantage of the wheels is despite being made of a softer polyurethane material the
wheels are more susceptible to slip on carpet at higher accelerations that polyurethane
covered orthogonal wheels.

2.4 Low Level Control

Before we can discuss the design and operation of the control system implemented in
the RoboRoos robots we must firstly explain what a control system is and the theory
behind their operation. Control systems can be designed to have many different response
characteristics dependent on their configuration. Two major configurations of a control
system that will be discussed are open loop and closed loop control systems. Each system
has it own merits and applications.
To design a control system to control a process we must first define the system in detail.
In particular we must establish what the input and output of the system is and the desired
response of the system. The input of the system is defined as a representation of the
desired response and the output of the system is the actual output of the system.
Two factors influence the output of the system to be different from the input. The first is
that the input to the system often changes instantaneously, i.e. a switch is activated, and
the physical limitations of the system do not allow the output to change instantaneously.
The path, which the output takes in reaching the desired output, is called the transient
response. The second factor that influences the output of the system is the degree of

26
approximation that the system makes of the desired output. The difference between the
input and the output is known as the steady state error of the system. The degree of steady
state error that is permitted within the system is always depended on the function that the
control system is performing.

2.4.1 Open and closed loop control systems

We can now describe the two control system configurations. A generic architecture of an
open loop system consist is illustrated in figure 2.11.

Plant and
controller
R(s) C(s)
G(s)
Input Output

Figure 2.11: Generic open loop system

The system consists of a controller, which reads in the desired input of the system and
drives the plant’s output to match the desired input. Open loop control systems have no
way of correcting for disturbance within the system and are simply commanded by the
input. This means that the system can deviate from the desired input and the system has
no way of correcting this error. A simple example of an open loop system is a toaster.
The input to the system is the colour of the toast we desire and as we are all aware that
the output of the system, i.e. the actual colour of the toast, is not monitored.
The inability of the open loop system to correct for disturbances can be overcome by
implementing a closed loop system. The generic architecture of a closed loop system is
illustrated in figure 2.12.

The closed loop system compensates for disturbances within the system by measuring the
actual output response of the system and feeding this measurement through a feedback
path to the controller of the system. The controller then calculates the difference between
the actual output of the system and the desired response of the system. If there is a
difference between the two signals, the system is then driven to correct the error. In the
case of there being no error between the input and the output, the system does not drive
the plant, since the plant’s response is already the desired response.

27
 Plant and
controller
R(s) + E(s) C(s)
G(s)
Input Actuating Output
− signal
+
(error)

H(s)

Feedback

Figure 2.12: Generic closed loop system

We can see that closed loop systems have the obvious advantage of greater accuracy, as
the system is less sensitive to the disturbances, noise and changes in the environment. In
addition the transient response and the steady state error of the system can be controlled
easily by redesigning the controller. This redesign is referred to as compensating the
system and the resulting hardware is often referred to as a compensator.
From the above configuration descriptions we needed to select a configuration to imple-
ment within the system. In order to do this we must first analyse the environment in which
we will be operating. The soccer game environment in which the RoboRoos compete in
is extremely dynamic and cluttered. The dynamic nature of the environment means that
the input to the system will be constantly changing. These constant changes to the input
mean that the transient response of the system has to be quick. In addition to this, the
system has to be very insensitive to external disturbances, as the robot will be colliding
with both other robots and the walls frequently.

2.4.2 Controllers and their response

The controller of a system can have one of two responses, an on-off response and a con-
tinuous response. An on-off response is where the controller changes the system from a
fully on state to a fully off state. If the control is continuous the response of the controller
is dependent on the error within the system.
In systems using continuous response it is often that the response may leave a steady state
error between the output and the input and the response may not be sufficiently rapid.
Often the overall operation of the system requires that the response of the controller be
improved to remove the steady state error or to improve the transient characteristics of the

28
response. To do this we introduce a compensator into the system.
The most common and simplest form of integrating a compensator is to introduce it into
the forward path of the system, though in some cases the compensator can be integrated
into the feedback loop. The form of compensation used is dependent on how the response
is being improved.

Proportional Integral Compensation

Proportional Integral (PI) compensation is an implementation of an active lag compen-

sator. A PI compensator is used to improve the steady state error of a system by placing
an open loop pole at the origin, increasing the system type by one. To ensure that the
original closed loop poles on the original root locus are not affected by the introduction


   
of the pole at the origin, a zero is placed very close to the to the open loop pole.
The resulting PI controller transfer function,  
 
. The pole of the controller




is located at  and the zero at  . This gives the PI controller an infinite gain at


zero frequency, improving the steady state error of the system. The and values of the
system must be selected very carefully to ensure a proper transient response and that the
system does not become unstable. The correct implementation of a PI compensator can
produce a transient response with little or no overshoot, however the speed of the response
suffers with the response being much slower.

Proportional Derivative Compensation

Proportional Derivative (PD) compensation is an implementation of an active lead com-

pensator. A PD controller is used to improve the characteristics of the transient response,
these include the percentage overshoot, the settling time and the rise time of the system.
Improvements to the transient response of the system can be achieved by either a simple
adjustment to the gain of the system or by adding zeros and poles to the forward path of
the system to produce a new open loop function whose closed loop poles go through the
design point on the plane.
A simplified version of the PD compensator has the transfer function,  

. 




The value of is selected to satisfy the steady state requirements of the system. The value

selected for governs the location of the zero that is added to the system. The location of
this zero changes the transient response of the system. Once again it is important to select


the values of and carefully so that the system does not become unstable.

29
Proportional Integral Derivative Compensation

Proportional Integral Derivative (PID) compensation is a combination of both PI and PD

compensation. The PID compensator is usually designed by first designing the PI com-
ponent of the compensator to improve the steady state error of the system before the PD
component of the compensator is designed. Often though, when designing for improve-
ments in either the transient response or the steady state error, improvements in one causes
a deterioration in the other. The final solution is always a trade off of transient response
to steady state error.

Feedforward Compensation

Feedforward compensation is designed to produce a position loop frequency response

with a constant magnitude and linear phase over the operational bandwidth. This char-
acteristic leads to a position loop which tracks the command with only a time delay as
shown in below.
Feedforward Response Graph

Uncompensated Command

Position

Feedforward Compensation

Time (s)

Figure 2.13: Response of a system to a step input

The use of feedforward compensation has been demonstrated to significantly improve

corner motion when integral gain is used in the position loop. Figure 2.13 shows a step
input into a system. The middle curve is the commanded motion while the motion with
and without feedforward compensation is indicated.

30
Chapter 3

Specifications

The ultimate aim of the RoboRoos 2001 team was to win the small size league at RoboCup
2001 in Seattle, USA in August of this year. In order to achieve this the robots underwent
a mechanical redesign. Before the redesign process could begin a detailed set of specifi-
cations were set down to ensure that the final design met our initial expectations.

Drive System Design Specifications

The criteria for the selection of the drive system were:

Compact design

Simple construction

Greater manoeuvrability and agility than previous design

Simple to control

Selecting a compact design for the drive system is one of the most important factors in
the selection process. The size restrictions that are imposed on the robots in the small size
league greatly influences the size and design of the drive system. Therefore a compact
design for the drive system would allow the mechanical designer greater freedom with the
overall design on the robot.
The simplicity of the design is important for two reasons. The limited construction facili-
ties available to the University means that overly intricate and complicated design would

31
require specialised manufacturing processes possibly outside of the University’s reach.
The second factor is the drive needs to be easily fixed in the case of failure during test-
ing or competition. In particular, during competition the team will not have access to
workshop facilities and will be required to make any necessary repairs with the limited
capabilities of hand tools.
The greatest disadvantage of the drive system of the RoboRoos 2000 team was the limited
manoeuvrability of the robots and the constraints on their motion. The differential drive
system of the robots was reasonably effective when the robot was moving in a straight
or curve line of motion but when the robot was required to move sideways the robots
required the following series of movements:

1. Turn on the spot

2. Move forwards/backwards

3. Turn again

4. Move forwards/backwards

5. Turn to face the desired direction again.

Ideally the new drive system should allow the robot to move freely and unconstricted in
any direction and face any direction while moving.
Finally an important factor is the selection of the drive system is the ability to operate the
drive system in a controlled manner. It is very important to be able to perform optimal
real time control of the drive system. Without real time control the new drive system will
be more of a disadvantage than an advantage. In order to create a control system for the
drive the kinematics must be derived. Once the kinematics have been derived the process
of creating a control system can begin.

32
Drive system component specifications

Once the drive system design has been selected the components that will make up the
drive system must be selected. The criteria that must be adhered to in the selection of the
motors, gears and encoders of the system are:

Physical size and shape

Performance characteristics

Power requirements

Due to the size restrictions imposed on the robots within the F180 small size league,
the components that are selected to form the drive system must meet the performance
requirements and be as small as possible.

Table 3.1 contains the desired performance characteristics of the new robots. The figures
contained within the table were used during the selection of the motors and gears.

 
Desired

 
Maximum Acceleration  3
Maximum Velocity
Time over half field




2.5
1
Table 3.1: Desired mechanical capabilities of the RoboRoos 2001 robots

With the power supply for the robots is limited to a 7.2V battery supply, the operating
voltage of the motor must be able to operate within this voltage range. In addition to
this the maximum operating current of the motors combined cannot exceed the current
capacity of the batteries.

33
Control System Specifications

Once the drive system has been selected the next major task is to write the control system
that will operate the drive system. The control system for the RoboRoos 2001 team must
meet the following requirements:

Integrate into the existing system

Operate the drive in a smooth and controlled manner

Correct modelling of the drive system

Due to the successful operation of the RoboRoos 2000 operating system it was decided
that that the system would remain the same with changes made to incorporate the added
mechanical capabilities and functionality of the new robots.

It is very important in the context of the system for the robots to operate in a controlled
manner. The system would be considered a failure if the robot’s motion did not exhibit
the control and precision that is inherent in other components of the overall system.

Perhaps the most important aspect of the control system is the correct modelling of the
control system and the kinematics of the drive system. Incorrect modelling of the system
would result in the drive not operating correctly nullifying all the processes that run above
it.

34
Chapter 4

Drive System

4.1 Principles of omni-directional drive system

It was decided that in order to be competitive on a mechanical level the introduction

of a drive system that provided the full three degrees of freedom of planar motion was
required. The obvious choice for the drive system that would provide the required freedom
of motion was the orthogonal wheels omni-directional drive system. The next matter to
decide in the design of the drive system was the layout of the wheel assemblies within the
drive system

4.1.1 Omni-directional drive configuration

“Lateral” v “Longitudinal” configuration

The first of the orthogonal wheels omni-directional drive layouts that was investigated was
the longitudinal orthogonal wheel assembly. This layout provides full omni-directional
translational motion but failed to provide rotation about the vertical axis without slippage
of the wheels. Rotation is seen as a very important ability that the robot must possess
in order to navigate quickly and efficiently around the field and it is common for robots
during matches to uses rotation as a means of retrieving a ball that is stuck in the corner
of the field.

35
In addition to the layout’s inability to rotate about a vertical axis, a means of sensing which
wheel is in contact with the ground at any instant is also required. This form of sensing
only complicates the drive assembly and the control system and introduces another point
of failure into the system.
The second layout of the orthogonal wheel omni-directional drive system was the lat-
eral orthogonal wheel assembly. This layout provides full omni-directional translational
motion and rotation about a vertical axis with no discontinuous operation of the drive mo-
tors. The layout provides three degrees of freedom within planar motion and in addition
all degrees of motion are fully decoupled.

"Y" v "Delta" configuration

An option that was considered when designing the drive system for the robots was an
omni directional drive system where the wheel assemblies were mounted in a delta con-
figuration. The benefit of this configuration is that it offers a more compact drive system
design. The drawback with the compact design is that there is no space at the bottom of
the robot to mount the batteries. The batteries need to be located as low as possible to keep
the centre of mass low. Figure 4.1 illustrates how the wheel assemblies are orientated in
the delta configuration.

Xref

Yref

Figure 4.1: Delta configuration omni-directional drive

The complication of the delta design, which was the biggest factor contributing to its
exclusion, was the fact that the wheels of each of the wheel assemblies are at a different

36
length for the centre of the robot. This causes the inner wheel of each assembly to provide
more torque, when turning the robot. The difference in distance from the centre of the
robot also causes a difference in encoder counts taken for the robot to turn a given angle,
depending on whether the inner or outer wheel of each assembly is touching the ground.
A means of sensing which wheel is contacting the ground is required to overcome the
problem. The introduction of this means of sensing greatly complicates the drive control
system and introduces another component within the system that is susceptible to damage
during the course of a soccer match.
In selecting the configuration of the drive system it was decided that the “Y” lateral or-
thogonal wheel assembly would be used in the RoboRoos 2001 team. This design was
chosen over the “delta” configuration because despite producing a more compact design
the complications associated with this drive system outweighed the benefits that it pro-
vided. The longitudinal orthogonal wheel assembly was also rejected due to the fact that
the assembly cannot rotate about the vertical axis.
The final design of the lateral orthogonal wheel assembly in the “Y” configuration is
illustrated in figure 4.2. A mechanical drawing of the underside of the robot is included
in Appendix B.1, detailing the components that make up the underside of the robot.

Figure 4.2: Lateral orthogonal wheel assembly drive system in “Y” configuration

37
4.2 Kinematics of omni-directional drive system

To understand the operation of the omni-directional drive as a whole the kinematics be-
hind each drive system must be investigated. Figure 4.3 illustrates the unconstrained and
constrained motion vectors of each wheel assembly. The constrained motion of the wheel
assembly is directly controlled through the control of the motors.
The driving (constrained) force generated by the wheel assembly is perpendicular to the
axis of the drive motors. This driving force can be broken down into individual Cartesian
components. When the wheels are driven with a unit force the resulting force component
in the x direction relative to the robot’s internal reference frame,
  !




9 '
 .

 
The component in the y direction relative to the robot’s internal reference frame, 





!    .
Constrained
Vx
Xref

Yref Unconstrained

Vy

Figure 4.3: Kinematics of an individual wheel assembly

Each of the three wheel assemblies can be modelled in the same manner to create a com-
plete kinematics model of the forces provide by the drive system. Figure 4.4 illustrates
the forces generated by the drive system and the conventions used when modelling the
drive system implemented in the RoboRoos 2001 team.
Using figure 4.4 the kinematics equations of the drive system can be derived. The equa-
tions as implemented in the RoboRoos 2001 control system are:




 
  
 
  (4.1)

 5   5    
' (4.2)
     
' (4.3)

38
 ψ
Vx
w1
Xref
2 θ
Vy V Vy

w2 1
Vx

Yref

y
Vy
3
w3

Figure 4.4: Kinematic representation of the forces generated by the omni-directional drive
system implemented in the RoboRoos 2001 robots.

It can be seen that when the robot is travelling in the forward direction the force is only
provided by the front two wheel assemblies. When the robot is rotating on the spot all the
translational forces cancel resulting in a pure rotation of the robots. Figure 4.5 illustrates
the force provided by each wheel assembly relative to the heading of the robot.

Physical Performance

The maximum forward straight line acceleration of the robot is calculated using the fol-
lowing equations:

Wheel torque = motor operating torque x gear ratio

force per wheel = wheel torque / wheel radius
maximum forward straight line acceleration = ((force per wheel x no of driv-
ing wheels) / robot mass) x forward component of force

39
 1
w1
w2
w3

force
0

-1
-180 0 180
relative angle

Figure 4.5: Force provided by each wheel assembly relative to heading

The maximum velocity of the robot is calculated in the forward direction using the fol-
lowing equation:

maximum velocity = (maximum motor RPM / Gear ratio) x circumference of

wheels x forward component of velocity

Table 4.1 contains the calculated performance characteristics of the RoboRoos 2001 robots.
These figures are based on the final design of the omni-directional drive system.

   
Actual


 
Maximum Acceleration  2.38
Maximum Velocity
Time over half field

 
2.49
0.97
Table 4.1: Actual mechanical capabilities of the RoboRoos 2001 robots

40
4.3 Component Selection

4.3.1 Motors

The motors that were selected for the drive system were the Faulhaber 2224 06 SR. The
DC micromotor possesses a skew wound ironless rotor coil that enables the motor to have
a unusually high output torque for it small size and low mass. The small diameter of the
motor allows the motor to fit between the drive shafts, at the same height as the rollers.
This lowered the centre of mass and allowed the kicking mechanism to be mounted at
its optimum height. The motor’s short length also allowed the entire drive to fit easily
within the maximum allowable robot diameter of 180mm. Table contains a list of the
characteristics of the drive motors.

Nominal Voltage 6 V
Max Speed 8000 rpm
Max Torque 5 mNm
Max Current 1.2 A
Weight 49 grams
Table 4.2: Faulhaber 2224 06 SR drive motor characteristics

4.3.2 Encoders

In keeping with the compact design criteria of the drive system the motors were fitted
with built in magnetic encoders. The 512 line encoders are mounted on the rear of the
motors adding only 1.4mm to the overall length of the motor. The 512 line encoders were
selected due to the small sampling time allowed within each execution of the loop. The
higher the number of lines the greater the resolution of distance travelled is achieved.

The encoders are read using quadrature decoding, which is a Timer Processing Unit (TPU)
input function. Quadrature decoding uses two channels to decodes a pair of signals that


are out of phase by to increment or decrement a position counter. When quadrature
decoding is used in conjunction with a slotted encoder, the control system can distinguish
not only the distance travelled by the encoder but also can taking into account any changes
in the direction of travel. Figure 4.6 diagrammatically illustrates how quadrature decoding
operates.

41
 Figure 4.6: Quadrature decoding [18]

4.3.3 Gear Train

The gear train of the drive system was design by David Cusack for a detailed design
analysis refer to his undergraduate thesis [15]. The motors of the drive system are cou-
pled to the wheel shafts through a simple single stage spur gearbox. A gear ratio of 4.2
was achieved using a 10-tooth brass pinion driving two 42-tooth brass gears. Figure 4.7
illustrates the gear train arrangement used.

Figure 4.7: Single stage spur gearbox on each drive assembly

42
The gear ratio was set at 4.2 for the following reasons:

Achieves the best equilibrium between acceleration and velocity minimising travel
time over the expected average journey distance

It was achievable in one stage of gear reduction, minimising the size, mass and
number of moving parts in the drive train

It satisfied a vast number of geometric relationships between components of the

drive system, the batteries, and the kicker

43
Chapter 5

Robot Control Software

5.1 Reactive Navigation Schemas

The navigation process begins with the vision system which determines the location and
orientation of each robot (including opposition robots) on the field and the position and
velocity of the ball. This information is passed to the Multi-Agent Planning System
(MAPS), which determines the strategy for the robot team. MAPS chooses which robot
is to kick the ball, where it is to be kicked and where the other robots should move to.
The actual kicking and navigating actions are enacted via the reactive navigation schemas
operating locally on the robots.
The obstacle map (OM) schema is responsible for determining the distance, bearing and
size of each of the obstacles on the field and guarantees the appropriate obstacle map.
Each obstacle on the field including the walls are mapped separately into the OM repre-
sentation. The goal direction (GD) schema is responsible for determining the direction
to head in order to reach the goal location as set by MAPS. The heading direction of the
robot is chosen by subtracting the OM input form the GD input as shown in figure 5.1 .
From the reactive navigation schemas the distance to the desired location, the heading and
the facing angle are determined. These values are then passed to the appropriate motor
control schema selected by the function set_omni_move in the function OmniNavigate.
The modifications to the navigation system were the responsibility of David Ball, for a
complete analysis of the navigation system operation refer to his undergraduate thesis
[16].

44
 OM GD

−180 180 −180 180

+
−

−180 180

Figure 5.1: Example of the OM and GD integration process to produce the best heading
for the robot [19].

5.2 Reactive Control Loop

The reactive control loop that had been implemented in the RoboRoos 2000 team had
proven itself robust and reliable both in testing and in competition. It was for this reason
that the control system implemented in the RoboRoos 2001 team will follow the same
operational structure.
The control system that was implemented in the 2001 team was not written from scratch
but modified from the 2000 team. This was largely due to its proven operation and the
fact that the electrical hardware was the same as the 2000 team. The majority of changes
that were made to the reactive control loop code were to allow for control of the new
omni-directional drive system.

The reactive control loop is initiated every millisecond by the Periodic Interrupt Timer
(PIT). Once initiated the reactive control loop receives the desired wheel velocities, cal-
culates the proportional and the integral errors on the wheel velocities and calculates the
PWM signals that are sent to the motors. Figure 5.2 illustrates the new control model that
is implemented in the reactive control loop.
The complete source code of the reactive control loop is included in Appendix C. The
flowchart in figure 5.3 illustrates in details the operation of the reactive control loop.
In determining the output of the system the control loop calculates the proportional and
integral errors of the wheel velocities. The velocity proportional errors for each of the
wheels are calculated as shown:

45
 w1dvel +
Σ w1err G(s) w1pwm

+ −
w2dvel w2pwm
Σ w2err G(s) MOTORS
+ −
w3dvel w3err
Σ G(s) w3pwm

w1vel
w2vel
ENCODERS
w3vel

Figure 5.2: Control model implemented on the RoboRoos 2001 robots

Wheel velocity error = desired wheel velocity - actual wheel velocity

The wheel velocity integral error is calculated as follows:

Wheel integral error = wheel velocity proportional error

Once the error values are known using the above calculations the PWM values output to
the motor can be calculated as follows:

PWM = (proportional error + ( integral error * integral constant)) * propor-

tional constant + (desired velocity * feedforward constant)

Due to the PI control loop constants each having the value of 1, the above equation was
simplified to:

PWM = proportional error + integral error + desired velocity

46
 PIT Increment
Interrupt Timer

Read encoder
position count

Call control
routine

Calculate
change in
position

Test for yes Correct for

overflow overflow

no

Calculate wheel
velocity proportional
error

Calculate wheel
integral error

check if stuck Stuck flag

yes true

no

Calculate wheel
PWM values

Check duty Set PWM value

cycle limits yes to maximum

no

Output PWM
values for each
channel

Figure 5.3: Flowchart of the control loop implemented on the RoboRoos 2001 robots

47
5.3 Motion Control Routine

OmniControl is called by the reactive control loop running on the robots once during ev-
ery interrupt cycle. The OmniControl function is responsible for calculating the wheel
velocities for the next reactive control loop iteration and for performing the path inte-
gration process. The platform velocity, heading and facing angle are determined in the
reactive navigation schemas. The motor control schemas calculate the platform velocities
based on the information passed down from the reactive navigation schemas.
Once the function has returned from performing the path integration the individual wheel
velocities are calculated from the platform velocity. The equations used in calculating the
wheel velocities are:

 
 5   
)

!
 (  
)
 (5.1)

$ 
)
  )

!
(5.2)

 5       


'     
)
 (5.3)

where  is the velocity of wheel assembly 1,  is the velocity of wheel assembly 2

5

and   is the velocity of wheel assembly 3, )
 is direction relative to the robot’s internal
reference frame and is the rotational velocity of the robot about the vertical axis. Due to
the microprocessor being limited to operating on integers only, a lookup table is created
containing the velocity components of each wheel for each direction.

5.3.1 Motor Control Schemas

Omni_halt_fn

This function is responsible for bringing the robot to a graceful halt as quickly as pos-
sible. The function performs this by reducing the robot’s velocity each iterative loop of
the control system by the maximum deceleration allowable that does not cause slip or
discontinuity of the motors.

48
Omni_move_fn

This function is responsible for the general motion of the robot, controlling the accelera-
tion, deceleration, heading and facing direction of the robot. The function receives from
the higher navigation processes the values for the distance the robot needs to travel, the
change in heading required and the change in facing required.
The operation of the omni_move_fn motor schema can be broken down onto three main
operations. The first operation that the schema performs is to reduce the difference be-
tween the desired facing angle and the actual facing angle taking into account the transla-
tional velocity of the robot.

The second operation performed calculates the translational component of the robots mo-
tion. As the goal location is specified by a length parameter, the robot’s velocity is ramped
up at a maximum acceleration until a translational velocity is achieved that will minimise
the travelling time but still remains within the physical bounds of the robot.
The final operation that the omni_move_fn motor schema performs is to align the robot
with the desired heading set by the navigation schemas. The robot’s rotational velocity is
ramped until a rotational velocity is achieved that minimises the travelling time to reach
the desired heading. This operation takes into account the positive and negative rotation
possible by the robot.

5.3.2 Path Integration

Path integration is performed to maintain the robot’s internal reference of position and
heading and allows for the position and the heading reported by the vision system to be
reconciled with the sensor delay. Path integration aboard the robots is performed by the
OmniIntegrate function. The flowchart in figure 5.4 details the operation of the OmniIn-
tegrate function.

49
In order to determine the platform velocity from the wheel velocities the OmniIntegrate
function uses the following inverse kinematic equations:




    
(5.4)



 

    
'

 (5.5)
   
  

 
'
(5.6)


where


 is relative to the robots internal reference frame,

 is relative to the robots

internal reference frame,   
 is the rotational velocity of the robot about the vertical
axis,  is the velocity of wheel assembly 1,  is the velocity of wheel assembly 2 and
5

  is the velocity of wheel assembly 3.

These equations are derived from the inverse kinematic equation described in chapter 2.
The equations have been simplified and rewritten to match the conventions detailed in
figure4.4.

Read wheel
OmniControl
velocities

Calculate platform
velocities

Calculate
new heading

Calculate
new position

Update position
list

Figure 5.4: Flowchart of the path integration function implemented on the RoboRoos
2001 robots

50
Chapter 6

Results & Discussion

The design and implementation of the RoboRoos 2001 robots was completed within seven
months to meet the deadline of RoboCup 2001. The ultimate test for the drive system
and its control system was the RoboCup 2001 competition is Seattle, USA. The team
completed successfully in the small size league convincingly winning our round robin
group and progressing to the quarter finals. In the quarter finals the RoboRoos went down
1-0 to the Field Rangers in extra time courtesy of the “Golden Goal” rule.
Due to time constraints prior to our departure for the competition, the drive system and
control system of the robots were only briefly tested and the completed robots only had
one test run as a team the night before departing for the tournament. The tournament
served as our testing environment with a great deal of time spent before and after the
games tweaking the system to produce the best performance results.

The design of the control system began on paper with the derivation of the kinematic
equations. Once these were derived a great deal of time was spent understanding the op-
eration of the drive system. In order to aid in the understanding of the drives operation a
drive system was made from Lego. The Lego design of the drive system is described in the
undergraduate thesis written by David Cusack. Once the operation of the system was un-
derstood the simulator served as an excellent tool for developing the new motor schemas
that would utilise the new mechanical abilities of the omni-directional drive system.
The first test run of the control system on the robot produced very favourable results
with the robot following the predefined path as accurately as possible considering the
mechanical state of the robot base. Testing in the early stages was very tricky as the
wheels on the drive system had no rubber coating. This meant that the robots suffered

51
greatly from slip, which could not be corrected as the vision system was not operational
at this stage.
Implementing the control system on the robots met it usual hurdles with minor changes
required to the code in order to account for the physical limitations of the processor. The
most common fault that was encountered was integer overflow. Once the problem was
identified it was easily rectified.
A problem inherent in the motor control schemas is that the robot slows down to make
small changes in its heading. This results in the robot spending too much time adjusting
to the optimal heading only to have to readjust again due a change in the environment. In
order to overcome this problem, small adjustments in the heading of the robot should be
made without a change in the translational velocity of the robot.
Aside from the above described problem the control system for the omni-directional sys-
tem operates correctly. The robot is able to traverse and rotate in a manner described
by the navigation process. The reactive control loop also operates correctly with the any
integral error introduced into the system corrected for.

Drive Performance

The desired characteristics of the drive system that were set in the predesign specifica-



tions have been met as closely as physically possible. The desired maximum velocity of

 
 
was met exactly with the actual maximum velocity being
maximum forward straight line acceleration of the robot was 


 
. The actual

of the desired maximum velocity of 



which was short
. This difference in acceleration did not result
in a substantial increase in the time taken to traverse between two points.
I believe that the reactive navigation and control code which has been modified from
the RoboRoos 2000 system, though it has successfully implemented the capabilities of
the omni-directional drive, does not fully utilise the true potential that the drive has to
offer in terms of manoeuvrability. The support to this notion is the fact that the robot
is unable to traverse in a straight line while rotating around it vertical axis, which is an
inherent property of the lateral orthogonal omni-directional wheel assembly. With the
robot operating correctly in all other defined motion operations the problem cannot be
attributed to incorrect kinematic modelling of the system.

52
6.1 Further Work

Reactive Control Loop Tuning

The most important work that needs to be completed on the robots before the next com-
petition is the tuning of the PI control system constants. At the moment, the constant for
the feedforward control loop are all set to one. Modelling the system using a program
such as Matlab and physical testing of the system needs to be performed to determine if
the values of one for the constants are in fact the values that provide optimal control of
the drive system.
In order to model the system, a complete model of the drive system needs to be cre-
ated. Even with the correct modelling of the drive system constants that produce a better
response may be unachievable due to the microprocessor being limited to operating on
integers only.

Dribbling Mechanism

When the dribbling bar is incorporated into the design of the robots, a new motor control
schema will have to be written to fully utilise the advantage that the dribbling bar can
offer. We predict that the dribbling bar will not be able to retain control of the ball while
rotating about the centre of the robot.
To enable the robot to rotate while maintaining control of the ball the robot must pivot
about the ball. In doing this the ball is not moving and therefore can remain in full contact
with the dribbling bar at all times. In order for the omni_pivot_fn function to be able
to accurately pivot around the ball, the location of the ball will have to be accurately
determined by the vision system. Once the location of the ball is known the function must
then calculate a circular path around the ball that will enable the robot to face the ball and
have control of it during the entire move.
If the dribbling bar is able to exert enough downward force on the ball that the robot is able
to move away from the ball backwards and still retain possession then the omni_pivot_fn
function will not have to be so rigorously controlled.

53
Chapter 7

Conclusion

The control system designed for the RoboRoos 2001 robot soccer team has now been
explained in detail. The control system forms an integral part of the RoboRoos system
acting as the interface between the intelligent navigation system and hardware of the
system.
The major points of the thesis are listed below:

Correct kinematic models have been established for the new omni-directional sys-
tem.

The reactive control system has been redesigned to control the omni-directional
drive and operates correctly.

New motor schemas were written to take into account the new manoeuvrability of
the robots.

The control system integrates into the existing system correctly.

PI tuning needs to be performed in order to establish if better constants for the

feedforward PI control loop are available.

Correct the decrease in velocity when small changes in the heading are made.

The complete navigation system has to be revised to operate purely based on the
omni-directional system not a modified version of the differential drive system.

54
Although there is still work to be completed on the design of the control system for the
RoboRoos 2001 robot soccer team, a robust operational system exists that effectively
models and controls the drive system. The performance of the control system has been
proven in international competition. With further refinements, the control system can
maximise the manoeuvring advantages that the omni-directional drive offers.

55
Bibliography

[1] The RoboCup Federation, 2001, RoboCup official site,

http://www.robocup.org/02.html, (1 October, 2001)

[2] The RoboCup Federation, 2001, RoboCup-2001 Simulation Soccer Events,

http://www.cs.cmu.edu/~galk/rc01/, (10 October, 2001)

[3] Noda, 2001, Overview of Soccer Server, http://sserver.sourceforge.net/NEW/Overview.html,

(10 October, 2001)

[4] The RoboCup Federation, 2001, RoboCup World Cup 2001 Seattle
RoboCup-Rescue Simulation League, http://www.r.cs.kobe-u.ac.jp/robocup-
rescue/robocup2001sche-e.html, (10 October, 2001)

[5] Francois, G.P. and Killough S.M, 1994, A New Family of Omnidirectional and Holo-
nomic Wheeled Platforms for Mobile Robots, IEEE Transactions on Robotics and
Automation, vol. 10, no. 2, pp. 480-489.

[6] Nise N.S, 2000, Control Systems Engineering, John Wiley and Sons, New York,
USA.

[7] D’Andrea R, et al., 2000, The Cornell Robocup Team, Sibley School of Mechanical
and Aerospace Engineering, Cornell University, USA.

[8] Kalmar-Nagy T, and D’Andrea R., 2001, Real Time, Near-Optimal Trajectory Con-
trol of an Omni-Directional Vehicle, Symposium on Modelling, Control and Diag-
nostics of Large-Scale Systems, ASME IMECE ’01, New York, USA.

[9] Ford M., 1998, Operating System and Motor Control Software for a Soccer Play-
ing Robot, Bachelor of Engineering Thesis, Department of Computer Science and
Electrical Engineering, University of Queensland, Australia.

56
[10] Pfrunder A, 1998, An On-board Navigation System for Mobile Robots, Bachelor of
Engineering Thesis, Department of Computer Science and Electrical Engineering,
University of Queensland, Australia.

[11] Boyd R, 1998, The Hardware Design of a Soccer Playing Robot, Bachelor of Engi-
neering Thesis, Department of Computer Science and Electrical Engineering, Uni-
versity of Queensland, Australia.

[12] Veloso M, Pagello and Kitano H, 2000 RoboCup -99: Robot Soccer World Cup III,
Springer.

[13] Wyeth G, Browning B and Tews A, 1998, UQ RoboRoos: Preliminary Results for
a Robot Soccer Team, Department of Computer Science and Electrical Engineering,
University of Queensland, Australia.

[14] Acroname, 2001, 4cm omni-directional poly roller wheel,

http://www.acroname.com/robotics/parts/R97-4CM-POLY-ROLLER.html, (12
October 2001)

[15] Cusack D, 2001, Mechanical Redesign of the RoboRoos (robot soccer team), Bach-
elor of Engineering Thesis, Department of Mechanical Engineering ,University of
Queensland, Australia.

[16] Ball D, 2001, On-board Intelligence for the RoboRoos, Bachelor of Engineering
Thesis, Department of Information Technology and Electrical Engineering ,Univer-
sity of Queensland, Australia.

[17] Motorola Inc, 1997, Pulse Width Modulation TPU Function (PWM), Databook.

[18] Motorola Inc, 1993, Fast Quadrature Decode TPU Function (FQD), Databook.

[19] Browning B., 2000, Biologically Plausible Spatial Navigation for a Mobile Robot,
PhD dissertation, Department of Computer Science and Electrical Engineering, The
University of Queensland, Australia.

[20] The RoboCup Federation, 2001, RoboCup F-180 League Rules.

http://arti.vub.ac.be/RoboCup/rules/rules.html (16 October, 2001)

57
Appendix A

Complete System

58
 Camera
Figure A.1: Overview of the complete RoboRoos system
59

Transmitter
PC running vision systems
and MAPS

RoboRoos 2001 robots

x5

RoboRoos 2000 robots

x3
Appendix B

Mechanical Drawing of RoboRoos 2001

Drive System

60
 Base Plate

Kicker Motor
Battery Clip

Batteries
Front

Gear Train
Drive Shaft
Drive Motor
Ball Claw

Wheel

Figure B.1: Mechanical drawing of the drive system and components on the underside of
the RoboRoos 2001 robots

61
 Appendix C

Reactive Control System Source Code

Program C.1: Reactive Control System

1 /
2
3 TITLE : Servo . c
4
5 PURPOSE : Program f o r p r o p o r t i o n a l i n t e g r a l c o n t r o l of t h e r o b o t
6 A d a p t e d f r o m c o d e f r o m w r i t t e n by Gordon Wyeth
7
8 WRITTEN BY : R u s s e l l Boyd
9 Michael Stevens
10 Miles Ford
11 B r e t t Browning
12 Gordon Wyeth
13
14 REVISION HISTORY :
15 13/5/98
R e w r i t t e n f o r code i n t e g r a t i o n
16
17 MAJOR OMNI REVISION
18 0 1 / 0 8 / 0 1
Written to o p e r a t e the omni_drive
DB/RM
19 /
20
21 #include " mc6833x . h "
22 #include " types . h"
23 #include " tpu . h"
24 #include " servo . h"
25 #include " sci . h"
26 #include " Orobot . h "
27
28 #include " robotio . h"
29 #include " speaker . h"
30 #include " kicker . h"
31
32 / Needed f o r r o b o t e n c o d e r w i d t h n u m b e r s /
33 # i n c l u d e " . . \ common \ t y p e s . h "
34
35 / Include simulator f i l e s /
36 # i n c l u d e " . . \ common \ r t y p e s . h "
37 # i n c l u d e " . . \ Orobot \ O c o n t r o l . h "
38
39
40 / T u r n on o r o f f t h e e n a b l e / d i s a b l e s e r v o f u n c t i o n s /
41 / / # d e f i n e ENABLE_DISABLE_SERVO
42
43 / PI C o n t r o l C o n s t a n t s /
44 # d e f i n e FKP 1
45 # d e f i n e TKP 1
46 # d e f i n e FKI 1
47 # d e f i n e TKI 1
48 # d e f i n e FFWD 1
49 # d e f i n e TFWD 1
50
51 / I n t e g r a l e r r o r s a t u r a t i o n p o i n t s . U n i t s a r e mm and h e a d i n g s r e s p e c t i v e l y /
52 # d e f i n e FSTUCK (100 OCOUNTPERMM )
53 # d e f i n e FUNSTUCK (5 OCOUNTPERMM )
54 # d e f i n e TSTUCK 0 x3FFFF
55
56 / Number o f c o u n t s p e r ms b e f o r e TPU m i g h t s t u f f up /
57 # d e f i n e FAST_THRESHOLD 5 0
58
59 / B i g g e s t v a l u e o f v e l b e f o r e we a s s u m e an o v e r f l o w . /
60 # d e f i n e W_OVERFLOW 0 x800
61
62 /  GLOBALS  /
63 long w1int , w2int , w3int ;
64 int w1 , w2 , w3 ;

62
 65 int w1pwm , w2pwm , w3pwm ;
66 int w1vel , w2vel , w3vel ;
67 int w1err , w2err , w3err ;
68 int time = 0 , f l a g = 0 , stuck = 0 ;
69 int w1dvel = 0 , w2dvel = 0 , w3dvel = 0 ;
70 int oldw1 = 0 , oldw2 = 0 , oldw3 = 0 ;
71
72
73 int stuckdir = 1;
74
75 i n t f o r w a r d _ e n a b l e = TRUE ;
76 i n t r o t a t i o n _ e n a b l e = TRUE ;
77
78 int freq = 200;
79
80
81 /
82 PIT_intr

83
84 This i n t e r r u p t r e c e i v e s t h e d e s i r e d wheel v e l o c i t i e s , c a l c u l a t e s
85 w h e e l v e l o c i t y e r r o r and t h e n c a l c u l a t e s t h e r e q u i r e d d u t y c y c l e
86 to decrease the error using proportional i nt eg ra l control
87 /
88
89
90 void i n t e r r u p t P I T _ i n t r ( void )
91 {
92
93 LED2_ON ( ) ;
94
95
96 / C l e a r t h e w a t c h d o g by w r i t i n g 0 x55 and 0 xAA i n o r d e r t o s o f t w a r e
97 service register .
98 /
99 SIM_REGS . SWSR = SIM_SWSR_RESET1 ;
100 SIM_REGS . SWSR = SIM_SWSR_RESET2 ;
101
102 time ++;
103
104 / Read e n c o d e r p o s i t i o n c o u n t /
105 w1 = ( ( i n t ) QDEC_read (W1QDECPRI ) ) ;
106 w2 = ( ( i n t ) QDEC_read (W2QDECPRI ) ) ;
107 w3 = ( ( i n t ) QDEC_read (W3QDECPRI ) ) ;
108
109 Kicker_Controller ( );
110
111 / Call the control routine /
112 O m n i C o n t r o l (& r o b o t . move ) ;
113
114 / Compute c h a n g e i n p o s i t i o n /
115 w 1 v e l = w1
oldw1 ;
116 w 2 v e l = w2
oldw2 ;
117 w 3 v e l = w3
oldw3 ;
118
119 / I f we a r e g o i n g t o f a s t , s w i t c h t o f a s t q u a d d e c o d e /
120 i f ( w 1 v e l > FAST_THRESHOLD )
121 F Q D E C _ s e t f a s t (W1QDECPRI ) ;
122 e l s e i f ( w 1 v e l <
FAST_THRESHOLD )
123 F Q D E C _ s e t f a s t (W1QDECPRI ) ;
124 else
125 FQDEC_setnormal (W1QDECPRI ) ;
126
127 i f ( w 2 v e l > FAST_THRESHOLD )
128 F Q D E C _ s e t f a s t (W2QDECPRI ) ;
129 e l s e i f ( w 2 v e l <
FAST_THRESHOLD )
130 F Q D E C _ s e t f a s t (W2QDECPRI ) ;
131 else
132 FQDEC_setnormal (W2QDECPRI ) ;
133
134 i f ( w 3 v e l > FAST_THRESHOLD )
135 F Q D E C _ s e t f a s t (W3QDECPRI ) ;
136 e l s e i f ( w 3 v e l <
FAST_THRESHOLD )
137 F Q D E C _ s e t f a s t (W3QDECPRI ) ;
138 else
139 FQDEC_setnormal (W3QDECPRI ) ;
140
141
142 /
143 T e s t f o r o v e r f l o w . Assume max c h a n g e b e t w e e n
144 i n t e r r u p t s i s one t u r n o r 2 5 6 .
145 GFW 2 6 / 7 Changed t h i s t o 2 0 4 8 and i m p l e m e n t e d a
146 # d e f i n e OVERFLOW
147 /
148
149 i f ( w 1 v e l > W_OVERFLOW )
150 w 1 v e l
= 0 xFFFF ;
151 e l s e i f ( w 1 v e l <
W_OVERFLOW )
152 w 1 v e l + = 0 xFFFF ;
153
154 i f ( w 2 v e l > W_OVERFLOW )
155 w 2 v e l
= 0 xFFFF ;
156 e l s e i f ( w 2 v e l <
W_OVERFLOW )
157 w 2 v e l + = 0 xFFFF ;
158
159 i f ( w 3 v e l > W_OVERFLOW )
160 w 3 v e l
= 0 xFFFF ;
161 e l s e i f ( w 3 v e l <
W_OVERFLOW )
162 w 3 v e l + = 0 xFFFF ;
163
164 / Convert v e l o c i t i e s to 1 / 2 5 6 th of encoder count /
165 w1vel < < = 8 ;
166 w2vel < < = 8 ;
167 w3vel < < = 8 ;
168

63
169 / Save c u r r e n t p o s i t i o n f o r next time /
170 oldw1 = w1 ;
171 oldw2 = w2 ;
172 oldw3 = w3 ;
173
174 / Compute w h e e l velocity proportional error /
175 w1err = w1dvel
w1vel ;
176 w2err = w2dvel
w2vel ;
177 w3err = w3dvel
w3vel ;
178
179 / Compute w h e e l v e l o c i t y i n t e g r a l e r r o r /
180 w1int + = w1err ;
181 w2int + = w2err ;
182 w3int + = w3err ;
183
184 / S a t u r a t e t h e i n t e g r a l e r r o r t o p r e v e n t t h e s e r v o windup /
185 s t u c k = FALSE ;
186 i f ( ABS ( w 1 i n t ) > FSTUCK ) {
187 w 1 i n t =
SGN( w 1 i n t ) FUNSTUCK ;
188 s t u c k = TRUE ;
189 }
190 i f ( ABS ( w 2 i n t ) > FSTUCK ) {
191 w 2 i n t =
SGN( w 2 i n t ) FUNSTUCK ;
192 s t u c k = TRUE ;
193 }
194 i f ( ABS ( w 3 i n t ) > FSTUCK ) {
195 w 3 i n t =
SGN( w 3 i n t ) FUNSTUCK ;
196 s t u c k = TRUE ;
197 }
198
199 /
200 Compute v i r t u a l m o t o r PWM v a l u e s
201 pwm = ( p r o p e r r o r + ( i n t e r r o r k i n t ) ) kprop + d e s i r e d v e l o c i t y in counts
202 /
203 w1pwm = ( ( w 1 e r r + ( w 1 i n t ) + w 1 d v e l ) ) ;
204 w2pwm = ( ( w 2 e r r + ( w 2 i n t ) + w 2 d v e l ) ) ;
205 w3pwm = ( ( w 3 e r r + ( w 3 i n t ) + w 3 d v e l ) ) ;
206
207 / Now c o n v e r t pwm v a l u e s b a c k t o s i n g l e e n c o d e r c o u n t s c a l e /
208 w1pwm > > = 8 ;
209 w2pwm > > = 8 ;
210 w3pwm > > = 8 ;
211
212 / C o n s t r a i n duty cycle to l i m i t s /
213 w1pwm = MAX(MIN ( w1pwm , PWM_PERIOD) ,
PWM_PERIOD ) ;
214 w2pwm = MAX(MIN ( w2pwm , PWM_PERIOD) ,
PWM_PERIOD ) ;
215 w3pwm = MAX(MIN ( w3pwm , PWM_PERIOD) ,
PWM_PERIOD ) ;
216
217 / O u t p u t PWM i n d e s i r e d d i r e c t i o n ( d i r o f m o t o r ) and d u t y c y c l e /
218 i f ( w1pwm > = 0 ) {
219 PWM_set (W1PWMCHA, ( u s h o r t ) ( PWM_PERIOD
w1pwm ) ) ;
220 PWM_set (W1PWMCHB , PWM_PERIOD ) ;
221 } else {
222 PWM_set (W1PWMCHA, PWM_PERIOD ) ;
223 PWM_set (W1PWMCHB , ( u s h o r t ) ( PWM_PERIOD + w1pwm ) ) ;
224 }
225 i f ( w2pwm > = 0 ) {
226 PWM_set (W2PWMCHA, ( u s h o r t ) ( PWM_PERIOD
w2pwm ) ) ;
227 PWM_set (W2PWMCHB , PWM_PERIOD ) ;
228 } else {
229 PWM_set (W2PWMCHA, PWM_PERIOD ) ;
230 PWM_set (W2PWMCHB , ( u s h o r t ) ( PWM_PERIOD + w2pwm ) ) ;
231 }
232 i f ( w3pwm > = 0 ) {
233 PWM_set (W3PWMCHA, ( u s h o r t ) ( PWM_PERIOD
w3pwm ) ) ;
234 PWM_set (W3PWMCHB , PWM_PERIOD ) ;
235 } else {
236 PWM_set (W3PWMCHA, PWM_PERIOD ) ;
237 PWM_set (W3PWMCHB , ( u s h o r t ) ( PWM_PERIOD + w3pwm ) ) ;
238 }
239
240 LED2_OFF ( ) ;
241
242 }
243
244 /
245 OmniMotor

246
247 The d e s i r e d v e l o c i t i e s f r o m O c o n t r o l a r e r e c e i v e d
248 /
249
250 v o i d OmniMotor ( i n t v e l 1 , i n t v e l 2 , i n t v e l 3 )
251 {
252 w1dvel = v e l 1 ;
253 w2dvel = v e l 2 ;
254 w3dvel = v e l 3 ;
255 }
256
257
258 /
259 OmniEncoder

260
261 S e t s t h e wheel v e l o c i t i e s t o e q u a l t h a t r e a d from t h e e n c o d e r s
262 /
263 v o i d OmniEncoder ( i n t v e l 1 , i n t vel2 , int vel3 )
264 {
265 v e l 1 = w1vel ;
266 v e l 2 = w2vel ;
267 v e l 3 = w3vel ;
268 }
269
270 /
271 ResetServo

272

64
273 This f u n c t i o n r e s e t s t h e i n t e g r a l e r r o r s of t h e s e r v o loop back
274 to zero .
275 /
276 void ResetServo ( void )
277 {
278 w1int = 0 ;
279 w2int = 0 ;
280 w3int = 0 ;
281 }
282
283
284 /
285 DisableServo

286
287 T h i s f u n c t i o n d i s a b l e s t h e f o r w a r d and / o r t h e t u r n s e r v o l o o p
288 /
289 v o i d D i s a b l e S e r v o ( i n t fwd , i n t r o t )
290 {
291 // i f ( fwd )
292 // f o r w a r d _ e n a b l e = FALSE ;
293 // if ( rot )
294 // r o t a t i o n _ e n a b l e = FALSE ;
295 }
296
297
298 /
299 EnableServo

300
301 This f u n c t i o n enables the f u l l servo loop
302 /
303 void EnableServo ( void )
304 {
305 // f o r w a r d _ e n a b l e = TRUE ;
306 // r o t a t i o n _ e n a b l e = TRUE ;
307 }

65
 Appendix D

Motion Control Routine Source Code

Program D.1: Motion Control Routine

1 /
2 TITLE : Ocontrol . c
3
4 PURPOSE : P e r f o r m s t h e a c t u a l c o n t r o l o f movement . I m p l e m e n t s t h e Schemas
5
6 WRITTEN BY : Gordon Wyeth
7 A s h l e y Tews
8
9 CREATED : 21/5/98
10
11 REVISION HISTORY :
12 23/4/99
Added c o d e f o r NPI
BB / AP
13
14 MAJOR OMNI REVISION :
15 01/08/01
R e v i s e d t o c o n t r o l omni
d r i v e
DB /RM
16
17 /
18 #include < s t d l i b . h>
19 #include " . . \ common \ r t y p e s . h "
20 #include " Ocontrol . h"
21 #include " Omotor . h "
22 #include " . . \ common \ l o o k u p . h "
23 #include " . . \ common \ t y p e s . h "
24 #include " . . \ common \ common . h "
25
26 /
27 Debug S w i t c h e s

28
29 /
30 # d e f i n e DEBUG
31
32 # i f d e f DEBUG
33 # i f d e f SIMUL
34 # i n c l u d e < windows . h>
35 # i n c l u d e " . . \ simcode \ debug . h "
36 #endif
37 #endif
38
39 # i f n d e f SIMUL
40 # include " . . \ board \ r o b o t i o . h"
41 #endif
42
43 /
44 Compiler Switches
45 ENC_FBACK
T h i s s w i t c h e s t h e e n c o d e r f e e d b a c k f o r t h e NPI on
46 /
47 # d e f i n e ENC_FBACK
48 # d e f i n e OCOUNTPERGORDO4 OCOUNTPERGORDO 4
49 # d e f i n e USE_FLIP_CODE
50
51
52 /  CONSTANTS  /
53 # d e f i n e K256_ON_ROOT3 (( int )((256. 0 / 1. 73205) + 0. 5))
54 # d e f i n e K256_ON_3 (( int )(( 2 56 . 0 / 3 . 0 ) + 0. 5) )
55
56 / t a b l e lookups f o r t r i g f u n c t i o n s /
57 # d e f i n e COS_COUNTS ( v ) COS ( C l i p H d g 3 6 0 ( ( v ) / OCOUNTPERGORDO ) )
58 # d e f i n e SIN_COUNTS ( v ) SIN ( C l i p H d g 3 6 0 ( ( v ) / OCOUNTPERGORDO ) )
59
60
61 /  GLOBALS  /
62 Move m;
63
64 /
65 Stuck f l a g s
t h e s e are l o c a t e d in servo . c but are not p r e s e n t
66 in simulation
67 /

66
 68 # i f n d e f SIMUL
69 extern i n t stuck ;
70 extern i n t stuckdir ;
71 extern i n t time ;
72 # else
73 extern i n t current_time ;
74 #endif
75
76
77 /  CODE  /
78
79
80 /
81 Ocontrol

82
83 T h i s f u n c t i o n g e t s c a l l e d o n c e e v e r y ms by t h e s e r v o l o o p on t h e r o b o t s
84 o r by t h e s i m u l a t o r . I t i s r e s p o n s i b l e f o r s e t t i n g t h e w h e e l v e l o c i t i e s
85 f o r t h e n e x t s e r v o i t e r a t i o n and f o r p e r f o r m i n g t h e p a t h i n t e g r a t i o n
86 p r o c e s s . To s e t t h e w h e e l v e l o c i t i e s i t b r a n c h e s t o t h e a p p r o p r i a t e
87 m o t i o n c o n t r o l schema a s s e t by t h e h i g h e r n a v i g a t i o n p r o c e s s e s .
88 /
89 v o i d O m n i C o n t r o l ( m o v e _ p t r move )
90 {
91 int dir ;
92
93 m = move ;
94
95 / Update t h e time v a r i a b l e /
96 # i f n d e f SIMUL
97 m
>t i m e = t i m e ;
98 # else
99 m
>t i m e = c u r r e n t _ t i m e ;
100 #endif
101
102
103
104 i f ( ! move
>l o o k u p s _ i n i t i a l i s e d )
105 return ;
106
107 / C a l l t h e m o t i o n c o n t r o l schema t h a t i s c u r r e n t l y b e i n g u s e d /
108
109 m
>m c t r l _ s c h e m a ( ) ;
110
111
112 # i f n d e f SIMUL
113
114 / If the robot is stuck
servo s a tura te s
then set the stuck flag /
115 m
>s t u c k = s t u c k ;
116 m
>s t u c k d i r = s t u c k d i r ;
117 #endif
118
119 /
120 Do t h e p a t h i n t e g r a t i o n . N o t e t h a t we s h o u l d c a l l t h i s
121 b e f o r e we c a l l t h e m o t o r f u n c t i o n s o t h a t a l l i s w e l l i n
122 t h e s i m u l a t o r when c o l l i d i n g w i t h o b s t a c l e s
123 /
124 O m n i I n t e g r a t e (m ) ;
125
126 / C a l c u l a t e t h e h e a d i n g u s e d when c a l c u l a t i n g t h e w h e e l v e l o c i t i e s /
127 d i r = C l i p H d g 3 6 0 ( (m
>f a c i n g + OCGON2 ) / OCOUNTPERGORDO ) ;
128
129 / C a l c u l a t e t h e wheel v e l o c i t i e s b a s e d on t h e d i r e c t i o n o f t r a v e l /
130 m
>v e l 1 = ( ( m
>v e l wheel1 [ d i r ] ) > > SIN_SHIFT ) + m
>r o t v e l ;
131 m
>v e l 2 = ( ( m
>v e l wheel2 [ d i r ] ) > > SIN_SHIFT ) + m
>r o t v e l ;
132 m
>v e l 3 = ( ( m
>v e l wheel3 [ d i r ] ) > > SIN_SHIFT ) + m
>r o t v e l ;
133
134 # i f d e f SIMUL
135 // DebugTxt ( " v1 :%d v2 :% d v3 :% d \ r \ n " , m
>v e l 1 , m
>v e l 2 , m
>v e l 3 ) ;
136 #endif
137
138 / S e t t h e a c t u a l wheel v e l o c i t i e s /
139 OmniMotor (m
>v e l 1 , m
>v e l 2 , m
>v e l 3 ) ;
140
141 /
142 t h i s w i l l do f o r now b u t n e e d t o e x t e n d f o r t h e two k i c k t y p e s .
143 note t h i s t h i s w i l l not f i r e the k i c k e r but only allow i t to f i r e d .
144 /
145 i f ( move
>a r m _ k i c k e r )
146 K i c k e r _ C o n t r o l _ I n t e r f a c e (TRUE ) ;
147 else
148 K i c k e r _ C o n t r o l _ I n t e r f a c e ( FALSE ) ;
149
150 }
151
152 /
153 OmniIntegrate

154
155 This f u n c t i o n performs the path i n t e g r a t i o n to maintain the robot ’ s i n t e r n a l
156 r e f e r e n c e o f p o s i t i o n and h e a d i n g and t o a l l o w f o r t h e p o s i t i o n and h e a d i n g
157 r e p o r t e d by t h e v i s i o n s y s t e m t o be r e c o n c i l e d w i t h t h e s e n s o r d e l a y .
158 /
159 v o i d O m n i I n t e g r a t e ( m o v e _ p t r m)
160 {
161
162 int vel1 , vel2 , vel3 ;
163 int velx , vely , ve lrot ;
164
165 / Read t h e c u r r e n t w h e e l v e l o c i t i e s f r o m t h e e n c o d e r s /
166 OmniEncoder (& v e l 1 , & v e l 2 , & v e l 3 ) ;
167
168 /
169 Perform the i n v e r s e k i n e m a t i c s to determine the p l a t f o r m v e l o c i t i e s
170 from t h e i n d i v i d u a l wheel v e l o c i t i e s .
171 /

67
172 velx = ( ( vel1
vel2 ) K256_ON_ROOT3) > > 8 ;
173 vely = ( ( vel1 + vel2
2 vel3 ) K256_ON_3 ) > > 8 ;
174 ve lrot = ( ( ( ( vel1 + vel2 + vel3 ) K256_ON_3 ) > > 8 ) ) ;
175
176 / C a l c u l a t e t h e new h e a d i n g f i r s t /
177 m
>hdg + = ( ( v e l r o t ) ) ;
178 i f ( m
>hdg > = ANG_360_COUNTS )
179 m
>hdg
= ANG_360_COUNTS ;
180 e l s e i f ( m
>hdg < 0 )
181 m
>hdg + = ANG_360_COUNTS ;
182
183 / Now c a l c u l a t e new p o s i t i o n /
184 m
>x + = ( v e l x COS_COUNTS(m
>hdg )
v e l y SIN_COUNTS (m
>hdg ) ) > > SIN_SHIFT ;
185 m
>y + = ( v e l x SIN_COUNTS (m
>hdg ) + v e l y COS_COUNTS(m
>hdg ) ) > > SIN_SHIFT ;
186
187 / Update t h e p o s i t i o n l i s t t o h a n d l e s e n s o r d e l a y /
188 (m
>x h e a d ) + + ;
189 i f ( m
>x h e a d > = m
>x p o s _ l i s t + LISTLEN )
190 m
>x h e a d = m
>x p o s _ l i s t ;
191 (m
>y h e a d ) + + ;
192 i f ( m
>y h e a d > = m
>y p o s _ l i s t + LISTLEN )
193 m
>y h e a d = m
>y p o s _ l i s t ;
194 (m
>h h e a d ) + + ;
195 i f ( m
>h h e a d > = m
>h p o s _ l i s t + LISTLEN )
196 m
>h h e a d = m
>h p o s _ l i s t ;
197 (m
>x h e a d ) = m
>x ;
198 (m
>y h e a d ) = m
>y ;
199 (m
>h h e a d ) = m
>hdg ;
200 }
201
202
203
204 /
205 MOTOR CONTROL SCHEMAS
206 /
207
208 /
209 omni_holy_shit

210
211 T h i s schema i m m e d i a t e l y b r i n g s t h e r o b o t t o a h a l t by s e t t i n g
212 the platform velocity to 0 .
213 /
214 void omni_holy_shit ( void )
215 {
216 m
>v e l = 0 ;
217 m
>r o t v e l = 0 ;
218 }
219
220 /
221 omni_halt_fn

222
223 T h i s schema b r i n g s t h e r o b o t t o a h a l t g r a c e f u l l y by r a m p i n g down
224 t h e r o b o t s v e l o c i t y u n t i l 0 a t t h e maximum a c c e l e r a t i o n
225 /
226
227 void omni_halt_fn ( void )
228 {
229 / Halts t ra n s l a t i o n a l velocity /
230 i f ( m
>v e l > m
>a c c )
231 m
>v e l
= m
>a c c ;
232 else
233 m
>v e l = 0 ;
234
235 / Halts the ro t a ti o na l velocity /
236 i f ( ABS (m
>r o t v e l ) > m
>t a c c ) {
237 i f ( m
>r o t v e l > 0 )
238 m
>r o t v e l
= m
>t a c c ;
239 else
240 m
>r o t v e l + = m
>t a c c ;
241 } else {
242 m
>r o t v e l = 0 ;
243 }
244 }
245
246
247 /
248 This f u n c t i o n i s r e s p o n s i b l e f o r t h e g e n e r a l motion of t h e r o b o t ,
249 c ontro lling the a cce lra tion , d e c e l e r a t i o n , h e a d i n g and f a c i n g
250 d i r e c t i o n o f t h e r o b o t . The d i s t a n c e t o t r a v e l , t h e change i n heading
251 and t h e c h a n g e i n f a c i n g a r e a l l s e t i n t h e f u n c t i o n setomnimove ( ) i n
252 Onschema . c
253 /
254 v o i d omni_move_fn ( v o i d )
255 {
256 int newv ;
257 int newvsq ;
258 int delta_facing ;
259
260
261 /
262 Reduce d e l t a f t o z e r o s m o o t h l y . F o r c a p p e d a c c e l e r a t i o n we n e e d t o a c c o u n t
263 t h e v e l o c i t y o f t h e r o b o t , a = ( v ^ 2 ) / r . I f you t h e n work o u t t h e c h a n g e i n
264 f a c i n g f o r t h e s p e c i f i e d r a d i u s you g e t d e l t a _ f a c i n g = a delt / v .
265 /
266
267 i f ( m
>d e l t a f ! = 0 ) {
268 // i f ( m
>v e l = = 0 ) { GFW
269 i f ( m
>v e l < 1 0 0 ) { / /DMB
270 d e l t a _ f a c i n g = m
>d e l t a f ;
271 } else {
272 d e l t a _ f a c i n g = IRAD2HDG ( SGN(m
>d e l t a f ) m
>d f a c c ) / m
>v e l ;
273 i f ( ABS ( d e l t a _ f a c i n g ) > ABS (m
>d e l t a f ) ) {
274 d e l t a _ f a c i n g = m
>d e l t a f ;
275 }

68
276 }
277 m
>d e l t a f
= d e l t a _ f a c i n g ;
278 m
>f a c i n g + = d e l t a _ f a c i n g ;
279 }
280
281 /
282 We w a n t t o r e a c h a g o a l l o c a t i o n a s s p e c i f i e d by t h e l e n g t h p a r a m e t e r .
283 At t h e same t i m e t h e maximum s p e e d m u s t r e m a i n b e l o w vx s o we ramp up o r
284 down t o r e a c h t h e l i n e a r v e l o c i t y a t amax s o a s t o m i n i m i z e t h e t i m e t o
285 r e d u c e l e n g t h b u t s t i l l s t a y w i t h i n bounds
286 /
287 i f ( m
>d e l t a l > 0 ) {
288 newv = m
>v e l + m
>a c c ;
289
290 /
291 We n e e d t o d i v i d e by 1 6 f o r a s c a l i n g f a c t o r t o p r e v e n t i n t o v e r f l o w , and by
292 2 a g a i n t o c o m p a r e ax < = v ^ 2 / 2 on t h e n e x t l i n e
293 /
294 newvsq = ( ( newv > > 2 ) ( newv > > 3 ) ) ;
295 i f ( ( m
>v f i n a l s q + ( m
>a c c ( m
>d e l t a l > > 4 ) ) ) < = newvsq )
296 newv = m
>v e l
m
>a c c ;
297
298 / I f we n e e d t o c h a n g e d i r e c t i o n , s l o w down /
299 i f ( ( ABS (m
>d e l t a f ) > ( OCOUNTPERGORDO4) ) & & (m
>v e l ! = 0 ) ) / / DMB c h a n g e d
300 newv = m
>v e l
m
>f a c c ;
301 i f ( newv > m
>maxv )
302 newv = m
>maxv ;
303 e l s e i f ( newv < 0 )
304 newv = 0 ;
305 m
>d e l t a l
= newv ;
306 i f ( m
>d e l t a l < 0 ) {
307 m
>d e l t a l = 0 ;
308 newv = 0 ;
309 }
310 } else {
311 newv = 0 ;
312 m
>d e l t a l = 0 ;
313 }
314
315 m
>v e l = newv ;
316
317
318 /
319 We w a n t t o r e a c h a d e s i r e d h e a d i n g a s p s p e c i f i e d by d e l t a h . The r o t a t i o n a l v e l o c i t y
320 i s ramped up t o m i n i m i s t h e t i m e t a k e n t o r e a c h t h e d e s i r e d h e a d i n g
321 /
322 i f ( ( m
>r o t v e l ! = 0 ) | | ( m
>d e l t a h ! = 0 ) ) {
323
324 / Attempt to a l i g n the robot with the given heading d i r e c t i o n /
325 i f ( m
>d e l t a h > 0 )
326 newv = m
>r o t v e l + m
>t a c c ;
327 e l s e i f ( m
>d e l t a h < 0 )
328 newv = m
>r o t v e l
m
>t a c c ;
329 else
330 newv = m
>r o t v e l ;
331
332 /
333 We n e e d t o d i v i d e by 1 6 f o r a s c a l i n g f a c t o r t o p r e v e n t i n t o v e r f l o w , and by
334 2 a g a i n t o c o m p a r e ax < = v ^ 2 / 2 on t h e n e x t l i n e
335 /
336 newvsq = ( ( newv > > 2 ) ( ABS ( newv ) > > 3 ) ) ;
337 i f ( m
>r v f i n a l s q + ( ( m
>t a c c m
>d e l t a h ) > > 4 ) < = newvsq )
338 newv = m
>r o t v e l
m
>t a c c ;
339 else
340 newv = m
>r o t v e l + m
>t a c c ;
341 i f ( newv > m
>m a x r o t v )
342 newv = m
>m a x r o t v ;
343 e l s e i f ( newv <
m
>m a x r o t v )
344 newv =
m
>m a x r o t v ;
345 m
>d e l t a h
= newv ;
346 m
>f a c i n g
= newv ;
347 m
>r o t v e l = newv ;
348 i f ( ( ABS (m
>r o t v e l ) < m
>t a c c ) & & (ABS (m
>d e l t a h ) < m
>t a c c ) ) {
349 m
>r o t v e l = 0 ;
350 m
>d e l t a h = 0 ;
351 }
352 }
353
354 }
355
356
357 /
358 omni_pivot_fn

359
360 T h i s f u n c t i o n w i l l move t h e r o b o t s o t h a t i t p i v o t s a r o u n d t h e b a l l
361 e n a b l i n g the robot to t u r n but s t i l l remain in c o n t r o l of the b a l l .
362 /
363 void omni_pivot_fn( void )
364 {
365
366 / To be w r i t t e n when t h e d r i b b l i n g h a r d w a r e i s i n s t a l l e d /
367
368 }

69
Appendix E

Small Size League Rules

For a full listing of the RoboCup 2001 Small Size League Rules visit:
http://arti.vub.ac.be/RoboCup/rules/rules.html

70