An Autonomous Vacuum Cleaner. Senior thesis by Stuart R. Harding

Transcription

i
Abstract
This thesis documents the development of an autonomous vacuum cleaner – RoboVac,
capable of vacuuming the floor of a previously unknown room. The thesis was started
in 1995 by Dowidiet and Duffy, who oversaw the construction of the robot body and
implemented a basic wall following behaviour using bump sensors. No significant
contributions to the thesis were made between 1995 and the start of this thesis.
The first task of the thesis was to recreate RoboVac to working condition. This
involoved designing and constructing a new main PCB and sonar sensors, and
implementing basic software functions to control the stepper motors, sonar sensors,
bump sensors and vacuum cleaner. The second task, which was the main objective of
this thesis, was to develop a control system that would enable RoboVac to efficiently
cover the floor surface of a room.
The control system developed for RoboVac uses a behavioural control
approach, rather than the planning approach used in most published work in the surface
coverage field. Behavioural control approaches differ from planning approaches in that
they do not use a centralised controller or an internal world map representation.
Instead, they use a number of cooperating behaviours, each of which handles a
particular aspect of the overall task. In RoboVac, these behaviours interact and control
the robot using an activation blackboard – a region of data which holds the sensor data,
motor commands and behaviour activation flags.
With two out of five designed behaviours implemented, the robot has been
shown to cover approximately two-thirds of the floor area of an empty room, the missed
area mainly due to robot dynamics and the sonar sensor limitations. Implementing the
remaining designed behaviours would enable the robot to cover the floor area of a room
containing obstacles, without missing areas to one side of the obstacle. With the
addition of extra suggested behaviours, RoboVac’s coverage performance could be
increased to nearly 100% of the floor area of a room containing obstacles. With
considerations given to the design of the RoboVac body, RoboVac will soon be
vacuuming the floors of real houses.
An Autonomous Vacuum Cleaner
ii
Acknowledgments
I would like to thank the following people for their help, guidance and support during
the course of this thesis.
Gordon Wyeth – for his supervision, tolerance and encouragement.
RoboVac – for being the ugliest thesis in the department.
James Kennedy – for listening to my constant ramblings when I needed to think,
and for rambling in return.
The other members of the Robotics lab – for making it such an interesting place
in which to work for a year.
And Kara Gregory – for always being there when needed.
iii
Table of Contents
ABSTRACT ................................................................................................................ I
ACKNOWLEDGMENTS ......................................................................................... II
TABLE OF CONTENTS..........................................................................................III
LIST OF TABLES AND FIGURES........................................................................ VI
OBJECTIVE STATEMENT ..................................................................................VII
CHAPTER 1 – INTRODUCTION .............................................................................1
1.1 Objective ...........................................................................................................1
1.2 Problem Description ........................................................................................1
1.3 Why an autonomous vacuum cleaner? ..........................................................2
1.4 Initial Status of RoboVac.................................................................................2
1.5 Thesis Goals and Restrictions .........................................................................3
1.5.1 Goals ..........................................................................................................3
1.5.2 Restrictions.................................................................................................3
1.6 Current Status of RoboVac .............................................................................4
1.7 What is to follow...............................................................................................4
CHAPTER 2 – SURFACE COVERAGE APPROACHES......................................5
2.1 Introduction ......................................................................................................5
2.2 Methods of Surface Coverage .........................................................................5
2.3 Planning Approaches .......................................................................................5
2.3.1 Planning strategy........................................................................................6
2.3.2 Robot Localisation Methods ......................................................................8
2.4 Behavioural Approaches ...............................................................................10
2.5 Random Motion Approach............................................................................11
iv
CHAPTER 3 – ROBOVAC SURFACE COVERAGE APPROACH ...................13
3.1 Introduction ....................................................................................................13
3.2 Choice of Approach........................................................................................13
3.3 A Behavioural Approach to Surface Coverage ...........................................13
3.3.1 Behavioural Components of Strip Filling ................................................14
3.3.2 Cavity Entry Behaviour............................................................................15
3.3.3 Detect Missed Area Behaviour ................................................................16
3.3.4 Missed Area Behaviour............................................................................16
3.3.5 Wall Following Behaviour.......................................................................17
CHAPTER 4 – ROBOVAC HARDWARE..............................................................18
4.1 Introduction ....................................................................................................18
4.2 CPU and Memory System .............................................................................19
4.3 Stepper Motor System ...................................................................................19
4.4 Sonar Sensor System......................................................................................20
4.5 Bump Sensor System......................................................................................21
4.6 Vacuum Cleaner Control System .................................................................21
4.7 Power Supply and Regulation.......................................................................21
4.8 Main Circuit Board........................................................................................21
CHAPTER 5 – ROBOVAC SOFTWARE...............................................................23
5.1 Introduction ....................................................................................................23
5.2 Hardware Control Functions ........................................................................23
5.2.1 Stepper Motor Control Functions.............................................................23
5.2.2 Sonar Sensing Functions ..........................................................................25
5.2.3 Bumper Sensing .......................................................................................26
5.3 Surface Coverage Functions..........................................................................26
5.3.1 Activation Blackboard .............................................................................26
5.3.2 Strip-Filling Behaviour ............................................................................27
5.3.3 Wall Following Behaviour.......................................................................28
5.3.4 Cavity Entry Behaviour............................................................................28
5.3.5 Detect Missed Area Behaviour ................................................................29
5.3.6 Fill Missed Area Behaviour .....................................................................29
v
CHAPTER 6 – RESULTS AND DISCUSSION......................................................30
6.1 Results .............................................................................................................30
6.2 Discussion........................................................................................................32
CHAPTER 7 – CONCLUSIONS AND FUTURE DIRECTIONS ........................34
7.1 Conclusions .....................................................................................................34
7.2 Future Directions ...........................................................................................34
REFERENCES...........................................................................................................35
APPENDIX A – SCHEMATIC DIAGRAMS .........................................................36
APPENDIX B – CODE LISTINGS ..........................................................................42
vi
List of Tables and Figures
Figure 2.1 – Planning Approach to Robot Intelligence.........................................7
Figure 2.2 – Regular Region....................................................................................7
Figure 2.3 – An Elementary Region........................................................................8
Figure 2.4 – Complementary regions .....................................................................8
Figure 2.5 – Type 1 and Type 2 rule sequences .....................................................9
Figure 2.6 – Behavioural Approach to Robot Intelligence .................................10
Figure 2.7 – The Eureka Robot Vac .....................................................................13
Figure 3.1 – Strip-filling behaviour ......................................................................15
Figure 3.2 – Cavity entry behaviour.....................................................................16
Figure 3.3 – Detect missed area behaviour ..........................................................17
Figure 3.4 – Missed area behaviour 2 ..................................................................17
Figure 4.1 – RoboVac.............................................................................................19
Figure 4.2 – Sonar Sensor Theory ........................................................................21
Figure 4.3 – RoboVac Sonar Sensor .....................................................................21
Figure 4.4 – Top down view of RoboVac main PCB...........................................23
Table 5.1 – Stepper Motor Control Functions.....................................................25
Table 5.2 – Surface Coverage Software Functions..............................................27
Table 5.3 – Activation Blackboard .......................................................................28
Figure 6.1 – Deviation while running straight.....................................................31
Figure 6.2 – Wall Following Test Performance ...................................................32
Figure 6.3 – Empty Room Test..............................................................................32
vii
Objective Statement
To construct an autonomous vacuum cleaner, capable of efficiently and
comprehensively vacuuming the floor of an unmodified, unknown room.
1
Chapter 1
Introduction
1.1
Objective
The objective of this thesis was “to construct an autonomous vacuum cleaner,
capable of efficiently and comprehensively vacuuming the floor of an unmodified,
unknown room”. This objective statement placed several conditions on the design of
the vacuum cleaner:
•
“Autonomous” means fully self-contained and self-controlled. No power or
remote control leads are used. All sensing and control is done on-board.
•
“Efficiently and comprehensively vacuum” means that some intelligence must be
displayed in the way the vacuum cleaner moves about the room. All accessible
space on the floor must be covered in a power-conserving manner. This suggests
that some mapping is required, rather than random motion.
•
“Unmodified, unknown room” means the room should not have any artificial
navigational aids, such as beacons or tracks. The vacuum cleaner should not have
any pre-constructed maps of the room. Objects in the room should be able to stay
in place i.e. the vacuum cleaner should avoid obstacles.
As the vacuum cleaner is autonomous, the user interface is minimal. An On button
starts the vacuum cleaner, and, when the vacuum cleaner is finished, it simply turns
itself off. Later developments may add vacuum cleaner-like selections to the user
interface.
1.2 Problem Description
The autonomous vacuum cleaner has much in common with autonomous mobile robot
technology. The vacuum cleaner must contain locomotion, sensing, navigation, and
power systems like most mobile robots, as well as a vacuum cleaner system. Due to
this similarity, the awkwardly named autonomous vacuum cleaner was re-titled
RoboVac.
2
Introduction
The step from random movement to intelligent surface coverage is no small one.
Efficient surface coverage requires knowledge of the area that RoboVac has already
covered, and the area still to be covered. That is, instead of merely reacting to the
environment, RoboVac needs to sense the environment and plan its movements – it
needs to be proactive rather than reactive. This requires some sort of mapping and
localisation system.
What makes RoboVac different from other mobile robots is its required
navigational behaviour. It is interesting to note that while much research has been done
in the field of mobile robot path planning, or surface traversal methods, the field of
surface coverage has only recently been addressed.
1.3 Why an autonomous vacuum cleaner?
Much work has been done in the field of mobile robotics and artificial intelligence in
general. Much of this work focuses on making a robot or electronic system seem
intelligent by preprogramming reactions to various stimuli in an artificial and very
structured world. This thesis provides a practical and currently achievable application
for mobile robotics in the ‘real’ world.
1.4 Initial Status of RoboVac
RoboVac was designed and built in 1995 as a thesis project by Dowideit and Duffy
[Dowideit, 1995]. Dowideit and Duffy oversaw the construction of the mechanical
‘cart’ – the body of RoboVac – and implemented control to give RoboVac a basic wallfollowing bump-and-turn behaviour. The cart contained a 12V lead-acid battery, two
bi-phase stepper motors driving the rear wheels, a sonar array mounted toward the front
of the cart, bump sensors on all sides, and a dustbuster to simulate the vacuum cleaner.
At the end of 1995, Dowideit and Duffy had constructed a fully autonomous
mobile robot, with basic reactive behaviours.
Unfortunately they were unable to
incorporate the sonar sensors into the design due to time limitations.
RoboVac was then forgotten for two years. In 1998, Cheng did some superficial
research into path planning techniques for RoboVac [Cheng, 1998]. However, Cheng
missed the point of the autonomous vacuum cleaner – he was researching surface
traversal techniques, not surface coverage.
Introduction
3
At the start of this thesis, the physical state of RoboVac was grim. All that
remained was the cart, with stepper motors, wheels, bump sensors and dustbuster. The
sonar array was empty, and in the intervening years the PCBs used to control RoboVac
had deteriorated to an unusable state. RoboVac had a body but nothing to control that
body.
1.5 Thesis Goals and Restrictions
1.5.1 Goals
As a continuing thesis, the goals were set as follows:
1. RoboVac 1.0 – Revised Edition. Recreate RoboVac to working condition, as it
was in 1995 [Dowideit, 1995]. This entails designing a new PCB for control of all
systems of RoboVac, and software to control the basic functions of RoboVac
implemented in this version. These are motor control, vacuum cleaner control,
sonar and bump sensing and power supply. These basic functions will form the core
of the RoboVac behaviours.
2. RoboVac 2.0. Implement intelligent surface-coverage behaviour. This will be the
main focus of this thesis. A mapping and navigation planning system will need to
be developed to implement the surface coverage behaviour as previously described.
Using the sonar sensor range data, a map will be dynamically built using one of
various methods researched. This final layer of behaviours will give RoboVac
intelligence and allow it to act proactively.
1.5.2 Restrictions
With any list of goals, must come a list of restrictions – what will not be addressed in
the course of this thesis.
1. Efficiency of on-board vacuum cleaner. As this is a prototype, the vacuum cleaner
on board is for demonstration purposes only.
Therefore, the efficiency of the
vacuum action will not be addressed.
2. Shape and size of room. Due to practical limitations, involving additive odometry
errors and sensor range, the size of the room will be limited. A room no larger than
6m square will be used for testing. The room will be of basic rectangle shapes, ie.
4
Introduction
will have only 90° or 270° corners. As a down-looking sensor has yet to be
implemented, there will be no cliffs (eg. stairs) in the testing room.
3. Obstacles. There will only be a limited number of obstacles in the testing room.
These obstacles will be of sufficient height to be detected by the sonar sensors,
and/or bump sensors.
4. Floor Surface. The floor of the testing room will be hard. Later versions of
RoboVac may have carpet-traversing wheels, but the current version has wheels
only suitable for hard floors.
1.6 Current Status of RoboVac
At the completion of this thesis, the following goals had been achieved –
1. RoboVac 1.0 – Revised Edition. A new main PCB and new sonar sensors were
designed and constructed. Software was written and implemented to control the
basic hardware functions of RoboVac, these being motor control, sonar sensing,
bump sensing and vacuum control. At the completion of this goal, RoboVac was a
functional mobile robot, able to move and sense its surroundings.
2. RoboVac 2.0. A surface coverage control method was developed for RoboVac.
Software was written and implemented for this control method. At the completion
of this goal, RoboVac met the requirements in the objective statement satisfactorily,
subject to the restrictions described in Section 1.5.2.
1.7 What is to follow
Chapter 2 examines previous work, covering relevant areas of published research.
Chapter 3 develops a surface coverage control method for RoboVac chosen after an
extensive search of the literature.
Chapter 4 describes the RoboVac hardware.
Chapter 5 describes the RoboVac software, both basic hardware-control functions and
the surface coverage control software.
Chapter 6 summarises the main concepts described throughout the report.
Chapter 7 concludes the report by outlining a course for future work on RoboVac, and
summarises
the
main
concepts
described
throughout
the
report.
5
Chapter 2
Surface Coverage Approaches
2.1 Introduction
The objective of this thesis was to construct an autonomous vacuum cleaner, capable of
efficiently and comprehensively vacuuming the floor of an unmodified, unknown room.
This objective statement can be decomposed into two parts – the construction of the
vacuum cleaner, and the development of its control system, which enables it to achieve
its task. This chapter will review previous work relevant to the development of the
vacuum cleaner control system, while Chapter 3 will go on to develop a control strategy
for RoboVac.
2.2 Methods of Surface Coverage
Vacuum cleaning the floor of a room entails covering every accessible region of the
floor. Previous surface coverage methods have concentrated on a planning approach –
keeping a world map from which movement may be planned. Other possible methods
include a behavioural approach – reacting to the world using cooperating
‘behaviours’, or a ‘creepy crawly’-like random motion approach - mentioned here
only because a product using this approach is soon to be marketed.
The following sections review previous work in these approach areas.
2.3 Planning Approaches
Planning approaches to surface coverage use a world map – either pre-programmed or
developed as the robot explores – to plan and execute movement, which the robot will
follow to cover the whole floor (see Figure 2.1). Planning approaches theoretically
achieve a high efficiency, though keeping a world map requires high maintenance,
accurate sensors and a method of localising the robot within the world map. The
6
following two sections review planning strategies and their localisation methods,
respectively.
World Map
Sensors
Robot Control
Actuators
Figure 2.1 – Planning Approach to Robot Intelligence.
Information from the sensors is used to update the world
map, and that world map is used to plan action.
2.3.1 Planning strategy
The ‘regular regions’ strategy, illustrated in Figure
2.2, used a back-and-forth strip-filling process to
cover a previously unexplored environment [Cao et
al, 1988]. Cao defined an ‘elementary region’ as an
area that could be completely covered with a
continuous strip-filling process, while a ‘regular
region’ was defined as a region containing obstacles.
While the robot was fulfilling a strip-filling process
Figure 2.2 – Regular Region. The regular
region is divided into elementary regions
based on ‘cuts’ from obstacles. Each
elementary region is filled successively.
and encountered an obstacle, it continued to strip-fill down one side of the obstacle until
it encountered the lower extreme. At the lower extreme, it used a boundary-following
operation to follow the unfilled side of the obstacle round to the upper extreme, then
continued the strip-filling process. This strategy dynamically ‘cut’ the regular region
into elementary sub-regions, without the need to keep a global map.
The ‘complementary regions’ strategy took the regular regions approach a step further
[Suarez et al, 1995].
This strategy again dynamically split the overall pace into
elementary regions (Figure 2.3). Unlike the linear regular regions strategy, however,
7
the complementary regions strategy used a
recursive algorithm to cover the elementary
regions.
In the complementary regions strategy,
when the robot started it first defined a ‘base
line’ of the current elementary region, by moving
forward along the wall until it encountered a
front obstacle. It then used a strip-filling process
to fill the region, using side sonar to find other,
complementary regions – delimited by an
occupied-empty transition on the side sonar
Figure 2.3 – An Elementary Region is a region
that may be covered with one continuous stripfilling process. The baseline B defines the
beginning and end of the strip-filling process for
that elementary region.
followed by an empty-occupied one. The new
complementary region base line was defined by
the starting and ending points of the cavity entry.
As the robot moved along the base line, the side
sonar produced depth measurements, giving an
indication of the region’s geometry. Once the
region was defined the robot moved back to the
start point of the base line and proceeded to stripfill it (Figure 2.4). If a new region was found, the
process was recursively repeated.
Once each
child region was finished, the robot continued to
Figure 2.4 – Complementary regions. Each elementary
region is defined by a base line (B), and then strip-filled
perpendicular to the baseline. When new regions are
discovered, the process is recursively repeated.
strip-fill the parent region. This strategy resulted in a more robust process than regular
regions, allowing an irregularly shaped outer boundary.
Hofner et al proposed different strategies for robots with different kinematics to give the
smoothest and most efficient coverage pattern [Hofner et al, 1994; Hofner et al, 1998].
Given a pre-programmed region boundary, the robot pre-planned a sequence of parallel,
overlapping tracks connected by track change manoeuvres – U-turns and side shifts – to
fulfil the coverage task.
Each of these manoeuvres was planned as a series of
intermediate goal frames (∆x, ∆y, ∆θ) by calculating possible collision points with the
walls. Kinematics determined which track change manoeuvre was used. U-turns were
preferable, as no stopping was required. Two rule sequences were used. The first –
Type 1 – was used for small, agile robots with a turn radius smaller than the robot’s
8
width. Type 1 used a serpentine pattern of track
motions and U-turns (see Figure 2.5a). Type 2
rule sequence was used for robots with a turn
radius larger than their diameter, meaning that
the parallel tracks did not overlap.
This
sequence uses an ‘interlaced snake-like trail’
(Figure 2.5b), which started a serpentine
sequence and then went back over the area
which
the
previous
tracks
had
missed.
Obstacles were avoided by modifying the
intermediate goals.
Rather than pre-planning tracks based on a preprogrammed region boundary, Lang et al
determined tracks by following the outer
boundary of the region [Lang et al, 1998]. At
Figure 2.5 – Type 1 and Type 2 rule sequences. (a)
Type 1 is used for robots with small turn radius. (b) Type
2 is used for robots with larger turn radius.
quantised y intervals around the boundary, x and y coordinates were recorded. These
coordinates were then paired up by y coordinates to give a set of parallel tracks. These
tracks were then followed one by one. If an obstacle was encountered while traversing
a track, the robot reverted back to its wall-following state and traced the complete
outline of the obstacle.
While it did this, it updated the track list with the new
information, splitting any tracks which it crossed in the tracing state. It then continued
traversing tracks until all the tracks were finished.
2.3.2 Robot Localisation Methods
To implement a planning approach, a robot must have some method of accurately
localising itself within its world map. Two basic localising methods are usually used –
relative positioning and absolute positioning.
Relative positioning involves robot
odometry – calculating through motor speed or wheel position how far the robot has
moved. The problem with relative positioning is that wheels slip while traversing,
giving errors in both distance travelled and also relative heading. Borenstein detailed a
method for systemic correction of odometry errors [Borenstein, 1996], yet still the
9
possible error due to wheel slip quickly becomes unbounded unless another method of
positioning is used for error correction. This is where absolute positioning is used.
Absolute positioning seeks to position the robot within the region using data
gained from ranged sensors. This data is compared with previous knowledge of the
region the robot has either gained while exploring, or has been given before operation.
There are a few methods of doing this.
The complimentary regions strategy used a ‘re-positioning grid’ to store sensor scan
data [Suarez et al, 1995].
Each cell in the grid was used to store sensor scan
information obtained from within that cell. New sensor scans were compared with
known ones in the estimated area. Also, new scans were compared with neighbouring
ones for continuity. Scans matched with a grid cell are used for position correction.
New cells are added to the re-positioning grid.
Robot orientation is obtained by
assuming the vehicles moves in a straight line between successive reposition points, and
then calculating the vector joining successive reposition points (xi-1, yi-1) and (xi, yi).
Hofner et al used a method that extracted contours from the pre-programmed geometric
contour map that could be visible from the estimated position of the robot [Hofner et al,
1994]. Contours were then obtained from a sonar scan of the area. The map contours
were used to build a correspondence tree, in which each branch stored possible
correspondences of map contours and sensor contours, based on distance, orientation
and relative position. A full tree (ie. a tree with all sensor contours matched with map
contours) indicated a possible absolute location. The most reliable match was used to
determine the current position of the robot.
While both of the preceding methods were based on geometric maps, Barron used a
rather different approach [Wyeth & Barron, 1994]. Barron used a map consisting of
nodes, each of which was a landmark consisting of sensor reading data. For instance, a
corner landmark was two sides within a certain range, and two sides out of range.
These landmarks were then connected with motion vectors.
implementing this method as a sequence of instructions.
Barron succeeded in
10
For example, a sequence could be –
1. Follow the left wall at a distance of 100mm until a frontal range of 100mm is found.
2. Turn right and follow the left wall at 100mm until the left wall drops away (no left
range reading).
3. Turn left and follow left wall at 100mm until ....
Barron described this method as effectively turning the environment into a finite state
machine, where states were actual positions in space, and the transitions between them
consisted of physical motion. This seems to be a good way to implement any of the
above surface coverage algorithms, as the transitional movements provides constant
position update (ie. follow wall at a certain distance).
Also, each of the surface
coverage algorithms discussed involved a very state-like architecture – move forward
until a frontal obstruction is detected, U-turn, move forward until a frontal obstruction is
detected, follow wall until...
2.4 Behavioural Approaches
The previous planning approaches rely heavily on a well-structured environment, with a
world map representation, and apart from needing accurate sensors and high
maintenance, they typically do not react especially well to dynamic obstacles or
unexpected situations. Behavioural approaches try to eliminate this dependency on
internal, high maintenance models of the world by purely reacting to the world without
resorting to the internal model. This reactive technique uses the world as its own
model.
Behavioural approaches, rather than being controlled fully by one central
controller, merge different, simple behaviours to achieve a seemingly higher task (see
Figure 2.6). ARNI, a robotic hexapod, merged three simple behaviours with varying
priorities, to emulate the behavioural pattern of an American cockroach [Montiel,
1996]. The three behaviours – wandering, feeding (find and approach a specific IR
beacon) and obstacle avoidance (using SONAR and antennae) – interacted to provide an
intelligent-seeming system with a seemingly specific goal. With feeding having a
higher priority than the other behaviours, ARNI would wander, avoiding obstacles until
it saw food (the beacon) and would then charge toward the beacon.
11
Behaviour n
Behaviour 3
Sensors
Actuators
e.g. Object Location
Behaviour 2
e.g. Path Finding
Behaviour 1
e.g. Obstacle Avoidance
Figure 2.6 – Behavioural Approach to Robot Intelligence.
Sensory information is used by behaviours to determine an
action. The behaviours then compete to take control of the
actuation.
More complex tasks can also be implemented using this behavioural approach. Herbert,
a wheeled mobile robot with a robotic arm mounted on top, used a ‘colony’ of
behaviours in its task of navigating a real office environment, finding soda cans, picking
them up and returning them to a home point [Connell, 1993]. Each behaviour, covering
such tasks as wall following, can location, obstacle avoidance and can retrieval, was
used to add to the robot’s capabilities, and the exclusion or failure of a behaviour did
not mean the complete failure of the system, just that the system operated with
downgraded performance. On the other hand, addition of behaviours caused the robot
to operate with increased performance.
Herbert was controlled by 40 behaviours implemented on 24 separate
processors. Each of the behaviours operated on different aspects of the robot’s task.
Herbert was unique, in that it was almost completely stateless (no memory), and there
was absolutely no inter-behaviour communication, except through the outside world.
That is, each behaviour competed to control the robot’s actuators (wheels, arm and
gripper) and the only input to each behaviour was some form of sensory input. This
extreme model was developed as an example of the capabilities of a behavioural
system.
2.5 Random Motion Approach
The Eureka Company has developed and will soon be marketing an autonomous
vacuum cleaner, shown in Figure 2.7[Eureka, 1999].
This robot first follows the
boundary of the room. When it has completed a circuit of the room, it starts a random
‘creepy-crawly’-like motion – when it encounters an obstacle with its radar sensors, it
‘bounces’ away in a random direction.
12
This approach was included in this review as a comparison of the current
technology being marketed in this field. This process would probably cover the floor of
a room. With no control method, however, the system has no way of acting efficiently.
Figure 2.7 – The Eureka Robot Vac.
13
Chapter 3
RoboVac Surface Coverage Approach
3.1 Introduction
After reviewing previous work in the field of surface coverage control approaches, an
approach was developed which was suitable and currently implementable on RoboVac.
RoboVac’s hardware (described in Chapter 4), necessarily places limits on the
sophistication of the control approach used. This chapter describes control method
developed for RoboVac. Chapter 5 goes on to describe the implementation of this
control approach in RoboVac 2.0.
3.2 Choice of Approach
The two control approaches researched were planning approaches and behavioural
approaches. The majority of previous work in the field of surface coverage has centred
on planning approaches. While a planning approach seemed most capable of covering
the entire floor efficiently, it was decided that the RoboVac sensors, both internal
(distance measurement) and external (sonar sensors), were not capable of maintaining
an accurate and hence useful world map, rendering the planning approach unachievable
in RoboVac’s current configuration. The application of a behavioural approach to
surface coverage had not been widely researched, so it was decided that RoboVac
would explore this area.
3.3 A Behavioural Approach to Surface Coverage
As described in Section 2.4, behavioural systems are composed of a number of
individual behaviours, each behaviour acting to cover one aspect of the robot control.
Seemingly complex tasks can be performed using a collection of interacting behaviours.
So what behaviours will pertain to surface coverage? As was done with Herbert (see
Section 2.4), the behaviours were built up, each adding to the functionality of the robot.
14
Each of the behaviours is run every update period.
This update period is
determined by the update rate of the major sensors – in this case, the sonar sensors.
Each behaviour checks for certain activation conditions. If the appropriate activation
conditions are present, the behaviour places its behavioural component onto the
activation blackboard. Note that behaviours can subsume others by modifying internal
activation flags, and can signal other behaviours to start in the same way. This is in
contrast to Herbert (Section 2.4), in which behaviours had no internal interaction. The
activation blackboard is a set of global data, which contains the latest sensor data,
internal activation flags, and motor commands.
The following sections describe the behaviours developed to control RoboVac’s
surface coverage task.
3.3.1 Behavioural Components of Strip Filling
The first behaviour that a vacuum cleaner should exhibit is strip filling – covering an
area with a continuous back-and-forth movement, as in Figure 3.1. This is the core
behaviour RoboVac exhibits and is made up of smaller components, which combine to
give the overall strip-filling behaviour. Each component takes certain inputs from the
activation blackboard – whether sensory or
internal – and attempts to take control of the
robot when those inputs are in a certain
configuration. Note that this is not a purely
behavioural
method.
Pure
behavioural
systems have typically been implemented in a
multi-processor configuration, whereas all of
RoboVac’s behaviours are implemented on the
Figure 3.1 – Strip-filling behaviour.
Strip filling entails covering the area
using a back-and-forth motion.
one processor.
Strip filling requires the following sequence of events –
1. Move in a straight line until a boundary or obstacle is reached.
2. Execute a 180° turn, ending up beside the ‘strip’ just traversed.
3. Move in a straight line until a boundary or obstacle is reached.
4. Execute a 180° turn in the opposite direction to 2, ending up beside the ‘strip’ just
traversed and in a new ‘strip’.
5. Repeat from 3.
15
The following behaviour components are used to implement strip filling –
•
Move straight forward. The primary behaviour in the strip-filling process.
•
Monitor front sensor for obstacle. When obstacle is close, signal turn.
•
Monitor side sensors for obstructions that will inhibit turning. This is necessary
for RoboVac due to RoboVac’s turning circle being larger than its width, causing
it to swing out considerably while turning. This is implemented as a separate
behaviour as it runs concurrently with the move straight forward behaviour. It
needs to keep a memory of side obstructions over a distance of two RoboVac
body lengths.
•
Turn in opposite direction to previous turn, unless turning obstruction is present.
If obstruction is present, turn in opposite direction and signal failed turn.
As can be seen from the descriptions of each behavioural component, they are not
entirely stateless. This is in contrast to Herbert, the pure behavioural system described
in Section 2.4.
These strip filling behaviours will work perfectly in a rectangular room with no
obstacles. With only the strip filling behaviours implemented, RoboVac will fully
cover an empty rectangular room, assuming that it is started in one corner. If an
obstacle is present, an area to one side of the obstacle will be missed, as the strip-filling
behaviour will only cover the area on one side of the obstacle. As more behaviours are
added, however, RoboVac’s abilities will increase.
3.3.2 Cavity Entry Behaviour
When a failed turn has been signalled, the cavityentry behaviour comes into effect.
While the
robot is retracing the track it has just completed,
this behaviour monitors the side sensor for any
‘cavity’ in the obstruction that foiled its turn.
If a cavity is found, the robot performs a
90° turn into the cavity, and then an opposite 90°
turn to align itself with its previous motion
direction. The strip filling operation is then continued.
Figure 3.2 – Cavity entry behaviour. The
failed turn signals the cavity entry behaviour
(red) which then retraces and looks for a cavity.
16
3.3.3 Detect Missed Area Behaviour
The missed area behaviour monitors and records the fill-direction side sensor readings
along each strip. When each strip is completed, this behaviour checks the previous strip
readings and checks to see if an area has been missed. This is done based on the length
of the previous strip compared to the length of the current strip (green), and also taking
into account the side sensor readings of the previous strip (red), as shown in Figure 3.3.
If an area has been missed, the missed area flag
is set.
3.3.4 Missed Area Behaviour
The missed area behaviour handles two types
of missed area – a missed area to one side of a
free-standing obstacle, as in Figure 3.3, and a
missed area to one side of an obstacle against
the wall, shown in Figure 3.4.
The
two
cases
have
different
recognition criteria. The free-standing obstacle
Figure 3.3 – Detect missed area behaviour .
The sonar readings and length of the previous
strip (red) compared to the length of the new strip
(green) indicates that an area has been missed.
Missed Area Behaviour 1 – The first missed
area case (yellow) traces down the opposite side
of the free-standing obstacle and then continues
strip filling.
case is signalled when an obstacle is passed on
the already-filled side of the robot, and the
missed area flag is active. The obstacle against
the wall case is signalled when the cavity entry
behaviour signals complete and the missed area
flag is active.
Note that these two cases do not cover all
possibilities, and some areas will be missed for
certain obstacle configurations.
This problem
may be fixed, however, with additional missed
area behaviours.
Figure 3.4 – Missed area behaviour 2 –
obstacle against wall. The second missed area
case (yellow) retraces the path back to the missed
area and then continues strip-filling.
17
3.3.5 Wall Following Behaviour
Due to wheel slippage and imprecise wheel diameters, errors in heading are likely to
occur when strips are traversed, even when the robot is supposedly moving straight. If
these errors are left unchecked, they will grow unbounded, rendering any position
estimation useless.
Wall following behaviour constantly measures the side sonar
reading, and updates the robot’s heading to keep it at a certain distance from a wall.
Only relatively small changes are accepted. Large discrepancies are ignored, as they
would most probably be caused by irregularities in the wall surface (such as doorways)
or obstacles being passed.
18
Chapter 4
RoboVac Hardware
4.1 Introduction
Before we can develop the fully functional vacuum cleaner described in the last chapter,
we must first examine the RoboVac hardware, which the higher-level behaviours will
use to interact with the world around it. RoboVac’s hardware is what allows it to move,
sense, and so interact with its environment. The software may know what RoboVac
should do, but this is not overly useful without the sonar sensors’ ‘view’ of the world,
and RoboVac would not be mobile without its stepper motors. This section gives an
overview of these hardware systems.
Figure 4.1 – RoboVac
RoboVac consists of the following hardware systems:
1. CPU and Memory System
2. Stepper Motor System
3. Sonar Sensor System
4. Bump Sensor System
5. Vacuum Cleaner Control System
6. Power Supply and Regulation
7. Main Circuit Board
RoboVac Hardware
19
4.2 CPU and Memory System
The processor used in RoboVac was the Intel 80C196KC. This processor’s advantage
over the other processors in use in this department was the 80C196KC’s high speed
input/output port, necessary for the sonar sensors. This processor was also used by
Dowideit [Dowideit, 1995]. The processor uses a 32kb EEPROM for program storage,
and a 32kb RAM for temporary data storage, on an 8-bit data bus. The software
developed for RoboVac was compiled using the HiTech 80196 cross-compiler, and
tested using the HiTech Lucifer monitor program.
4.3 Stepper Motor System
RoboVac uses two 9 volt bipolar stepper motors for locomotion. The wheels are
arranged in a wheelchair configuration, with the driven wheels at the rear of the cart,
and a free bearing at the front. This allows RoboVac to turn by applying different
speeds to each motor. Stepper motors were used because, unlike DC motors, stepper
motors operate in discrete steps, requiring no position feedback, and have high torque at
low speeds.
To simplify the processor interface required to control the stepper motors, two
pairs of stepper motor controllers (L297, SGS-THOMSON) and dual full-bridge drivers
(L298, SGS-THOMSON) were used. The L297 provides the complex phase output
needed to control the stepper motors, while the L298 converts the logic-level signals
from the L297 to high-current, high-voltage signals to power the motors [SGSTHOMSON, 1995]. Each L297 takes four signals from the processor – direction, full or
half step, enable and clock. When the first three are set appropriately, the period of the
clock determines the speed of rotation of the motor. Distance travelled can then be
computed using the number of steps taken, and the constant step distance.
One problem encountered with the stepper motor system was the heat generated
by the pair of L298’s. If foreseen, this problem could be countered by placing the
L298’s at the edge of the PCB, so they could be mounted on the aluminium body of the
robot.
20
RoboVac Hardware
4.4 Sonar Sensor System
Sonar Sensor Theory
Sonar sensors transmit a pulse of ultrasonic energy and measure the time until the signal
returns. The time of flight is proportional to the distance to the nearest object. If
conversion to metric dimensions is required, the following equation may be used.
Distance (metres) = TOF(sec) x 344m/s
where 344m/s is the approximate speed of sound in air.
Sonar sensors have a beam width
of 30°, meaning that the first object in a
30° arc in front of the sensor will cause a
return signal, as shown in Figure 4.2.
The precise direction of the object can
not be determined within the 30° beam.
This makes sonar sensors useful for
general-purpose range finding, but does
not suit them for the task of accurate
world map maintenance.
Figure 4.2 – Sonar Sensor Theory. The beam
width of a sonar sensor is approximately 30°. The
first object (green) will return a reading (red), while
the other objects will be ignored. The precise
direction of the closest object within the 30° beam is
unknown.
RoboVac Sonar Sensors
The new design of RoboVac’s sonar sensor
circuit boards uses the schematic originally
designed by Wyeth [Barron, 1992].
Using
surface mount devices, the sonar sensor has been
designed as one transceiver module. As shown
in Figure 4.3, all devices are mounted securely
on the PCB, including the sonar transducers.
Figure 4.3 – RoboVac Sonar Sensor
The result is a small, lightweight, compact design, with no extraneous wires.
The sonar sensors are interfaced to the processor’s high speed input/output port
through a set of multiplexers, allowing up to sixteen sonar sensors to be interfaced to
the processor. To send a pulse of energy, the processor activates the transmit line on the
sonar board and waits for the reply on the receive line. The longer the transmit line is
activated, the stronger the pulse of energy and the larger distance the sonar is able to
detect. However, when the pulse is being transmitted, the receiver must be ignored so
RoboVac Hardware
21
as to not receive the pulse on the way out. A longer pulse means that the minimum
detectable distance is larger. RoboVac’s sonar sensors have a minimum range of
200mm and a maximum range of 3000mm.
4.5 Bump Sensor System
The bump sensor system is a last-resort sensing system to detect obstacles which the
sonar did not detect for any reason. The bump sensor system consists of four switches –
left, right, front, back – attached to the ‘bumper bars’ around the RoboVac body. These
sensors are interfaced to the processor on Port 0, leaving four inputs free.
4.6 Vacuum Cleaner Control System
The vacuum cleaner control system controls the activation of the on-board vacuum.
The vacuum is operated at 5 volts by pulse-width-modulating (PWM) the 12-volt
battery voltage. PWM switches the voltage source at high speed (19kHz) while varying
the duty cycle to achieve the desired voltage. The switching is achieved through a
logic-level MOSFET controlled by the PWM0 output of the processor. To achieve 5
Volts, the duty cycle used is 40% (40% of 12V = 5V). To turn the vacuum cleaner off,
0% duty cycle is used.
4.7 Power Supply and Regulation
RoboVac is powered by a 12 volt, 6.6Ah sealed lead-acid battery.
A National
Semiconductor 78L05 linear regulator is used to convert the 12 volts to the 5 volts
required by the processor and other components. With the stepper motors running, the
battery lasts around one hour before needing recharging.
4.8 Main Circuit Board
The PCB used by Dowideit was in an unusable state at the start of this thesis.
Components were missing and some PCB tracks had lifted from the board. The new
PCB is derived from the design used by Dowideit. Figure 4.4 shows the current
RoboVac circuit board.
22
RoboVac Hardware
The circuit board consists of the following sections –
A. CPU and memory
B. Sonar sensor interface
C. Stepper motor control and interface
D. Power regulation and vacuum control
E. Bump Sensor Interface
F. PC Communication Interface
The heat sinks required for the stepper motor section can be seen at the bottom of the
PCB.
Figure 4.4 Top down view of RoboVac main PCB
23
Chapter 5
RoboVac Software
5.1 Introduction
The RoboVac software is what enables it to fulfil its specified task – vacuum the floor.
The software described here is divided into two sections – the basic hardware control
functions, and the surface coverage control functions.
5.2 Hardware Control Functions
The basic hardware control functions form a basic core level of software, which the
higher level behaviours use to achieve their tasks. These functions serve to abstract the
actual details of motor control and sensor use from these higher level behaviours. With
these hardware control functions, RoboVac may be called a functional mobile robot.
These hardware control functions consist of the following.
•
Stepper Motor Control
•
Sonar Sensing
•
Bumper Sensing
5.2.1 Stepper Motor Control Functions
The stepper motors are controlled by four signals. These are Clock, full/half step,
direction and enabled. The L297 stepper controller is attached to PORT 1. The
controller is set to full step mode in the start-up initialisation sequence. The motors are
kept enabled while RoboVac is running. This leaves the motor control functions to
handle direction and clock.
The clock signal is used to step the motors. Each transition of the clock signal
turns the stepper motor one step in the direction indicated by the direction signal. The
frequency of the clock signal transitions determines the angular speed of the wheels and
RoboVac Software
24
hence the speed of the robot. Also, the difference in the distance travelled by each
wheel can be used to determine the robot’s relative heading.
As the stepper motors need a constant periodic update, a system time interrupt
was set up, which increments once every millisecond. Every system time ‘tick’, the
stepper toggle, stepper ramp and stepper turn tasks are called.
Stepper Control Functions
Stepper Toggle Tasks
left_stepper_task()
right_stepper_task()
Stepper Ramping
- calls ramp_left_stepper(speed, direction)
ramp_right_stepper(speed, direction)
ramp_both_steppers(speed, direction)
- tasks ramp_left_task()
ramp_right_task()
Stepper Turning
- calls turn_to_angle(angle)
- tasks turn_task(LEFT)
turn_task(RIGHT)
General stepper calls
set_left_stepper(speed, direction)
set_right_stepper(speed, direction)
Table 5.1 – Stepper Motor Control Functions. These functions
form the basic stepper motor control. The functions are divided into
calls – used by the program to request an action – and tasks – period
update functions.
The stepper motor functions (see Table 5.1) are implemented in the file
‘motors.c’.
Stepper Toggle Tasks
The toggle tasks - left_stepper_task and right_stepper_task - check whether either of the
stepper motor are scheduled to be stepped, by checking the global variables
left_stepper_time or right_stepper_time against the current system time. If they match,
the relevant stepper clock signal is switched, and the current left_stepper_period or
right_stepper_period is added to the relevant time variable hence causing a periodic
switching of each stepper’s clock signal at a rate between 25 and 50Hz. This rate gives
the robot speeds up to 1m/s. The distance each wheel has gone is recorded in the two
variables left_stepper_count and right_stepper_count.
Stepper Ramp Tasks
The stepper motors can not jump straight from a standing start to full speed. Likewise,
the stepper motors cannot jump from full speed forwards to full speed backwards, for
example. The stepper ramp tasks – ramp_left_task and ramp_right_task – handle the
‘ramping’ of the stepper clock speeds, to keep the speed changes within the limits of the
RoboVac Software
25
stepper’s abilities. When a new speed is set, using the subroutines ramp_left_stepper
and ramp_right_stepper, these tasks ramp the stepper clock speeds by one millisecond
every two stepper clock pulses, giving an acceleration of approximately 1m/s2.
Stepper Turn Tasks
RoboVac turns by differing the speed of rotation of its two wheels. To turn left the left
wheel must run slower than the right, while to turn right the right wheel must run slower
than the left. The subroutine turn_to_angle sets a required difference in the stepper
counts according to the required angle and the direction of the turn (a negative angle
indicating a left turn), and also sets the appropriate motor speeds. The stepper turn task
(turn_task(LEFT) or turn_task(RIGHT)) then monitors the difference between the left
and right stepper counts, to calculate when the respective motors should ramp back to
their original speeds. This is calculated based on the number of counts each stepper will
make while the motor speed ramp is occurring.
5.2.2 Sonar Sensing Functions
As described earlier in this chapter, sonar sensors send a pulse of ultrasonic energy and
measure the time taken for the signal to return. Because the speed of sound is fairly
high (~344m/s), the measurement needs to be fast and accurate. RoboVac’s sonar
sensors use the high speed input/output port for measurement. The high speed output
(HSO) of the 80C196KC uses a CAM (Content-Addressable Memory) table. Each of
the CAM table’s eight entries allows one event to be stored, along with an associated
time tag. When the time tag of a CAM table entry matches the current free-runningcounter TIMER1 value, the associated event is executed. These events can toggle pins
or call a software interrupt. The high speed input (HSI) module stores the value of
TIMER1 when it receives a transition on its associated HSI pin.
A sonar reading starts by setting up the CAM entries to generate all parts of the
sonar reading.
The first and second CAM entries turn the specified sonar sensor
transmitter on and off (the pulse). The third CAM entry turns on the HSI to receive the
return signal after an appropriate blanking time, as described earlier in this chapter. The
fourth CAM entry updates the blackboard range array (rtime) with the new range data,
while the fifth sets up the CAM for the next sonar reading.
The sonar functions are implemented in the file ‘sonar.c’.
26
RoboVac Software
5.2.3 Bumper Sensing
The bumpers are used as last-resort obstacle detection. The bumper sensing task reads
IOPORT0 to check the status of the bumpers. The bumper status is then stored in the
blackboard variable bump_status for other behaviours to use.
5.3 Surface Coverage Functions
RoboVac, with the hardware control functions described in the previous section, is a
functional mobile robot. The remaining task is to implement the surface coverage
control approach developed in Chapter 3.
This section describes the higher-level
behaviours that give RoboVac its intelligence. As can be seen, a seemingly complex
task is achieved through the cooperation of relatively simple behaviours.
Each of these behaviour functions is run every update period – when the sonar
sensor function sets the updated flag.
Surface Coverage Software Functions
Activation Blackboard
Implemented in data area
Strip fill behaviour
implemented in main program
Cavity entry behaviour
cavity_entry()
Detect missed area behaviour
detect_missed_area()
Fill Missed area behaviour
fill_missed_area()
Wall following behaviour
follow_wall(distance, side)
Table 5.2 – Surface Coverage Software Functions. These
functions implement the controlling behaviours of the robot, enabling
it to fulfil its required objective.
5.3.1 Activation Blackboard
The activation blackboard, shown in Table 5.3, is the means of communication between
behaviours, motors and sensors. Sensor data is placed on the blackboard as it arrives.
Internal activation flags are used for communication between behaviours through the
blackboard. Behaviours monitor the sensor data and internal activation flags and if their
particular activation conditions are found, they execute their behavioural component.
This is done by modifying the information on the blackboard to change the motor
commands and disable or start other behaviours through the internal activation flags.
RoboVac Software
27
Activation Blackboard
Sonar Sensor Data
rtime[3]
Bumper Sensor Data
bump_save
Motor Data
modified using motor functions
Internal Activation Flags
failed_turn_flag
task_complete_flag
missed_area_flag
Strip Record Range Data
strip_record[n]
Next Strip Length Estimation
next_strip_length
Table 5.3 – Activation Blackboard. This data is used for
communication between the behaviours, sensors and motors.
5.3.2 Strip-Filling Behaviour
The strip-filling behaviour was implemented according to the steps described in Section
3.3.1. This is the dominant behaviour and so was implemented in the main program.
The strip-filling software implemented the following algorithm –
1. Move straight along wall, using wall following behaviour when appropriate to
correct any heading errors, and also recording periodic range readings on the side
away from the wall.
2. When an obstacle or wall comes within a certain distance (500mm) in front of the
robot, execute 180° turn away from the wall, if possible. If not, signal failed turn
and stop. This turn rotates about the inside wheel of the turn, ending up one robot
width away beside its turn starting position.
3. Move straight along wall, using wall following behaviour when appropriate to
correct any heading errors.
4. When an obstacle or wall comes within the minimum distance, turn 180° away from
wall, if possible. If not, signal failed turn and stop.
5. Repeat from 3.
This simple behaviour enabled RoboVac to totally cover a regularly shaped room (no
wall cavities) without obstacles. If obstacles were present, RoboVac would only strip
fill to one side of the obstacle.
To add more sophistication to the algorithm, more behaviours should be added.
More behaviours – cavity entry, detect missed area and fill missed area – are described
28
RoboVac Software
in Section 5.3.4 and 5.3.5. These behaviours were not implemented in this thesis, but
their design is discussed for later development.
5.3.3 Wall Following Behaviour
follow_wall(distance, side)
The wall-following behaviour constantly checked for small discrepancies in the range
readings on the already-filled side of the robot. When small discrepancies were found,
this behaviour made small adjustments to the robot’s heading.
When large
discrepancies were found, the range reading was updated with the new range data, as a
large discrepancy was most frequently caused by an irregularity in the wall surface, not
by orientation error.
The adjustments were calculated using a proportional-derivative control loop –
that is adjustments were made based on the calculated error in position, and also the
change in absolute distance from the wall. The derivative component serves to keep the
robot in a fair alignment with the wall, while the proportional component serves to
maintain the required distance from the wall.
5.3.4 Cavity Entry Behaviour
This behaviour was not implemented in this thesis, but its design is discussed here.
When a failed turn is signalled, the cavity entry behaviour checks the recorded
range reading data of the just completed strip (strip_record), to determine if there was a
cavity (Figure 3.2). This is done by checking for a long-short range transition in the
recorded range data sequence. If the check determines that there is more area to be
covered, this behaviour turns the robot 180° (turn_to_angle), away from the obstacle
that caused the failed turn, bringing it back onto a previous strip. This behaviour then
moves the robot along that strip, monitoring its side sensor to find the cavity. When it
finds the cavity, it executes a 90° turn into the cavity, and then a 90° turn in the opposite
direction, and signals for the strip filling to continue. If the check determined that there
was no cavity, this behaviour signals task complete.
29
RoboVac Software
5.3.5 Detect Missed Area Behaviour
This behaviour was not implemented in this thesis, but its design is discussed here.
Each time a turn is executed while strip-filling, this behaviour compares the
length of the just completed strip (strip_record[0]) with the previous estimation of how
long that strip should be (next_strip_length), as shown in Figure 3.3. If the comparison
is considerably different, then a missed area is signalled. When the check has been
completed, the range data from the just completed strip is used to make an estimation of
how long the next strip should be, by adding all the range readings which should fit into
the next strip (i.e. not too short).
This estimation is then used as the next
next_strip_length.
5.3.6 Fill Missed Area Behaviour
This behaviour, after a missed area had been signalled, begins to check for either of two
conditions – a close-far transition on the already-filled side of the robot (free-standing
obstacle), or a signal from the cavity entry behaviour indicating completion of the task
(indicating against the wall obstacle).
These two conditions would be handled
differently, as follows.
1. Free-standing obstacle – When this condition is detected, the missed area behaviour
executes a 90° turn, turning down beside the obstacle. The behaviour moves down
the side of the obstacle until it can no longer see the obstacle with its side sensor. It
then executes an opposite 90° turn, and signals for the strip filling to continue. This
behaviour is shown in Figure 3.3.
2. Against the wall obstacle – When the cavity entry behaviour signals complete while
a missed area is signalled, the missed area behaviour traces the wall back to the
missed area as in Figure 3.4, and signals for the strip filling to continue.
30
Chapter 6
Results and Discussion
6.1 Results
To obtain performance results of the RoboVac system, a number of tests were carried
out. This section describes each test and the performance of RoboVac in each of those
tests.
Wall Following Test
The wall following tests were run along a straight length of wall at a distance of 2m
from the wall.
Side sonar range readings were recorded as they were updated –
approximately six per second at intervals of approximately 3cm. The first test measured
the deviation of the robot from a straight course while moving straight forward. As can
be seen in Figure 6.1, the robot does not have a dependable straight forward movement.
Sonar Measurements while Running Straight
1.2
Distance from Wall (Metres)
1.18
1.16
1.14
1.12
1.1
1.08
0
0.5
1
1.5
2
Distance Travelled (Metres)
Figure 6.2 – Deviation while running straight. This chart shows the
distance of the robot from side wall while moving straight forward. As
can be seen, the robot does not move in a perfectly straight line.
2.5
31
A deviation of nearly 20cm over a forward movement of 2m is not acceptable and
would ruin any attempts at accurate robot localisation.
The second test measured the deviation while the wall following behaviour was
active, as shown in Figure 6.2. The wall following behaviour exhibited an average error
under 2% - around 4cm at 2 metres. Note, however, that a small change in direction to
the wall increases the measured distance significantly, so the actual average positional
error would be around 2cm at 2 metres.
Wall Following Error - 2m away from wall
6.0%
Percentage Error
4.0%
2.0%
0.0%
0
0.5
1
1.5
2
-2.0%
1st Run
2nd Run
-4.0%
Distance Travelled (metres)
Figure 6.2 – Wall Following Test Performance. The wall following
test was conducted along a straight stretch of wall.
Empty Room Test
The empty room test consisted of a rectangular area with no obstacles, 4m x 3m, with
‘walls’ high enough to be detected by the sonar sensors.
RoboVac performed in this test
similar to that shown in Figure 6.3. As can
be seen, small areas of the room were
consistently missed.
The following list
details these missed areas.
A. The missed areas along the start wall
and along the end wall were due to the
minimum range of the side sonar
sensors ~200mm
B. The missed areas when approaching
the side walls were due to the
Figure 6.3 Empty Room Test. The empty
room was a rectangular area with no obstacles.
RoboVac strip filled the room as shown in
green. The missed areas A, B and C are each
explained.
32
minimum range of the front sonar sensor ~200mm.
C. The missed area while turning was due to the dynamics of the robot. When the
robot turns, it rotates around the inside rear wheel, which causes it to end up beside
and behind its turn-start position, causing it to miss ~60cm of floor.
The result of the empty room test found that the robot covered approximately
66% of the total floor area of the 3m x 4m room. The major part of the missed area was
due to the minimum distance of the sonar sensors. The performance figure could be
increased to nearly 90% with the addition of a wall-hugging behaviour which skirts the
room boundary without using the side sonar sensors, maybe using IR sensors or the
bump sensors.
6.2 Discussion
This thesis has described a mobile robot system which fulfils its objective of vacuuming
a room autonomously. This system varies from previous work in this area in that it uses
a behavioural rather than a planning approach to control the surface coverage task.
The developed behavioural control method has two advantages over a planning
approach.
1. Dispenses with the need for a complex, maintenance-intensive world map, and
hence dispenses with the need for accurate sensors required to maintain those maps.
Instead the control method as implemented uses the world as its own model,
recording only immediate data necessary for the task at hand.
2. Dispenses with the need for large amounts of processor time and power while
maintaining the world maps, and planning movements. Instead, movements are
implemented by cooperating behaviours, each of which affects some particular part
of the overall surface coverage process.
These advantages, however, led to some problems –
•
Not using a world map made it more difficult to check that total surface coverage
had been completed. With a world map, coverage can be recorded on the map,
while with the implemented control method, the only record of what area the
robot has covered is its orientation in the world. This problem would be reduced
by implementing missed area and cavity entry behaviours, which check for missed
areas and enter them. The performance of RoboVac could be increased by adding
these extra behaviours.
•
The robot is sensitive to orientation errors.
33
The implemented behavioural
approach was efficient as long as it kept a tolerably accurate orientation. The wall
following behaviour was implemented to aid in orientation correction to reduce
this problem. Even so, unseen obstacles (such as a book or some other non sonarperceivable object) cause collisions and hence orientation errors. No behaviours
were implemented to deal with collisions. When the wall following behaviour is
not in use, such as when the wall is too far away, the orientation errors accumulate
quickly, throwing off any position estimation.
The performance of the robot’s surface coverage was severely affected by two factors –
the minimum range of the sonar sensors, and the dynamics of the robot. The long
wheelchair configuration of the robot is ill suited to the task requirements. A better
design would be a circular robot, with the driven wheels on a central axis, similar to the
Eureka robot shown in Figure 2.7. This design would eliminate the missed areas when
the robot turns.
34
Chapter 7
Conclusions and Future Directions
7.1 Conclusions
This thesis has described the development of a mobile robot – RoboVac – that
autonomously vacuums an area, avoiding obstacles and attempting to cover any missed
areas.
The major achievements of this thesis were –
•
A new main PCB and new sonar sensors were designed and constructed.
•
Software functions were written and implemented to control each of the RoboVac
hardware systems – stepper motors, sonar sensors, bump sensors and vacuum
control.
•
A surface coverage control method was developed. This method uses a
behavioural approach to the surface coverage task, rather than the planning
approach adopted in relevant published work.
•
The surface coverage control method was implemented successfully on RoboVac,
enabling it to achieve the objective task of this thesis in a controlled environment.
7.2 Future Directions
The RoboVac behaviours are by no means complete. The missed area behaviour
particularly needs work, to enable it to recognise and cope with more situations. Extra
behaviours may need to be added to cope with situations such as obstacle collisions.
The body of RoboVac is ill-suited to its intended task. A circular body, with driven
wheels on a central axis, would eliminate many of the robot’s dynamics problems.
Who knows? Within a few years, RoboVac may be vacuuming the floors of houses
around the world!
35
References
[Wyeth & Barron, 1994] G.F. Wyeth and I. Barron, An Autonomous Blimp, Field and Service
Robotics, Ed. Alex Zelinksy, Springer Verlag, 1998, pp. 464-470
[Borenstein, 1996] J. Borenstein, “Measurement and Correction of Systematic Odometry
Errors in Mobile Robots”, IEEE Transactions on Robotics and Automation, vol. 12,
no 6., Dec. 1996, p 869 – 876.
[Cao et al, 1988] Cao, Zuo Liang, Yuyu Huang, Ernest L. Hall., “Region Filling Operations
with Random Obstacle Avoidance for Mobile Robots”, Journal of Robotic Systems,
Vol. 5, No. 2, April 1988, p87-102.
[Cheng, 1998] T. Cheng, An Autonomous Vacuum Cleaner, UQ Department of Electrical and
Computer Engineering Undergraduate Thesis, 1998.
[Connell, 1993] J. Connell, Minimalist Mobile Robotics, Academic Press, Inc., San Diego,
CA, USA, 1993.
[Dowideit, 1995] S. Dowideit, An Autonomous Vacuum Cleaner, UQ Department of Electrical
and Computer Engineering Undergraduate Thesis, 1995.
[Eureka, 1999] The Eureka Company, The Eureka Robot Vac – Going Where No Vacuum
Has Gone Before, http://www.eureka.com/whatsnew/robotvac.htm, 1999.
[Hofner et al, 1994] C. Hofner, G. Schmidt. “Path planning and guidance techniques for an
autonomous mobile cleaning robot”, Proceedings of the IEEE/RSJ International
Conference on Intelligent Robots and Systems, Munich, Germany, 12-16 Sept 1994,
p610-617.
[Hofner et al, 1998] C. Hofner, G. Schmidt. “An advanced path planning and navigation
approach for autonomous cleaning robot operations”, Proceedings of the IEEE/RSJ
International Conference on Intelligent Robots and Systems, Victoria, B.C., Canada,
October 1998, p1230-1235.
[Lang et al, 1998] S. Lang and Bing-Yung Chee. “Coordination of Behaviours for Mobile
Robot Floor Cleaning”, Proceedings of the IEEE/RSJ International Conference on
Intelligent Robots and Systems, Victoria, B.C., Canada, October 1998, p1236-1241.
[Montiel, 1996]
[Suarez et al, 1995] A. Suarez, E. Gonzalez, J.C. Cabo, Y. Rollot, B. Manuel, C. Moreno, F.
Artigue. “An Autonomous Vehicle for Surface Filling”, Proceedings of the 1995
Intelligent Vehicles Symposium, Detroit, MI, USA, 25-26 Sept., 1995. P364-369.
36
Appendix A
Schematic Diagrams
Appendix A – Schematic Diagrams
37
38
39
40
41
42
Appendix B
Code Listings
43
RoboVac.h
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
/* Robovac.h
*
* Defines for the RoboVac.
*
*/
#define LEFT_STEPPER_SPEED_BIT 0x04
#define LEFT_STEPPER_DIRN_BIT 0x10
#define LEFT_STEPPER_HALF_BIT 0x40
#define RIGHT_STEPPER_SPEED_BIT 0x08
#define RIGHT_STEPPER_DIRN_BIT 0x20
#define RIGHT_STEPPER_HALF_BIT 0x80
#define STEPPER_PORT
IOPORT1
#define DEBUG 0
#define SONAR_DEBUG 1
#define OFF 35
#define ON 01
#define RAMP_CHANGE 1
#define RAMP_TIME 3
#define FORWARD 0x00
#define REVERSE 0x01
#define TURN_180 169
#define FULL_RAMP_TIME 29
#define NO_OF_SONARS 3
#define LEFT 2
#define RIGHT 1
#define FRONT 0
#define LEFT_CORR 20
#define WALL_TURN_DEVIATION 3
/* Sonar function prototypes */
void start_sonar(void);
void activate_HSI(void);
void setup_ping(void);
void return_data(void);
/* Motor function prototpes */
void init_motors(void);
void left_stepper_task(void);
void right_stepper_task(void);
void ramp_task(char which);
void ramp_left_stepper(unsigned short new_speed, unsigned char new_dirn);
void ramp_right_stepper(unsigned short new_speed, unsigned char new_dirn);
void left_turn(int angle);
void right_turn(int angle);
void ramp_both_steppers (unsigned short new_speed, unsigned char new_dirn);
/* Sonar Variables */
extern unsigned int rtime[3];
extern unsigned int rstatus[3];
extern unsigned char updated_flag;
/* Motor Variables */
extern long left_stepper_count; /* Very rough distance measurement */
extern long right_stepper_count;
extern unsigned char Turn_Flag;
extern unsigned long System_Time;
extern unsigned char set_speed;
61.
extern unsigned char turn_speed
RoboVac.h
44
45
RoboVac.c
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
/*
RoboVac.C
* Main Program file for RoboVac
*
*/
#include <8096.h>
#include <stdio.h>
#include <intr96.h>
#include "robovac.h"
void interrupt software_interrupt(void);
void interrupt HSI_int(void);
void init_chip(void);
void init_vectors(void);
void follow_wall (char which_side, unsigned int distance);
void turn_to_angle(long angle);
int abs (int number);
unsigned long System_Time;
unsigned int offset;
unsigned char bump_save;
unsigned char bump_flag;
int error[2];
void main(void)
{
init_chip();
init_vectors();
init_motors();
IOPORT1 = 0x00;
IOPORT2 ^= 0x80;
IOC0 = 0x00;
INT_MASK = 0x20;
System_Time = 0;
start_sonar();
ei();
/* Wait for bump, then start */
while (System_Time < 10);
while ((bump_save & 0x0F) == 0x0F);
while ((bump_save & 0x0F) != 0x0F);
offset = rtime[RIGHT];
/*
* Strip Fill
*/
while ((bump_save & 0x0F) == 0x0F) {
/*
* First leg of strip fill
*/
while (rtime[0] > 4000) {
if (updated_flag == 0) {
follow_wall(RIGHT, offset);
updated_flag = 1;
}
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
if (abs(error[1]) > 500) {
if (abs(error[0]) > 500) {
offset = rtime[RIGHT];
if (offset < 2300) offset = 2300;
}
}
}
/*
* turn away from wall
*/
turn_to_angle(-180);
while(Turn_Flag == LEFT);
offset += 1380;
/*
* Secondary Leg of Strip Fill
*/
while (rtime[0] > 4000) {
if (updated_flag == 0) {
follow_wall(LEFT, offset);
updated_flag = 1;
}
if (abs(error[1]) > 500) {
if (abs(error[0]) > 500) {
offset = rtime[LEFT];
if (offset < 2300) offset = 2300;
}
}
}
/*
* Turn away from wall
*/
turn_to_angle(180);
while(Turn_Flag == RIGHT);
offset += 1380;
}
ramp_both_steppers(OFF, FORWARD);
/*
* Finish
*/
}
/*
* Follow wall * This routine sets motor commands to follow the wall, based
* on positional error and change in absolute position.
*/
void follow_wall (char which_side, unsigned int distance) {
char left=set_speed;
char right = set_speed;
int prop, div, turn;
error[0] = error[1];
error[1] = rtime[which_side] - distance;
/* Proportional control
* If prop -'ve need to turn out
*/
prop = error[1] / 60;
if (prop < -WALL_TURN_DEVIATION) prop = -WALL_TURN_DEVIATION;
46
RoboVac.c
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
if (prop > WALL_TURN_DEVIATION) prop = WALL_TURN_DEVIATION;
if (prop == 0) {
if (error[1] > 0) prop = 1;
if (error[1] < 0) prop = -1;
}
/* Derivative control
* If deriv -'ve need to
*/
div = error[1] - error[0];
div = div / 35;
if (div < -WALL_TURN_DEVIATION) div = -WALL_TURN_DEVIATION;
if (div > WALL_TURN_DEVIATION) div = WALL_TURN_DEVIATION;
/* Add together to give PD control */
turn = prop + div;
if (which_side == LEFT) {
if (turn < 0) {
right = right - turn;
if (DEBUG == 1) {
printf("turn right - ");
}
}
else if (turn > 0) {
left = left + turn;
if (DEBUG == 1) {
printf("turn left - ");
}
}
}
else if (which_side == RIGHT) {
if (turn < 0) {
left = left - turn;
}
else if (turn > 0) {
right = right + turn;
}
}
ramp_left_stepper(left, FORWARD);
ramp_right_stepper(right, FORWARD);
if (DEBUG == 1) {
printf("Dist = %5u, ", rtime[which_side]);
printf("prop = %d, div = %d, turn = %d\n", prop, div, turn);
}
}
/*
* software_interrupt
*
This interrupt service routine handles software interrupts.
*
It first saves the I/O status registers, then uses those to
*
determine what caused the interrupt.
*
These will either be the sonar, or the system timer.
*/
void interrupt software_interrupt() {
unsigned char save;
unsigned char save2;
save = IOS1;
save2 = IOS2;
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.
209.
210.
211.
212.
213.
214.
215.
216.
217.
218.
219.
220.
221.
222.
223.
224.
225.
226.
227.
228.
229.
230.
231.
232.
233.
234.
235.
236.
237.
238.
239.
/*
* This turns on the HSI to receive the ping
*/
if ((save & 0x02) == 0x02) {
activate_HSI();
}
/*
* This sets up all the CAM entries to generate and receive
* the next ping
*/
if ((save & 0x04) == 0x04) {
setup_ping();
}
/*
* HSO INT for returning the data
*/
if ((save & 0x08) == 0x08) {
return_data();
}
/*
* TIMER2 reset interrupt for system time and motors
*/
if ((save2 & 0x40) == 0x40) {
System_Time++;
left_stepper_task();
right_stepper_task();
bump_save = IOPORT0;
if (bump_save != 255) bump_flag = 1;
}
ei();
}
/*
*
Init_Chip
*
This routine initialises the I/O control registers and
*
starts up the system timer interrupt.
*/
void init_chip(void) {
IOC1 = 0x22;
IOC2 = 0x48;
WSR = 1;
IOC3 = 0x01;
WSR = 0;
/*
* Enable System Timer cam entry.
* E -> reset timer 2
* D -> Lock command into cam
*
Use Timer 2 as reference
*
Call Interrupt
*/
HSO_COMMAND = 0xDE;
HSO_TIME = 1250;
STEPPER_PORT |= RIGHT_STEPPER_HALF_BIT;
RoboVac.c
240.
241.
242.
243.
244.
245.
246.
247.
248.
249.
250.
251.
252.
253.
254.
255.
256.
257.
258.
259.
260.
261.
262.
STEPPER_PORT |= LEFT_STEPPER_HALF_BIT;
}
/*
* init_vectors
*
This routine sets up the interrupt vectors
*/
void init_vectors(void) {
di();
set_vector(INTR_SOFTWARE_TIMER, software_interrupt);
ei();
}
int abs (int number)
{
if (number >= 0)
return number;
else
return -number;
}
47
48
Motors.c
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
/*
* Motors.c
*
Basic motor control functions for RoboVac
*/
#include <8096.h>
#include <stdio.h>
#include <intr96.h>
void set_left_stepper(unsigned short period, unsigned char dirn);
void set_right_stepper(unsigned short period, unsigned char dirn);
void turn_task(char direction);
void ramp_task(char which);
unsigned char Turn_Flag;
unsigned char Ramp_Flag[3];
unsigned long left_stepper_time; /* Time to toggle left stepper */
unsigned short left_stepper_period; /* Left stepper speed */
unsigned char left_stepper_dirn; /* 0 = forward */
unsigned long right_stepper_time; /* Time to toggle right stepper */
unsigned short right_stepper_period; /* Right stepper speed */
unsigned char right_stepper_dirn; /* 0 = forward */
long left_stepper_count; /* Very rough distance measurement */
long right_stepper_count;
int left_speed_before_turn;
int right_speed_before_turn;
long turn_end_count;
unsigned char set_speed;
unsigned char turn_speed;
long turn_end_diff;
unsigned long next_change[3];
unsigned short new_stepper_speed[3];
unsigned char new_stepper_dirn[3];
/* Speed to ramp to */
/*
* Set left stepper
*
This routine sets a given speed and dirn to the left stepper.
*/
void set_left_stepper(unsigned short period, unsigned char dirn) {
left_stepper_period = period;
if (left_stepper_dirn != dirn) {
if (dirn == FORWARD) {
STEPPER_PORT &= ~LEFT_STEPPER_DIRN_BIT;
}
else {
STEPPER_PORT |= LEFT_STEPPER_DIRN_BIT;
}
}
left_stepper_dirn = dirn;
}
/*
* Set right stepper
*
This routine sets a given speed and dirn to the right stepper.
*/
void set_right_stepper(unsigned short period, unsigned char dirn) {
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
right_stepper_period = period;
if (right_stepper_dirn != dirn) {
if (dirn == FORWARD) {
STEPPER_PORT &= ~RIGHT_STEPPER_DIRN_BIT;
}
else {
STEPPER_PORT |= RIGHT_STEPPER_DIRN_BIT;
}
}
right_stepper_dirn = dirn;
}
/*
* Left Stepper Toggle
*
This routine toggles the speed bit on the left stepper - causing
*
the next step.
*/
void left_stepper_task(void) {
if (left_stepper_time <= System_Time) {
/* Step the left stepper */
if (left_stepper_period != OFF) {
STEPPER_PORT ^= LEFT_STEPPER_SPEED_BIT;
left_stepper_count++;
left_stepper_time = System_Time + left_stepper_period;
}
else {
left_stepper_time = System_Time + 1;
}
if (Turn_Flag == RIGHT) {
turn_task(RIGHT);
}
}
if ((System_Time >= next_change[LEFT]) && (Ramp_Flag[LEFT] == ON)) {
ramp_task(LEFT);
}
}
/*
* Right Stepper Toggle
*
This routine toggles the speed bit on the right stepper - causing
*
the next step.
*/
void right_stepper_task(void) {
if (right_stepper_time <= System_Time) {
/* Step the right stepper */
if (right_stepper_period != OFF) {
STEPPER_PORT ^= RIGHT_STEPPER_SPEED_BIT;
right_stepper_count++;
right_stepper_time = System_Time + right_stepper_period;
}
else {
right_stepper_time = System_Time + 1;
}
if (Turn_Flag == LEFT) {
turn_task(LEFT);
}
}
if ((System_Time >= next_change[RIGHT]) && (Ramp_Flag[RIGHT] == ON)) {
ramp_task(RIGHT);
49
Motors.c
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
130.
131.
132.
133.
134.
135.
136.
137.
138.
139.
140.
141.
142.
143.
144.
145.
146.
147.
148.
149.
150.
151.
152.
153.
154.
155.
156.
157.
158.
159.
160.
161.
162.
163.
164.
165.
166.
167.
168.
169.
170.
171.
172.
173.
174.
175.
176.
177.
178.
179.
}
}
void init_motors(void)
{
set_left_stepper(OFF, FORWARD);
set_right_stepper(OFF,FORWARD);
left_stepper_time = System_Time + 1;
right_stepper_time = System_Time + 1;
left_stepper_count = 1000;
right_stepper_count = 1000;
set_speed = 15;
turn_speed = 22;
}
/*
* ramp_left_stepper
*
This routine is used by the user to initialise a ramp on the
*
left stepper.
*/
void ramp_left_stepper(unsigned short new_speed, unsigned char new_dirn) {
new_stepper_speed[LEFT] = new_speed;
new_stepper_dirn[LEFT] = new_dirn;
Ramp_Flag[LEFT] = ON;
if (next_change[LEFT] < System_Time)
next_change[LEFT] = System_Time + (left_stepper_period * RAMP_TIME);
}
/*
* ramp_right_stepper
*
This routine is used by the user to initialise a ramp on the
*
right stepper.
*/
void ramp_right_stepper(unsigned short new_speed, unsigned char new_dirn) {
new_stepper_speed[RIGHT] = new_speed;
new_stepper_dirn[RIGHT] = new_dirn;
Ramp_Flag[RIGHT] = ON;
if (next_change[RIGHT] < System_Time)
next_change[RIGHT] = System_Time + (right_stepper_period * RAMP_TIME);
}
/*
* ramp_both_steppers
*
This routine is used by the user to initialise a ramp on both
*
steppers.
*/
void ramp_both_steppers (unsigned short new_speed, unsigned char new_dirn) {
ramp_left_stepper(new_speed, new_dirn);
ramp_right_stepper(new_speed, new_dirn);
}
/*
* ramp_task
*
*
This routine handles ramping up, down and backwards of the
specified stepper.
180.
181.
182.
183.
184.
185.
186.
187.
188.
189.
190.
191.
192.
193.
194.
195.
196.
197.
198.
199.
200.
201.
202.
203.
204.
205.
206.
207.
208.
209.
210.
211.
212.
213.
214.
215.
216.
217.
218.
219.
220.
221.
222.
223.
224.
225.
226.
227.
228.
229.
230.
231.
232.
233.
234.
235.
236.
237.
238.
239.
*
This is done because the inertia of the robot must be taken into
*
account when changing speeds.
*/
void ramp_task(char which)
{
unsigned short new_speed, new_dirn;
unsigned char intended_speed, intended_dirn;
unsigned short ramp_end;
switch(which) {
case LEFT:
new_speed = left_stepper_period;
new_dirn = left_stepper_dirn;
break;
case RIGHT:
new_speed = right_stepper_period;
new_dirn = right_stepper_dirn;
break;
}
intended_dirn = new_stepper_dirn[which];
intended_speed = new_stepper_speed[which];
/*
* check if need to change dirn
* if we do, must run to stop first, then ramp back up.
*/
if (new_dirn == intended_dirn) {
ramp_end = intended_speed;
}
else {
ramp_end = OFF;
}
/*
* Ramp to new speed
*/
if (ramp_end > new_speed) {
new_speed += RAMP_CHANGE;
if (ramp_end <= new_speed) {
/* Check if finished ramp */
new_speed = ramp_end;
}
}
else if (ramp_end < new_speed) {
new_speed -= RAMP_CHANGE;
if (ramp_end >= new_speed) {
/* Check if finished ramp */
new_speed = ramp_end;
}
}
/*
* change dirn for ramp back up
*/
if (new_speed == OFF) {
new_dirn = intended_dirn;
}
/*
* If we are there, disable the ramp
*/
if (intended_speed == new_speed && intended_dirn == new_dirn) {
/* we are there */
Ramp_Flag[which] = OFF;
50
Motors.c
240.
241.
242.
243.
244.
245.
246.
247.
248.
249.
250.
251.
252.
253.
254.
255.
256.
257.
258.
259.
260.
261.
262.
263.
264.
265.
266.
267.
268.
269.
270.
271.
272.
273.
274.
275.
276.
277.
278.
279.
280.
281.
282.
283.
284.
285.
286.
287.
288.
289.
290.
291.
292.
293.
294.
295.
296.
297.
298.
}
/*
* make the change and set up the next one
*/
switch (which) {
case LEFT:
set_left_stepper (new_speed, new_dirn);
next_change[which] = System_Time + (left_stepper_period * RAMP_TIME);
break;
case RIGHT:
set_right_stepper (new_speed, new_dirn);
next_change[which] = System_Time + (right_stepper_period * RAMP_TIME);
break;
}
}
/*
* turn task
*
Periodic stepper pulser
*/
void turn_task(char direction)
{
long inside_stepper_count, outside_stepper_count;
long inside_stepper_period, outside_stepper_period;
long temp5;
long working, working_2;
char i, ix;
switch (direction) {
case RIGHT:
inside_stepper_count = right_stepper_count;
outside_stepper_count = left_stepper_count;
inside_stepper_period = right_stepper_period;
outside_stepper_period = left_stepper_period;
break;
case LEFT:
inside_stepper_count = left_stepper_count;
outside_stepper_count = right_stepper_count;
inside_stepper_period = left_stepper_period;
outside_stepper_period = right_stepper_period;
break;
}
/* need to work out, with ramping if we need to finish the turn */
if (left_speed_before_turn == OFF) {
if (right_speed_before_turn == OFF) {
/* Static Turn */
working = 2 * (35 - outside_stepper_period) - 1;
if (outside_stepper_count >= turn_end_count - working) {
case RIGHT:
ramp_left_stepper(OFF, FORWARD);
Turn_Flag = RIGHT;
break;
case LEFT:
ramp_right_stepper(OFF,
FORWARD);
Turn_Flag = LEFT;
break;
}
299.
300.
301.
302.
303.
304.
305.
306.
307.
308.
309.
310.
311.
312.
313.
314.
315.
316.
317.
318.
319.
320.
321.
322.
323.
324.
325.
326.
327.
328.
329.
330.
331.
332.
333.
334.
335.
336.
337.
338.
339.
340.
341.
342.
343.
344.
345.
346.
347.
348.
349.
350.
351.
352.
353.
354.
355.
356.
357.
358.
}
}
}
else {
/* Turn while moving */
ix = inside_stepper_period - outside_stepper_period;
working_2 = 0;
for (i = ix; i > 0; i --) {
working_2 += i;
}
working_2 *= RAMP_TIME;
working_2 /= outside_stepper_period; /* The number of steps the */
/* outside stepper will take*/
/* while the inside ramps */
working_2 ++;
temp5 = inside_stepper_count + turn_end_diff - working_2;
if (outside_stepper_count >= temp5) {
case RIGHT:
ramp_right_stepper(left_stepper_period, FORWARD);
Turn_Flag = OFF;
break;
case LEFT:
ramp_left_stepper(right_stepper_period, FORWARD);
Turn_Flag = OFF;
break;
}
}
}
}
/*
* Turn to angle
*
Turns to an angle relative to the current heading of RoboVac
*/
void turn_to_angle(long angle) {
/*
* 180deg = 169 differential counts
* need to take ramping counts into account
* time to full ramp = 29 counts
* Right turn is positive, left is negative.
*/
long diff_required = angle * 169/180;
long current_diff = left_stepper_count - right_stepper_count;
long difference = diff_required + current_diff;
left_stepper_count --;
left_speed_before_turn = left_stepper_period;
right_speed_before_turn = right_stepper_period;
if (angle > 0) {
/* need right turn */
ramp_left_stepper(turn_speed, FORWARD);
ramp_right_stepper(OFF, FORWARD);
turn_end_count = left_stepper_count + difference;
turn_end_diff = difference;
Turn_Flag = RIGHT;
if (DEBUG == 1) {
Motors.c
359.
360.
361.
362.
363.
364.
365.
366.
367.
368.
369.
370.
371.
372.
373.
374.
printf("right turn ->");
}
}
else if (angle < 0) {
/* need left turn */
ramp_right_stepper(turn_speed, FORWARD);
ramp_left_stepper(OFF, FORWARD);
turn_end_count = right_stepper_count - difference;
turn_end_diff = -difference;
Turn_Flag = LEFT;
if (DEBUG == 1) {
printf("left turn ->");
}
}
}
51
52
Sonars.c
/*
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
* Sonar.c
*
Basic functions to control the sonar sensing
*/
#include <8096.h>
#include <stdio.h>
#include <intr96.h>
unsigned int rtime[3];
unsigned int rstatus[3];
unsigned int choose_ping[2] = {20, 150};
unsigned int choose_blank[2] = {1000, 2500};
unsigned int ping_length[3] = {1, 1, 1};
unsigned int blank_time[3] = {1, 1, 1};
unsigned char valid_count[3] = {0,0,0};
unsigned char short_ping[3] = {0,0,0};
unsigned char current_sonar;
unsigned char internal_flag;
unsigned char updated_flag;
void activate_HSI (void)
{
int i;
int rubbish;
for (i = 1; i <= 8; i++) {
rubbish = HSI_STATUS;
rubbish = HSI_TIME;
}
INT_MASK = 0x24;
IOC0 = 0x01;
}
void setup_ping (void)
{
/* Select the next sonar */
current_sonar++;
if (current_sonar > (NO_OF_SONARS - 1))
current_sonar = 0;
IOPORT1 &= ~0x03;
IOPORT1 += current_sonar;
/* Dynamic ping length settings */
if (short_ping[current_sonar] == 1) {
ping_length[current_sonar] = 1;
blank_time[current_sonar] = 1;
short_ping[current_sonar] = 0;
}
else {
valid_count[current_sonar]++;
if (valid_count[current_sonar] > 3) {
ping_length[current_sonar] = 0;
blank_time[current_sonar] = 0;
valid_count[current_sonar] = 0;
}
}
internal_flag = 1;
IOC0 = 0x00;
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
91.
92.
93.
94.
95.
96.
97.
98.
99.
100.
101.
102.
103.
104.
105.
106.
107.
108.
109.
110.
111.
112.
113.
114.
115.
116.
117.
118.
119.
INT_MASK = 0x20;
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x00;
/* HSO to turn on pulse */
HSO_TIME = 10000;
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x20;
/* HSO to turn off pulse */
HSO_TIME = 10000 + choose_ping[ping_length[current_sonar]];
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x19;
/* HSO for turning on HSI */
HSO_TIME = 10000 + choose_blank[blank_time[current_sonar]];
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x1B; /* HSO for returning the data */
HSO_TIME = 40000;
INT_MASK = 0x20;
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x1A; /* HSO for setting up the next ping */
HSO_TIME = 60000;
}
/*
* return data
*
HSO CAM entry to return the sonar data
*/
void return_data(void)
{
IOC0 = 0x00;
INT_MASK = 0x24;
if (internal_flag != 0 && ((INT_PENDING | 0x04) != 0x04)) {
if (ping_length[current_sonar] == 0)
else
rtime[current_sonar] = 50000;
}
if (current_sonar == (NO_OF_SONARS - 1)) {
updated_flag = 0;
}
rstatus[current_sonar] = ping_length[current_sonar];
}
/*
*
Start_Sonar
*
This routine starts up the sonar pings.
*/
void start_sonar(void)
{
while ((IOS0 & 0x40) == 0x40);
HSO_COMMAND = 0x1A; /* HSO int for starting the ping */
HSO_TIME = 60000;
INT_MASK = 0x20;
set_vector(INTR_HSI_DATA,HSI_int);
current_sonar = 0x00;
updated_flag = 1;
}
/*
* HSI_int
*
This interrupt service routine receives the ping and places
*
it into the rtime buffer.
*/
void interrupt HSI_int() {
Sonars.c
120.
121.
122.
123.
124.
125.
126.
127.
128.
129.
unsigned char same;
same = HSI_STATUS;
rtime[current_sonar] = rtime[current_sonar];
rtime[current_sonar] = HSI_TIME - 10000;
internal_flag = 0;
INT_MASK = 0x20;
ei();
}
53
Sonars.c
54

An Autonomous Vacuum Cleaner. Senior thesis by Stuart R. Harding

Transcription

Similar documents

stride - Cloudfront.net

12-Volt Unipolar Stepper Motor (#27964)

View Spec Sheet

CNC Router Project - Home Metal Shop Club

High Noon

Collamat® C 6600

Motor/Stepper Driver Board - 5108 Board Overview

ISO 9001 : 2008

The Discovery of New Elements

Emergence of spontaneous proto