realtime control of a mobile robot using
DESCRIPTION
real time application using PIC controllerTRANSCRIPT
-
Realtime Control of a Mobile Robot Using
Matlab
by
Kai Wu, B.Eng
Thesis
Presented to the department of Electrical Engineering and Computer Science
The University of Applied Science Hamburg
for the Degree of
Master of Engineering
The University of Applied Science Hamburg
Oct 2004
-
Realtime Control of a Mobile Robot Using
Matlab
Kai Wu, MEng.
The University of Applied Science Hamburg, 2004
Supervisor: Prof. Dr. Thomas Holzhueter
Second Supervisor: Prof. Dr. Ulf Claussen
In this thesis a real time control application for a mobile robot will be im-
plemented based on a MATLAB Simulink block diagram. The mobile robot
is called AmigoBot which has two driven wheels and 8 sonar sensors. The
block is built for running on the xPC Target which is a real time environment
provided by MATLAB. The control application will control the robot moving
along a wall at a certain distance. The track of the wall can be a straight one
or a curved one. The required distance can be a constant or a mathematic
function (e.g. a step function). This work is actually divided into two parts.
The first part is the construction of a communication block diagram which is
used for establishing a connection between the xPC Target and the AmigoBot.
The second part is creating a control system for the robot, which is based on
state-space control.
ii
-
ACKNOWLEDGMENTS
Acknowledgments
I would like to thank Prof.Dr.Thomas Holzhueter, my supervisor who give
me this chance, for his many suggestions and constant support during this
research. I am also thankful to Mr.Zeyn-Kranz and Mr.Suchan for their ad-
vice and help for preparing the robot and other things for me.Thanks for
Prof.Dr.Ulf Claussen whod like to be my second supervisor. Thanks for
Mr.Stender who shares his knowledge about the robot with me. Thanks for
my parents who are always supporting me.
Kai Wu
The University of Applied Science Hamburg
Oct 2004
iii
-
CONTENTS
Contents
Abstract ii
Acknowledgments iii
List of Tables vii
List of Figures viii
Chapter 1 Introduction 1
1.1 Problem Overview . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Organization of the Thesis . . . . . . . . . . . . . . . . . . . . 5
Chapter 2 Design of the AmigoBots Communication Interface 7
2.1 The xPC Target . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.1.1 Host and Target PC . . . . . . . . . . . . . . . . . . . 8
2.1.2 xPC Target Software Set . . . . . . . . . . . . . . . . . 8
2.1.3 Downloading A Model to the Target PC . . . . . . . . 10
2.2 The AmigoBot . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.2.1 Communication Packet Protocol . . . . . . . . . . . . . 14
2.2.2 Packet Data Types . . . . . . . . . . . . . . . . . . . . 15
2.2.3 Packet Checksum . . . . . . . . . . . . . . . . . . . . . 16
2.2.4 Packet Errors . . . . . . . . . . . . . . . . . . . . . . . 16
2.2.5 Server Information Packets . . . . . . . . . . . . . . . . 16
2.2.6 Client Commands . . . . . . . . . . . . . . . . . . . . . 18
2.2.7 Client Command Argument Types . . . . . . . . . . . 20
2.3 Connection between the xPC Target and AmigoBot . . . . . . 20
2.4 The AmigoBot Communication Block . . . . . . . . . . . . . . 21
iv
-
CONTENTS
Chapter 3 Implementation of the AmigoBot Communication Sys-
tem 22
3.1 Analyzing the Synchronization and Initialization Process . . 22
3.1.1 Analyzing the Synchronization Process . . . . . . . . . 22
3.1.2 Analyzing the Initialization Process . . . . . . . . . . 26
3.1.2.1 Opening the ServersOPEN . . . . . . . . . . 26
3.1.2.2 Set the Sonar Firing SequencePOLLING . . 26
3.1.2.3 Enable the Motor . . . . . . . . . . . . . . . . 27
3.1.3 Building the Synchronization and Initialization Model 27
3.1.3.1 Subsystem Synchronization and Initialization 32
3.2 Receive and Decode the SIPs . . . . . . . . . . . . . . . . . . 37
3.2.1 Subsystem Receive the SIPs . . . . . . . . . . . . . . . 37
3.2.2 Subsystem Decode the SIPs . . . . . . . . . . . . . . . 40
3.3 Setting the Wheel Speed . . . . . . . . . . . . . . . . . . . . . 43
3.4 Testing the Communication Block and Analyzing the Server
Information . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
Chapter 4 A Real-Time Control Application for the AmigoBot 58
4.1 Mobile Robot Control . . . . . . . . . . . . . . . . . . . . . . 58
4.1.1 State Space System Basics . . . . . . . . . . . . . . . 60
4.1.2 State Space System of the AmigoBot . . . . . . . . . . 61
4.2 Design of the Control Loop of the AmigoBot . . . . . . . . . . 63
4.2.1 Building the Simulation Model . . . . . . . . . . . . . 63
4.2.2 Building the Real-Time Control Model . . . . . . . . . 70
4.3 Experimental Verification of the Robot Controller . . . . . . . 73
Chapter 5 Conclusions and Suggestions for Future Work 80
5.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
5.2 Suggestions for the Future Work . . . . . . . . . . . . . . . . . 82
Appendix A AmigOS Command Set 83
Appendix B Initial File 87
Appendix C Control Parameter Setup File 89
Bibliography 92
v
-
CONTENTS
Index 93
Vita 95
vi
-
LIST OF TABLES
List of Tables
2.1 Main Elements of AmigOS Communication Packet Protocol . 15
2.2 AmigOS Communication Packet Data Types . . . . . . . . . . 15
2.3 Standard AmigOS Server Information Packet (SIP) . . . . . . 18
2.4 AmigOS Client Command Packet . . . . . . . . . . . . . . . . 19
3.1 Synchronization Test Case 1 . . . . . . . . . . . . . . . . . . . 24
3.2 Synchronization Test Case 2 . . . . . . . . . . . . . . . . . . . 24
3.3 Synchronization Test Case 3 . . . . . . . . . . . . . . . . . . . 24
3.4 Synchronization Test Case 4 . . . . . . . . . . . . . . . . . . . 25
3.5 AmigOS Wheel Speed Analysis (mm/sec) . . . . . . . . . . . . 52
A.1 AmigOS Command Set . . . . . . . . . . . . . . . . . . . . . . 86
vii
-
LIST OF FIGURES
List of Figures
2.1 Real-Time Workshop Code Generation Process . . . . . . . . 9
2.2 Hardware Connection between Target PC and Host PC . . . 11
2.3 Configuration of Target Boot Disk . . . . . . . . . . . . . . . 12
2.4 xPC Target Boots, the Kernel and Display Information on the
Target PC . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.5 Loading a MATLAB Simulink Model to xPC Target . . . . . . 13
2.6 AmigoBots Physical Characteristics. . . . . . . . . . . . . . . 13
2.7 AmigoBots Client-server Architecture . . . . . . . . . . . . . 14
2.8 Hardware Connection between Target PC and AmigoBot . . . 20
2.9 The AmigoBot Communication Block . . . . . . . . . . . . . 21
3.1 Flow Chart of How to Start The AmigoBot . . . . . . . . . . 28
3.2 AmigoBot Connector . . . . . . . . . . . . . . . . . . . . . . 29
3.3 Second Level of the AmigoBot Connector. . . . . . . . . . . . 31
3.4 Synchronization & Initialization. . . . . . . . . . . . . . . . . . 33
3.5 The RS232 Received Buffer on the Target PC . . . . . . . . . 34
3.6 Sending the SYNC0 Packet . . . . . . . . . . . . . . . . . . . 35
3.7 Receiving the Echo of SYNC0 . . . . . . . . . . . . . . . . . 35
3.8 Receiving the Echo of SYNC1 . . . . . . . . . . . . . . . . . 36
3.9 Subsystem Receiving SIP . . . . . . . . . . . . . . . . . . . . . 37
3.10 Sub Subsystem Receiving SIP . . . . . . . . . . . . . . . . . . 38
3.11 Decoding the SIP to User Wanted Data . . . . . . . . . . . . . 41
3.12 Sub Subsystem to Detect the Speed of the Left Wheel . . . . . 41
3.13 Sub Subsystem Get the Sonar Values: New Sonar Readings
SonarRangeA and SonarRangeB . . . . . . . . . . . . . . . . . 43
3.14 Generating a Setting Wheel Speed Command Packet . . . . . 44
3.15 Five Examples of the Speed Commands . . . . . . . . . . . . . 45
viii
-
LIST OF FIGURES
3.16 Timing Sequence Diagram of the Communication Block. . . . 47
3.17 Time Analysis of the Communication Block . . . . . . . . . . 48
3.18 Received X Position Information. . . . . . . . . . . . . . . . . 48
3.19 Setting Different Speeds of Left and Right Wheels . . . . . . . 50
3.20 Analyzing the Received Wheel Speed. . . . . . . . . . . . . . . 51
3.21 Received Left Wheel Speed with 7 Steps. . . . . . . . . . . . . 51
3.22 Received X Position Information (Wheel Speed with 6 Steps). 52
3.23 Find The Maximum Speed. . . . . . . . . . . . . . . . . . . . 53
3.24 Set the Left and Right Wheel Speed to Positive and Negative
Values . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
3.25 Find the Max and Min Sonar Working Range. . . . . . . . . . 54
3.26 Experiment To Test the Behavior of the Sonar 5 . . . . . . . . 55
3.27 Experimental Results of Rotating the Sonar 5 . . . . . . . . . 56
3.28 Comparison of the Sonar Sensor Reflection with Two Materials. 57
4.1 Rotational Motion of Robot . . . . . . . . . . . . . . . . . . . 59
4.2 Translational Motion of Robot . . . . . . . . . . . . . . . . . 60
4.3 A State Variable Control System . . . . . . . . . . . . . . . . 61
4.4 Non-linearized Mathematic Model of The AmigoBot. . . . . . 62
4.5 Linearized and Non-linearized Model of the Open loop Control
of the AmigoBot . . . . . . . . . . . . . . . . . . . . . . . . . 63
4.6 Standard Control Loop for the Mobile Robot. . . . . . . . . . 63
4.7 Building a Controller for the System. . . . . . . . . . . . . . 65
4.8 Linear Simulation Results. . . . . . . . . . . . . . . . . . . . 66
4.9 Linear Model with a Reference Input . . . . . . . . . . . . . . 67
4.10 Simulation Result of the Linear Model with a Step Reference
Input . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
4.11 Derivation of Y Values From The Sonar 5 . . . . . . . . . . . 68
4.12 State-Space Control with Observer . . . . . . . . . . . . . . . 69
4.13 The Final Nonlinear Model of the AmigoBot . . . . . . . . . . 69
4.14 Simulation Result of Sending a Reference Step Function to the
Final Nonlinear Model of the AmigoBot . . . . . . . . . . . . 70
4.15 The Top Level of the AmigoBots Final Control System . . . . 70
4.16 Subsystem Observer and Controller . . . . . . . . . . . . . . . 71
4.17 Sub Subsystem Observer . . . . . . . . . . . . . . . . . . . . . 71
4.18 Subsystem AmigoBot Plant of the Final Control System . . . 72
ix
-
LIST OF FIGURES
4.19 Sub Subsystem Protection and Position Translation . . . . . . 73
4.20 The Track for the AmigoBot. . . . . . . . . . . . . . . . . . . 74
4.21 Experiment A: The Received Sonar,Theta,U and the Given In-
put Information. . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.22 Experiment B: The Received Sonar,Theta,U and the Given In-
put Information. . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.23 Experiment C: The Received Sonar,Theta,U and the Given In-
put Information. . . . . . . . . . . . . . . . . . . . . . . . . . 79
x
-
Chapter 1
Introduction
This thesis is based on a research work in the Automatic Control Laboratory
at the Department of Electrical Engineering and Computer Science of the
University of Applied Sciences Hamburg. In the laboratory some mobile robots
are used in student courses and research works. These robots have their own
software and control package. On the other hand, the software tool MATLAB
is the primary software used in the department for communication and control
because of its advantages. Therefore it is a very interesting research topic to
develop an interface to drive the robots by using MATLAB.
There are two possibilities to implement this interface. The simplest
approach is to use the basic software (ARIA) from the robots provider (Ac-
tivMedia). This solution is only possible in the MATLAB simulation mode
and was investigated in another thesis, see [12]. The drawbacks of this method
are possibly the inaccurate timing and the dependency of the windows oper-
ating system. Because of these drawbacks, the second possibility comes with
the idea of implementing a real-time interface for the robot. The xPC Target
from the MATLAB family provides a small and simple real time kernel to re-
alize this interface. This interface will be used later for the automatic control
laboratory.
1.1 Problem Overview
In order to find the real-time behavior of the interface, a real-time control
application for the mobile robot by using MATLAB will be included in the
thesis. This real-time control application will control a mobile robot moving
1
-
1.1 Problem Overview
along a wall at a given distance. The track of the wall can be a straight one or
a curved one. The given distance can be set as a constant or as a mathematic
function (e.g. a step function).
One of the most important tasks for this thesis is to find out the func-
tionalities provided by the MATLAB for a real-time control system. MATLAB
is a high level, standard and widely spread software. It is a high-performance
language for technical computing. The Simulink, Real-Time Workshop and
xPC Target from the MATLAB family will be used in this thesis too, as they
are very powerful in building a real-time control system. The Simulink is a
software package used to model, simulate, and analyze systems whose outputs
change over time. The Real-Time Workshop is an extension of capabilities
found in Simulink and MATLAB to enable rapid prototyping of real-time
software applications on a variety of systems. The xPC Target is a solution
for prototyping, testing, and deploying real-time systems using standard PC
hardware. All these products from the MATLAB family are commonly used
to realize a real-time control application whose behavior is changed over time.
They are spread in almost every university, company and research center, so
in this thesis they are used to implement this real-time control application.
The final product of this thesis will be a Simulink block diagram running on
the xPC Target.
To realize this real-time control application a mobile robot and a real-
time environment are required for this project.
For the mobile robot the three-wheeled mobile robot AmigoBot from
ActivMedia was chosen in this application. AmigoBot is a small, 2 driven
wheels, differential drive and intelligent mobile robot. The AmigoBot drive
and sensor systems are powered and processed from a single controller, driven
by a high-performance, I/O-rich 20-MHz Hitachi H8 microprocessor. The
AmigoBot micro controller comes loaded with AmigOS operating system soft-
ware that manages all the low-level systems and electronics of the mobile robot.
It can be connected externally with RS232 serial link or wireless modem. The
AmigoBot comes standard with a single array of eight sonar sensors. The
sonar positions are fixed: one on each side, four facing forward, and two at the
rear, together providing nearly 360 degrees of range sensing. The sonar firing
rate is 20 Hz (50 milliseconds per sonar) and sensitivity ranges from 10cm (6
inches) to more than 5 meters (16 feet). The tasks of the AmigoBot in this
application are data acquisition and following the clients control command,
2
-
1.1 Problem Overview
once a connection between the client and the AmigoBot is established. The
AmigOS will send the position information of the robot back to the client and
waits for the clients commands. The position information of the robot comes
from the feed back information of different sensors. One of the most important
pieces of feed back information is the distance from the wall. This informa-
tion is detected by one of the sonar sensors just above the right wheel. The
AmigoBot has its own communication protocol so the communication block
works on the client has to follow the communication protocol of the AmigoBot.
The xPC Target from the MATLAB is chosen to support the real-time
environment. It is an environment that uses a target PC, separate from a host
PC, for running real-time applications. This environment includes a host PC
and a target PC. The host PC is a development platform which has Visual
C++, MATLAB Simulink and Real-Time Workshop. The target PC is just a
normal PC which can be booted with an xPC boot disk. xPC Target does not
require DOS, Windows, Linux, or any another operating system on the target
PC. Instead, the target PC is booted with a boot disk that includes the highly
optimized xPC Target kernel. It is created on the host PC by setting up the
xPC Target environment properties for example the xPC Target kernel specific
for either serial or network communication. To create a target application, a
Simulink model will be created at first. xPC Target then uses the Simulink
model, Real-Time Workshop, and a third-party compiler to create the target
application on the host PC. Real-Time Workshop provides the utilities to
convert the Simulink models into C code and then with a third-party C/C++
compiler, compile the code into a real-time executable. This executable is then
converted to an image suitable for xPC Target and uploaded to the target PC.
The task of the xPC Target in this application is to design a control system
for the robot by constructing a Simulink block diagram. This block diagram
will be uploaded to the target PC from the host PC. After the running of
the application, the target PC will communicate with the AmigoBot via a
wireless modem. The block diagram on the target PC will analyze the received
information from the robot and send new commands to the robot in order to
control it.
The implementation of this application is divided into two parts. The
first part of the application is to build a communication block in a Simulink
diagram. By using this Simulink block diagram the xPC Target can commu-
nicate with the AmigoBot in real-time. Here the communication means the
3
-
1.1 Problem Overview
xPC Target will establish a connection with the AmigoBot and through this
connection the application running on the target PC can receive information
from the robot and send control commands to the robot. The received infor-
mation will be sent to the control block and a new client command will be
generated. The content of the new client command depends on the result of
analyzing the received information. In this part the most important thing to
do is to establish a connection between the xPC Target and robot by following
the communication protocol of the AmigoBot. This part can be divided into
three steps:
1. The aim of the first step is to synchronize the connection and initialize
the robot. The synchronization is realized by sending and receiving some
synchronization packets. The initialization is done by sending initializa-
tion packets to the robot.
2. In the second step, the received information from the AmigoBot will be
decoded into categories.
3. The task of the third step is to send the client commands to the AmigoBot.
Problems occurring in this part are how to build the synchronization process,
how to decode the information from the robot and how to send different com-
mands to the robot. These problems will be solved by different solutions which
will be introduced during the implementation.
The second part of the application is to find a control method which
can control the robot moving along the wall with a given distance. This part
can be divided into two steps:
1. At first a mathematical description of the robot will be created and ana-
lyzed. From the mathematical view, the two driven wheeled robot can be
seen as a state-space system so the state-space control method is chosen
for this application. When using the state-space system to describe the
robot, there are only two state variables: the vertical position and the
angular position of the robot. Here the vertical and angular positions
described in a 2D plane are the y position and position. The horizontal
position of the robot is not of interest in this application. The two state
variables are needed by the controller to generate feed back information
to the robot. The feed back information is actually the speed at which
4
-
1.2 Organization of the Thesis
the two wheels should be set. In this step the first task is to find how to
get these two state variables. The distance from the robot to the wall
is the only information which can be used in this application. It can be
converted to the y position of the robot. It is detected by one of the
sonar sensors on the robot. Anther state variable cant be established
directly but it can be recovered by using an observer.
2. After the mathematical behavior of the robot is clear, the second task
in this part is to build a simulator to simulate the behavior of the robot.
By using this simulator the reasonable control parameters can be found
out to let the robot move along the wall in real time. In this application
there are two problems which affect the control system:
The sonars sensitivity is influenced by the robots position andthe material of the wall. The sonar sensor uses an ultrasonic wave
which reflects back from the wall to measure the distance from the
wall. If the sonar sensor has a large angle with the wall then there
may be no waves reflected back to the sensor which will result in a
wrong feed back information. This problem will be solved by using
a better material which can reflect the ultrasonic waves to every
direction. The details of the solution will be introduced later.
The sonar value will only be refreshed every 400 milliseconds bydefault. As the control block used in this application is a feed
back control system, the information coming from the robot is very
important. A feed back rate of 400 milliseconds is not sufficient. To
solve this problem a new sonar polling sequence is set to let only
one sonar sensor working all the time. This will lead the sonar value
to be refreshed every 100 milliseconds. How to realize this will be
explained in the following sections.
In the following chapters, how to realize this application and solutions to the
problems will be explained in detail.
1.2 Organization of the Thesis
This thesis is broken up into parts according to the main steps taken in the
realization of the real-time control system of a mobile robot: The next four
5
-
1.2 Organization of the Thesis
chapters detail the design process, the hardware, the software and the resulting
problems.
Chapter 2 outlines the design issue of the communication block of thisapplication. The design issue includes which hardware and software
are used in this project, and reasons for them. The interface of the
communication block.
Chapter 3 starts with the implementation of the communication block.In this chapter lots of experiments are used to analyze the behavior
of the AmigoBot. By working with these experiments some changes are
added to the project design which is mentioned in Chapter 2. During the
implementation some new problems and the problems listed in Chapter
2 are solved by the final model. In the last part of Chapter 3 some tests
are used to demonstrate how the communication block works.
Chapter 4 opens with the design of the control block. The mathematicalbackground of this control system will be given in the beginning of the
chapter. The simulation of the control block and the main factors which
will influence the control system will be shown too. At the end of this
chapter the implementation of the control system with some experiments
will be introduced.
Chapter 5 summarizes the contributions of this thesis and poses sugges-tions and goals for future work.
The appendices to this thesis give detailed information on specific topicsrelated to the work presented. Appendix A is the table AmigOS Com-
mand Set. Appendices B and C are some initial MATLAB m file used
in the project.
6
-
Chapter 2
Design of the AmigoBots
Communication Interface
In this section the design procedure of the communication model for the real-
time control system will be introduced. The design procedure starts from
choosing the software and hardware. After this the interface of the commu-
nication model will be given. In the following sections more details about
the design procedure will be explained. As mentioned in the introduction,
the hardware used in this application is the AmigoBot and xPCTarget(Host
and Target PC). The software used in this application is the AmigOS and the
MATLAB xPC Target software set.
2.1 The xPC Target
The title of this thesis shows this is a real-time control application. Currently,
most control applications are based on PC control and the use of the Windows
operating system. In this application the Windows operating system will only
be used on the host PC. The reason is that it is not an exact real-time operating
system and is expensive. For educational purpose and real-time requirements
of this application, the xPC Target is used to realize this control task. The
xPC Target is a solution for prototyping, testing, and deploying real-time
systems using standard PC hardware. It is an environment which uses a target
PC, separate from a host PC, for running real-time applications. Changing
parameters in the target application while it is running in real time, and
checking the results by viewing signal data, are two important prototyping
7
-
2.1 The xPC Target
tasks. xPC Target includes a command-line and graphical user interfaces to
complete these tasks.
2.1.1 Host and Target PC
The working procedure of the xPC Target is first to develop a real-time ap-
plication on the host PC. This application is built by creating a MATLAB
model file which uses the Simulink libraries. The model file will be compiled
as a real-time executable and then uploaded to the target PC. The target PC
will provide a real-time environment for the application. The application can
be started or stopped from the target PC or the host PC. Both the target and
host PC can control the application when it is running. For example the pa-
rameters of the application can be changed from the host or target PC during
the run time. The xPC Target hardware requires a host PC and a target PC.
The target PC only needs to have the I/O boards supported by xPC Target
and a boot disk. More details about the software on the host PC and target
PC and how they work together will be introduced in the following sections.
2.1.2 xPC Target Software Set
xPC Target is a PC-compatible product which is installed on a host computer
running a Microsoft Windows operating system. xPC Target requires the
following products from MathWorks:
MATLAB Control and interaction with the xPC Target software en-vironment and target application using a command-line interface.
Simulink Model dynamic physical systems and controllers using blockdiagrams.
Real-Time Workshop Convert Simulink blocks and Stateflow chartsinto C code.
C Compiler Use a third-party C compiler and Real-Time Workshop tobuild a target application. The C compiler can be a Microsoft Visual
C/C++ compiler (version 5.0, 6.0, or 7.0) or a Watcom C/C++ compiler
(version 10.6 or 11.0) or other supported compilers.
8
-
2.1 The xPC Target
xPC Target Embedded Option deploys stand-alone target applicationsand custom GUI applications that communicate with the target appli-
cation. Note that custom GUI applications can be created without the
xPC Target Embedded Options.
The MATLAB ,Simulink and C Compiler are common softwares used in every
university, they will not be detailed in this thesis. The xPC Target Embed-
ded Option will not be used in this application. The only thing need to be
mentioned is the Real-Time Workshop and MATLAB S-Functions (system-
functions).
Real-Time Workshop is an extension of capabilities of Simulink and
MATLAB that automatically generates, packages and compiles source code
from Simulink models to create real-time software applications on a variety
of systems. Real-Time Workshop provides the utilities to convert a Simulink
models into C code and then, with a third-party C/C++ compiler, compile
the code into a real-time executable. As shown in Figure(2.1) the main task of
Simulink
model.mdl
Real-Time Workshop Build
Target Language
Compiler
Make
model.exe
Real-Time Workshop
model.rtw
TLC program:
1.System target file
2.Block target file
3.Inlined S-Function
target files
4.Target Language
Compiler function
library
Run-time interface
support files
model.c
model.mk
Figure 2.1: Real-Time Workshop Code Generation Process
the Real-Time Workshop is to compile a Simulink model file to an executable
file which will work on different targets. More details about the Real-Time
Workshop can be find in [4].
9
-
2.1 The xPC Target
S-Functions (system-functions) provide a powerful mechanism for ex-
tending the capabilities of Simulink. The most common use of S-Functions is
to create custom Simulink blocks. It can be used for a variety of applications,
including:
Adding new general purpose blocks to Simulink Adding blocks that represent hardware device drivers Incorporating existing C code into a simulation Describing a system as a set of mathematical equations Using graphical animations
An advantage of using S-Functions is that it can be used to build a general
purpose block that can be used many times in a model, varying parameters
with each instance of the block. In this application the S-Functions work
together with the Real-Time Workshop to solve various kinds of problems.
These problems include:
Extending the set of algorithms (blocks) provided by Simulink and Real-Time Workshop
Interfacing existing (hand-written) C-code with Simulink and Real-TimeWorkshop
Generating highly optimized C-code for embedded systemsThe explanation of S-Function above means that the S-Function is a tool which
allows lets the developer to create their own MATLAB tool block for their
special applications. More details about the S-Function and why they are used
in this application will be introduced in Chapter 3.
2.1.3 Downloading A Model to the Target PC
In this section a simple introduction about how the xPC Target works will be
given. The first step is to build a connection between the host PC and the
target PC. There are two ways to connect them: one is the serial communi-
cation(e.g serial RS232) and the other is using network communication. Here
the network communication is used to connect the target and the host PC
because it has two advantages:
10
-
2.1 The xPC Target
Figure 2.2: Hardware Connection between Target PC and Host PC
Higher data throughput transfer up to 100 Mbit/second Longer distances between host and target computer
Figure(2.2) shows the connection of the Host PC and Target PC. The second
step is to create a Target Boot Disk. Figure(2.3) on Page 12 shows the config-
uration window of creating a Target Boot Disk. The third step is to boot the
Target PC with the created Target Boot Disk. After successfully booting the
Target PC a window like Figure(2.4) on Page 12 will appear which means the
connection between the Host PC and Target PC are connected. The fourth
step is to use a test program provided by MATLAB to test if a Simulink model
file can be uploaded to the Target PC and run. This is done in the MATLAB
Command Window, by typing xpctest. MATLAB runs the test script and
displays messages indicating the success or failure of a test.
### xPC Target Test Suite 2.0
### Host-Target interface is:TCP/IP(Ethernet)
### Test 1, Ping target system using standard ping: ... OK
### Test 2, Ping target system using xpctargetping: ... OK
### Test 3, Reboot target using direct call: ....... OK
### Test 4, Build and download xPC Target application using model
xpcosc: ... OK
### Test 5, Check host-target communication for commands: ... OK
### Test 6, Download xPC Target application using OOP: ... OK
### Test 7, Execute xPC Target application for 0.2s: ... OK
11
-
2.1 The xPC Target
Figure 2.3: Configuration of Target Boot Disk
Figure 2.4: xPC Target Boots, the Kernel and Display Information on theTarget PC
12
-
2.2 The AmigoBot
Figure 2.5: Loading a MATLAB Simulink Model to xPC Target
### Test 8, Upload logged data and compare it with
simulation: ... OK
### Test Suite successfully finished
The above message shows that the xPC Target is successfully connected. As
seen in Figure(2.5), the name of the model, the size of the model, the sam-
ple rate of the model and the status of the model are displayed in the xPC
Targets information window. More information about the xPC Target can be
found in [3] and also will be introduced in the following chapters during the
implementation of this application.
2.2 The AmigoBot
As described, the object to be controlled in this application is a mobile robot
called AmigoBot. The reasons for using the AmigoBot is that it is a commonly
used robot for educational purposes. The advantage of the AmigoBot and
more hardware specification of the AmigoBot can be found in [1]. Figure(2.6)
Sonar Sensor
28cm
33cm
Body
Figure 2.6: AmigoBots Physical Characteristics.
shows the physical characteristics of the AmigoBot. Here only the information
13
-
2.2 The AmigoBot
Client Application
Server Information Commands
Communication Packets
Velocity &Angle
ControlsPosition
Integration
Sonar & I/O
Schedules
PWM
Control
Encoder
Counting
Sonar
Ranging
I/O
Control
Server Informaiotn
Robot Specific Functions
Figure 2.7: AmigoBots Client-server Architecture
about the AmigoBots communication protocol and the sonar sensors will be
explained. The AmigoBot uses an intelligent client/server control architecture
developed by Dr. Kurt Konolige. In the model, the server works to manage all
the low-level details of the mobile robots systems. These include operating the
motors, firing the sonar, collecting sonar and motor encoder data, and so on.
The client application sends commands to the server and receives the returned
report from the server. Figure(2.7) shows the client-server architecture of the
AmigoBot. As described in the introduction the whole application is divided
into two steps: the communication model and the control model. Before
starting the design of the communication model and the control model, the
communication with AmigoBot via the AmigOS client-server interface will be
described.
2.2.1 Communication Packet Protocol
AmigOS communicates with a client application by using special packet proto-
cols: command packets from client to server, and Server Information Packets
14
-
2.2 The AmigoBot
(SIPs) from server to client. Both are byte data streams consisting of four main
elements: a two-byte header, a one-byte count of the number of command/data
bytes, the client command and its arguments or the server information data,
and finally, a two-byte checksum. The Table(2.1) lists the main elements of
AmigOS communication packet protocol.
Component Bytes Value Description
Header 2 0xFA, 0xFB Packet header; same for client and
server
Byte
Count
1 N + 2 Number of subsequent data bytes,
including checksum word, but not
Byte Count. Maximum 200 total
bytes.
Data N command or
SIB
Client command or server informa-
tion block (SIB; discussed in subse-
quent sections)
Checksum 2 computed Packet integrity checksum
Table 2.1: Main Elements of AmigOS Communication Packet Protocol
2.2.2 Packet Data Types
Client-command and server-information packets use integers (2 byte), words
(4 bytes), and strings (n < 200 bytes) as their data types. There is no sign
convention, as each packet type is interpreted idiosyncratically by the receiver.
Negative integers are sign-extended. The Table(2.2) shows the format of the
packet data types.
Data Type Bytes Order
integer 2 b0 low byte; b1 high byte
word 4 b0 low byte; b3 high byte
string up to 200, length-
prefixed
b0 length of string; b1 first
byte of string
Table 2.2: AmigOS Communication Packet Data Types
15
-
2.2 The AmigoBot
2.2.3 Packet Checksum
The checksum is used in almost every communication to check if there is
disturbance. The AmigOS communication protocol contains the checksum
too. It is calculated by successively adding data byte pairs (high byte first) to
the running checksum (initially zero), disregarding sign and overflow. If there
is an odd number of data bytes, the last byte is XORed to the low-order byte
of the checksum. Using the MATLAB existing tool block to calculate this
checksum for the robot is too complicated. An S-Function will be created to
calculate the checksum of the received block and compare it with the received
checksum to see if the received packet is wrong.
2.2.4 Packet Errors
Currently, AmigOS ignores a client command packet whose byte count exceeds
200 or has an erroneous checksum. The client should similarly ignore erroneous
server information packets. AmigOS does not acknowledge receipt of a com-
mand packet nor does it have any facility to handle client acknowledgment of
a server information packet.
2.2.5 Server Information Packets
Once connected, AmigOS automatically sends a packet of information back
to the client every 100 milliseconds, depending on the infoCycle setting in the
robot FLASH parameters. The standard AmigOS Server Information Packet
(SIP) informs the client about a number of the robots operating parameters
and readings, using the orders and data types shown in the Table(2.1) on
Page 15. AmigOS also supports several additional server information packet
types, including an alternative server information packet. Table(2.3) shows
the list of the format of SIP.
Name Data Type Description
Header integer Exactly 0xFA, 0xFB
Byte Count byte Number of data bytes + 2 Send SYNC0>Receive SYNC0
>Send SYNC1>Receive SYNC1 >Send SYNC2>Receive
SYNC2>POLLING Sonar >Send OPEN >ENABLE Mo-
32
-
3.1 Analyzing the Synchronization and Initialization Process
u1
case [ 1 ]:
case [ 2 ]:
case [ 3 ]:
case [ 4 ]:
case [ 5 ]:
case [ 6 ]:
case [ 7 ]:
case [ 8 ]:
case [ 9 ]:
case [ 10 ]:
case [ 13 ]:
State Machine
Tz-1
State Counter
case: { }Send SYNC1
case: { }Send SYNC0
case: { }Send OPEN
case: { }Send SYNC2
case: { }Receive SYNC2
case: { }Receive SYNC1
case: { }Receive SYNC0
case: { }Polling Sonar
NOT 2
case: { }Enable Motor
case: { }Disconnect
auto(double)
case: { }Clear the Receive Buffer
1Start
Figure 3.4: Synchronization & Initialization.
33
-
3.1 Analyzing the Synchronization and Initialization Process
tor. If the input Start changes to 0 then the process Disconnect will
be executed.
1. Clear the Receive Buffer: In this state a built-in S-Function will clear
the RS232 received buffer on the Target PC. Figure(3.5) shows the RS232
Current Received Byte
Current Read Byte
Figure 3.5: The RS232 Received Buffer on the Target PC
buffering mechanism of the target PC. The buffer is a circular buffer with
a size of 1024 bytes. There are two pointers of the circular buffer: one
points to the current received byte from the AmigoBot, the other points
to the current read byte by the receive block. If the receiving speed is
much faster than the reading speed and once the differences are greater
than 1024 bytes, the old unread bytes will be wrapped up by the newly
received bytes. If the receiving speed is slower than the reading speed,
the receive block will wait until a new byte comes. Why the first state
is the Clear the Receive Buffer is already explained in the previous
section for a case such as Table(3.3).
2. Send SYNC0: In this state the SYNC0 packet will be sent. Figure(3.6)
on Page 35 shows the SYNC0 packet is sent by the RS232 Binary Send
Block. The RS232 Binary Receive Block is a tool block provided by
the Simulink Library for the xPC Target. In this state a 6 bytes long
packet 0xFA 0xFB 0x03 0x00 0x00 0x00 will be sent to the AmigoBot
via the COM1 port of the Target PC with a sample rate inherited from
the upper block.
34
-
3.1 Analyzing the Synchronization and Initialization Process
[250 251 3 0 0 0]SYNC0
RS232 SendCOM1
RS232Binary Send
case: { }Action Port
Figure 3.6: Sending the SYNC0 Packet
3. Receive SYNC0: In this state the receiving process will follow the
flow chart given in the previous section. Figure(3.7) shows how this
UnpackUnpack
[250 251 3 0 0 0]
[250 251 3 0 0 0]
==
==
==
==
==
Data
Enable
RS232 SendCOM1
Data
Enable
RS232 SendCOM1
Length
Enable
Done
Data
RS232 ReceiveCOM1
Length
Enable
Done
Data
RS232 ReceiveCOM1
NOTOR
AND
3Length
255FF
251FB
250FA
(double)
(double)
auto(double)
3
0
1
3
case: { }
Figure 3.7: Receiving the Echo of SYNC0
receiving procedure works. The first step in this state is to receive 3
bytes. If the three bytes is 0xFA 0xFB 0x00 this means the SYNC0
is received by the AmigoBot and the process will go to the next state.
If not the program will receive the next 3 bytes. The second step is to
check these 3 new bytes, if they are 0x00 0x00 0x00 then the process
can go to the next state otherwise the SYNC0 has to be sent again.
4. Send SYNC1: In this state the SYNC1 packet will be sent.
35
-
3.1 Analyzing the Synchronization and Initialization Process
5. Receive SYNC1: In this state the echo of SYNC1 or the echo of the
SYNC0 packet will be received. Whether the echo of the SYNC1 or
SYNC0 will be received depends on the state Receive SYNC0. If
in the state Received SYNC0 there is no packet SYNC0 sent, then
the echo of SYNC1 will be received in this state. Otherwise the echo of
SYNC0 will be received. Figure(3.8) shows how this receive procedure
Packet Length
Unpack==
==
Length
Enable
Done
Data
RS232 ReceiveCOM1
Length
Enable
Done
Data
RS232 ReceiveCOM1
AND
251FB
250FA
auto(double)
auto(double)
1
3
case: { }
Figure 3.8: Receiving the Echo of SYNC1
works. This procedure is a standard receiving block for the AmigoBot.
Firstly it receives the first three bytes of an AmigoBot communication
packet and finds out the length of the received packet. Second it config-
ures another receiving block to receive the rest data of the packet. This
procedure guarantees a whole packet is received when the length of the
packet is unknown.
6. Send SYNC2: In this state the SYNC2 packet will be sent.
7. Receive SYNC2: In this state the echo of the SYNC2 or SYNC1
packet will be received.
8. POLLING Sonar: In this state a sonar sensor firing sequence will be
set to the AmigoBots sonar server. The sequence is defined in the initial
file and can be changed before each start of the application.
9. Send OPEN: Before this state the synchronization process is finished
and the xPC Target should successfully be connected to the AmigoBot.
36
-
3.2 Receive and Decode the SIPs
Now the initialization process can be started. The first task of the ini-
tialization process is to send an OPEN command to the AmigoBot.
10. ENABLE Motor: In this state the command ENABLE motor will
be sent and the motor of the AmigoBot will be started.
This is the implementation of the synchronization and the initialization subsys-
tems. When the synchronization and initialization are finished the AmigoBot
will start to send its SIPs to the client.
3.2 Receive and Decode the SIPs
3.2.1 Subsystem Receive the SIPs
In the following part, the functionality of the subsystem Receive and De-
code the SIPs will be introduced. Figure(3.9) shows the subsystem Receive
2Data
1Check Sum
u1 case [ 10 ]:Tz-1
State CounterRate Transition
auto(double)
case: { }CheckSum
Data
1Start
Figure 3.9: Subsystem Receiving SIP
SIP. In this subsystem a synchronous State Counter is used. This State
Counter works the same as the State Counter on the subsystem Synchro-
nization and Initialization and both of these two counters have the same
output value at the same time. The only difference is that before this State
Counter a Rate Transition tool block is used. This Rate Transition will
convert the sample time of this block from 1 second to 0.1 second which means
the subsystem connecting to the output, works with a sampling time of 0.1
second too. The reason of using it is that the AmigoBot sends the SIP packets
with a rate of 0.1s and the client has to send the commands with a rate of 0.1s
also. this subsystem there is another subsystem which has two outputs. The
two outputs are the checksum and received data. This subsystem will start
37
-
3.2 Receive and Decode the SIPs
to work only after the application has already been running for 10 seconds.
The reason is that the State Counter will enable the subsystem connect to
it when the counter counts 10. Figure (3.10) shows the sub subsystem Re-
2Data
1CheckSum
Data
CheckSum
AmigoBot SIP RS232 Receive 8COM1
AmigoBot SIP Receive Block
case: { }Action Port
Figure 3.10: Sub Subsystem Receiving SIP
ceive SIP. In this sub subsystem there is only a receive block. The receive
block contains an S-Function called rs232brec amigo8. This S-Function did
the most important job of the whole block. When this S-Function starts the
following situations may occur:
1. There is less than one SIP in the circular buffer.
2. There are several SIPs already in the circular buffer.
In the first step it will receive all the bytes from the target PCs RS232 circular
buffer. A variable bufCount will return the number of bytes in the circular
buffer. For the second step it will loop through these bytes, find if there are
any header bytes in the buffer. A variable current is used as an index to
help copying the bytes to the output buffer. Once the header is found, the
program will start to process a packet called OUTPUTPACKET which is a
packet just before this header and in the buffer. The third step is to calculate
the check sum of the packet OUTPUTPACKET by using the index variable
current. If the calculated check sum is the same as the received check sum
then the OUTPUTPACKET will be sent to the output buffer and the index
variable current will be reset. This programming logic will suit for both
situations listed above. If there is less than one SIP in the circular buffer, it
will wait until the whole packet arrives. If there are several SIPs in the buffer,
it will send the last received whole packet to the output buffer, free the spaces
in the buffer except the rest of the bytes in the buffer which belong to the next
38
-
3.2 Receive and Decode the SIPs
packet. This programming logic will also guarantee the distorted packet will
not be output. The following is part of the S-Function.
.......
//Read how many bytes are in the circular buffer
bufCount = rl32eReceiveBufferCount(port);
/*every time put all the received data into the buf*/
//Loop through the whole buffer and find the header position
while (bufCount) {
tmp = rl32eReceiveChar(port);
if ((tmp & 0xff00) != 0) {
printf("RS232Receive: Error\n");
return;
}
//Find continues "FA FB" as header
if(tmp==250) *findFA=*current;
if(tmp==251) *findFB=*current;
if((*findFB-*findFA)==1)
{
*findFAFB=1;
*findFA=*findFB=0;
}
//Once find the Header
if(*findFAFB==1)
{
//Calculate the CheckSum
for(i=2;i
-
3.2 Receive and Decode the SIPs
if(sum==(buf[*current-3]
-
3.2 Receive and Decode the SIPs
10SonarRangeB
9 SonarIndexB
8SonarRangeA
7 SonarIndexA
6SonarReading
5X
4Y
3Th
2Right
1Left
6a7 Y
get Y
4a5 X
get X
8a9 Th
get Th
22a28
SonarReading
SonarIndexA
SonarRangeA
SonartIndexB
SonarRangeB
get Sonar Readings
12a13 Right
get RightWheel Speed
10a11 Left
get Left Wheel Speed
Target ScopeId: 2
Scope (xPC) 2
Target ScopeId: 1
Scope (xPC) 1
1 Received data
Figure 3.11: Decoding the SIP to User Wanted Data
1Left
WHEELSPEEDFACTORData ValueAmigo Converter
Data converter
auto(double)1
10a11
Figure 3.12: Sub Subsystem to Detect the Speed of the Left Wheel
41
-
3.2 Receive and Decode the SIPs
amigo convert is given by two parameters the Start and the Mode to
reform the bytes to the user wanted value. The following piece of code is part
of the S-Function amigo convert. The parameter Start is used to signify
where the start position of the wanted data in the packet is. The parameter
Mode is used to tell how many bytes are used to compose the wanted data
and what kinds of value it is.
...
// get the start position of the value
// you want to reform
int start = (int)mxGetPr(START_ARG)[0];
// Select the Mode byte,unsigned int,signed int,int
int mode = (int)mxGetPr(MODE_ARG)[0];
int *y =(int *)ssGetOutputPortSignal(S,0);
int tmp;
unsigned char *u=(unsigned char *)ssGetInputPortSignalPtrs(S,0);
switch (mode)
{
// Byte
case 0 : tmp=(int)u[start]; break;
// Unsigned Integer
case 1 : tmp=(int)(u[start]+((unsigned short int)
u[start+1]
-
3.3 Setting the Wheel Speed
teger. The S-Function finds the start byte, left shifts the next byte by 8 bits
and then plus them together to get the speed. Figure(3.13) shows the sub sub-
5SonarRangeB
4SonartIndexB
3SonarRangeA
2SonarIndexA
1SonarReading
u1 if(..)
if { }In1 Out1
if { }In1 Out1
u1 if(..)
Data ValueAmigo Converter
Data ValueAmigo Converter
Data ValueAmigo Converter
Data ValueAmigo Converter
Data ValueAmigo Converter
auto(double)
auto(double)
auto(double)
auto(double)
auto(double)
1
22a28
Figure 3.13: Sub Subsystem Get the Sonar Values: New Sonar Readings Sonar-RangeA and SonarRangeB
system of getting sonar sensor values. Normally for a 100 milliseconds system
each SIP contains two new pieces of sonar information and for a 50 milliseconds
system only one new sonar value will be given. This sub subsystem provides
a functionality to choose the sonar values which are of interest. For example
if the sonar 4 and 5 are of interest, the user needs to set two variables in an
initial file:
SONARINDEX100A=4; SONARINDEX100B=5;;
This means if the output SonarIndexA=SONARINDEX100A=4 then the Sonar-
RangeA outputs the new measured data of sonar 4.
3.3 Setting the Wheel Speed
Figure(3.14) on Page 44 shows the block of creating a command packet for
setting wheel speed. The Table(A.1) on Page 83 shows the AmigOSs Com-
mand Set and the Table(2.4) on Page 19 shows the format of the AmigOSs
43
-
3.3 Setting the Wheel Speed
SPEEDFACTOR
SPEEDFACTOR
round
round Right
Left
Enable
AmigoBot Speed Command
RS232 SendCOM1
Give Speed
auto(int8)
auto(int8)
3Left
2Right
1Enable
Figure 3.14: Generating a Setting Wheel Speed Command Packet
Client Command Packet. These two tables together describe how to build a
Client Command Packet. Here the command number #32 is used to set the
wheel speed. The speed can be set to a positive or a negative value which
depends on the setting of the commands argument. An S-Function called
rs232bsend speed is created to format this command packet. The S-Function
will first check if the Enable input is set. If it is not set the program will
just do nothing otherwise the program needs to generate a command packet.
As known the argument type of the speed command VEL2 is integer and
the most significant bit signs to the speed of the left wheel. This means the
Argument Type parameter described in Table(2.4) is chosen by the speed of
the left wheel. If the speed of the left wheel is set to a non-negative value then
the value 0x3B will be put into the command packet, otherwise the value
0x1B will be used. After the argument type is fixed, the argument has to be
filled into the packet. In the specification of the AmigoBot there is no more
information about how to format this client command packet except in the
Table(A.1). The following examples of setting the speed command are found
by the author himself by experiments. Five examples of the speed commands
are given in Figure(3.15) on Page 45. In the previous section, the speed given
to the system is in a unit of mm/sec but the speed command packet only
accepts a unit of 40mm/sec. This means the value will be divided with 40 and
then fill into the corresponding position in the speed command packet.
1. Experiment 1 shows that both of the wheel speeds are positive so the
fifth byte of the packet is set to 0x3B. The S-Function will put the
speed of the right wheel into the sixth byte and the speed of the left wheel
to the seventh byte. The eighth and ninth bytes are the check sum of
44
-
3.3 Setting the Wheel Speed
FA FB 06 20 3B 01 01 21 3C
Header1 Header2 Length Command
VEL2
Left Wheel
Speed
Right Wheel
Speed
Argument
Postive
Integer
Check Sum
+
+
FA FB 06 20 1B 03 02 23 1D
Header1 Header2 Length Command
VEL2
Left Wheel
Speed
Right Wheel
Speed
Argument
Negative
Integer
Check Sum
Example1:
Left Wheel Speed: 1*40mm/sec=40mm/sec
Right Wheel Speed 1*40mm/sec=40mm/sec
Example3:
Left Wheel Speed: -3*40mm/sec=-120mm/sec
Right Wheel Speed -3*40mm/sec=-120mm/sec
FA FB 06 20 1B FF 00 1F 1B
Header1 Header2 Length Command
VEL2
Left Wheel
Speed
Right Wheel
Speed
Argument
Negative
Integer
Check Sum
Example4:
Left Wheel Speed: -1*40mm/sec=-40mm/sec
Right Wheel Speed 1*40mm/sec=40mm/sec
FA FB 06 20 1B 00 01 20 1C
Header1 Header2 Length Command
VEL2
Left Wheel
Speed
Right Wheel
Speed
Argument
Negative
Integer
Check Sum
Example5:
Left Wheel Speed: -1*40mm/sec=-40mm/sec
Right Wheel Speed 0*40mm/sec=0mm/sec
FA FB 06 20 3B FF 01 1F 3C
Header1 Header2 Length Command
VEL2
Left Wheel
Speed
Right Wheel
Speed
Argument
Negative
Integer
Check Sum
Example2:
Left Wheel Speed: 1*40mm/sec=40mm/sec
Right Wheel Speed -1*40mm/sec=-40mm/sec
Figure 3.15: Five Examples of the Speed Commands
45
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
the packet. The first byte of the check sum is calculated by adding the
fourth byte with the sixth byte. The second byte of the check sum is
calculated by adding the fifth byte with the seventh byte.
2. Experiment 2 is the case that the speed of the left wheel is a positive one
and the speed of the right wheel is a negative one. In this case the speed
of the right wheel -1 will be set to the fifth byte which presented by a
signed byte 0xFF. Now the sum of the fourth and sixth byte is equal
0x11F, but only the lower 8 bits 0x1F will be put into the first byte
of the check sum.
3. Experiment 3 shows that the both of the wheel speeds are negative so the
fifth byte of the packet is set to 0x1B. Now the format of the packet will
be different from the case when the left wheel speed is positive. Please
notice now the value of the sixth byte in the packet is the absolute value
of the right wheel speed and the value of the seventh byte is the absolute
value of the left wheel speed minus 1.
4. Experiment 4 is the case that the left wheel speed is negative and the
right wheel speed is positive. In this case, the value of the sixth byte
in the packet is presented by a signed byte 0xFF and the value of the
seventh byte is the absolute value of the left wheel speed minus 1.
5. Experiment 5 shows that the left wheel speed is negative and the right
wheel speed is 0. Now the value of the sixth byte is 0 and the value of
the seventh part is the absolute value of the left wheel speed.
The S-Function rs232bsend speed is designed base on these experiments
and the programming logic of this S-Function please look at the electrical file
deliver together with this thesis.
3.4 Testing the Communication Block and An-
alyzing the Server Information
The previous sections explained how the communication model is realized
and how the AmigoBot behaves during the communication process. In this
section some figures will illustrate the received information from the AmigoBot
46
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
such as the wheel speed, the robot position, the sonar sensor values and so
on. Figure(3.16) shows the timing sequence diagram of the communication
0 1 2 3 4 5 6 7 8 9 10 Time Line (seconds)11 12Clear the Received Buffer
Send SYNC0
Receive SYNC0
Send SYNC1
Receive SYNC1
Send SYNC2
Receive SYNC2
POLLING Sonar
Send OPEN
ENABLE Motor
Synchronization
Initialization
Start to receive SIP every 0.1 second
The first correct SIP has been
receiced then send the first
client command
Start to send client command every 0.1 second
The first client command is
accepted. Wheel speed is
accelerated
Figure 3.16: Timing Sequence Diagram of the Communication Block.
block. This sequence diagram indicates the expected timing behavior of the
communication block: Synchronization process will start from the 1st second
and finish at the 7th second. Initialization process will start from the 8th
second and finish at the 10th second. From the 11th second on, the SIP will be
received in every 100 milliseconds. The first correct SIP will be received at 11.2
second and at the same time the first client command will be sent. From the
11.2 second on, the client command will be send in every 100 milliseconds too.
The AmigOS will accept the first client command with a delay of 0.2 seconds
so the wheel speed will be set around 11.5 second. Figure(3.17) on Page 48
shows an experiment result of the communication blocks timing behavior. The
result of the experiment proves the correctness of the above sequence diagram.
It also shows why the SIPs are received from the 11th second but not the 10th
second. The reason is for the second state counter the sampling rate is 100
milliseconds so it will reach the value 10 from the 11th second. This figure
also shows there is a delay between the first correctly received SIP and the
first set wheel speed. The reason for this delay is the AmigoBot needs time to
accept the client command and accelerate the wheel speed.
As described in the specification most of the received values need to be
multiplied with a factor to get the real value. Table(2.3) on Page 16 contains
47
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 5 10 15 20 25 30 35 400
2
4
6
8
10
12
Time
Valu
e
Timing Analysis
ChecksumState Counter1State Counter2Speed/120
Figure 3.17: Time Analysis of the Communication Block
the list of the operating parameters which need to be multiplied with a factor.
The first operating parameter to be discussed is the robots position. The
position of the robot contains three kinds of information: the X position, the Y
position and the Theta position. These three kinds of information are relative
to the start point of the robot. Figure(3.18) shows the received X position.
0 5 10 15 20 25 30 350
1000
2000
3000
4000
5000
6000
Time
Valu
e
X Multiplied With Factor and Original X
X Multiplied With A Factor 0.5083Original X
Figure 3.18: Received X Position Information.
The red curve is the original data received from the SIP packet. The blue
48
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
curve is the original data multiplied with a factor of 0.5083. The blue curve is
proved as the actual value by measuring how far the robot moves with a ruler.
In this measurement both wheels of the robot have the same speed. This figure
also shows the sampling time of the model to be 100 ms while the X position
is increased every 100 ms. The Y position has the same behavior as the X
position and also needs to be multiplied with a factor 0.5083. Figure(3.19) on
Page 50 shows the case when two wheel speeds are set to different values. All
these values are already multiplied with the corresponding factors listed in the
Table(2.3). The theta values are converted to degrees in this MATLAB model.
The first figure shows that even though the given wheel speeds are constants,
the real speeds are always oscillating around the given values. The second
figure shows the position information of the robot if the two wheel speeds are
different. The third figure indicates the robot will move following a circular
track because of the different wheel speeds.
The previous section shows the positional information of the robot. Now
the motion information of the robot will be introduced the wheel speed of
the robot. Figure(3.20) on Page 51 shows the received wheel speed and the
speed calculated from the X position. In the experiment, the speed of both
wheels are set by the Client Command 0xFA 0xFB 0x06 0x20 0x3B 0x03
0x03 0x23 0x3E. As described in the Table(A.1) on Page 83 this means the
outputted wheel speed should be 60mm/sec. The figure on the left shows
the actual received value is about 90mm/sec. But the calculated speed from
the X position of the robot on the right figure is: speed=BC/BA=(2154-
996.3)/(30-20)=115.7mm/sec. This creates a problem: why the speeds are
different and which one is the correct one. In order to investigate the sys-
tem, the following experiment was done: Give 7 stepped speeds to both
wheels, for example in the client command set speed to 0,1,2,3,4,5 and 6 which
means 0mm/sec,20mm/sec,40mm/sec,60mm/sec, 80mm/sec,100mm/sec and
120mm/sec. Figure(3.21) on Page 51 shows the result of the experiment. The
figure shows the speed is increased about 30mm/sec for each step. The first
explanation of the problem may be that the quantization always affects the
speed by factors of (20mm+40mm)/2 sec=30mm/sec. Now why the given
speed is different to the received speed is clear. The quantization function of
the AmigOS will always quantize the input speed to the nearest step.
49
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 5 10 15 20 25 30 35 40 450
50
100
150
200
250
300
Time
Spe
ed
Left And Right Wheel Speed
Left Wheel Speed 2Right Wheel Speed 6
0 5 10 15 20 25 30 35 40 45-300
-200
-100
0
100
200
300
400
500
Time
Valu
e
X,Y Theta
ThetaYX
0 50 100 150 200 250 300 350 400 450 500-250
-200
-150
-100
-50
0
50
100
150
200
250
X Position
Y Po
sitio
n
AmigoBot Moving Track
Figure 3.19: Setting Different Speeds of Left and Right Wheels
50
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 5 10 15 20 25 30 350
20
40
60
80
100
120
140
Time
Spe
ed
Left And Right Wheel Speed
Left Wheel SpeedRight Wheel Speed
0 5 10 15 20 25 30 350
1000
2000
3000
4000
5000
6000
Time
Valu
e
X Multiplied With Factor and Original X
X Multiplied With A Factor 0.5083Original X
A (20 996.3) B (30 996.3)
C (30 2154)
Figure 3.20: Analyzing the Received Wheel Speed.
0 20 40 60 80 100 120 1400
20
40
60
80
100
120
140
160
180
200
Time
Spe
ed
Left And Right Wheel Speed
Left Wheel SpeedRight Wheel SpeedRamp * 3Set Speed * 30Set Speed * 20
Figure 3.21: Received Left Wheel Speed with 7 Steps.
51
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 20 40 60 80 100 120 1400
5000
10000
15000
Time
Valu
e
X
X
A(20 0) B (40 767)
C (60 2301)
D (80 4653)
E (100 7722)
F (120 11608)
Figure 3.22: Received X Position Information (Wheel Speed with 6 Steps).
Figure(3.22) shows the corresponding X position information of a given
wheel speed with 6 steps. From this figure the wheel speed can also be calcu-
lated from the X position.
Set Speed Received Wheel Speed Measured Speed From X Position
20 30 38.35 (B-A)/20
40 60 76.7 (C-B)/20
60 90 117.6 (D-C)/20
80 120 153.45 (E-D)/20
100 150 194.3 (F-E)/20
Table 3.5: AmigOS Wheel Speed Analysis (mm/sec)
Table(3.5) shows the problem of getting the actual speed of the AmigoBot.
From the experiment the measured speed is proven to have the same value as
the speed calculated from X position. The second explanation of the problem
why the given speed is different to the measured speed, still needs to be
discussed. One possible explanation is that there is a forgotten factor in the
specification. But before discovering the answer, the calculated speed from X
position will be chosen as the actual speed of the robot.
52
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 20 40 60 80 100 1200
500
1000
1500
2000
2500
3000
3500
4000
Time
Spe
ed
Left And Right Wheel Speed
Left Wheel SpeedRight Wheel SpeedRampSet Speed
Figure 3.23: Find The Maximum Speed.
Figure(3.23) shows the measured maximum speed as can be set to the
robot. In this experiment the wheel speeds are set with 10 steps. The step size
is 10 which means the speed will increase 400mm/sec in each step until it reach
the maximum value. As discussed before, there may be a forgotten factor in
the specification and the actual speed of the robot equals the measured value
multiply by 1.33. The maximum measured speed shown in the Figure(3.23) is
about 1590mm/sec so the maximum actual speed will be around 2114mm/sec.
0 10 20 30 40 50 60 70 80-500
0
500
1000
1500
2000
2500
3000
Time
Valu
e
X,Y Theta
ThetaYX
0 10 20 30 40 50 60 70 80-150
-100
-50
0
50
100
150
Time
Spe
ed
Left And Right Wheel Speed
Left Wheel SpeedRight Wheel Speed
Figure 3.24: Set the Left and Right Wheel Speed to Positive and NegativeValues
Figure(3.24) shows the model starting with a given speed 160mm/sec
for both wheels and after a certain time change the speed of the right wheel to
53
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
-160mm/sec then at last change the speed of the left wheel to -160mm/sec too.
This figure shows that the S-Function rs232bsend speed works as expected.
The x,y and position of the robot shows that the robot will move forward
when both speeds are positive. When the right wheel speed is negative and the
left speed is positive, the robot will rotate clock wise at the same x position.
When both speeds are negative, the robot will move straight forward again
from the point where the speed are set to negative.
In the following part the sonar information of the robot will be intro-
duced. In this project the sonar information will be used by the controller of
the robot. The AmigoBot has 8 sonar sensors which together provids nearly
360 degrees of range sensing. Figure(3.25) shows an experiment to find the
0 10 20 30 40 50 60 70 800
1000
2000
3000
4000
5000
6000
7000Analysis The Sonar Working Range
Time(s)
Dis
tance
To
W
all (
mm
)
Figure 3.25: Find the Max and Min Sonar Working Range.
maximum and minimum sonar working range. The experiment is like this:
First disable all the other sonar sensors except the one in the front. Second let
the robot move from a large distance towards the wall. Third hold the robot
by hand and move back from the nearest distance to far away from the wall.
When moving back keep the sonar vertical to the wall and move slowly. This
figure shows the working range of the sonar is from about 6880 mm to 160 mm
and the sonar will recognize the distance larger or smaller than the range as
54
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
the maximum or minimum value. When letting the robot itself move towards
the wall some times the sonar will lost the signal reflected back and recognize
the wall as being far away. The reason for this is the sonar is not directly fac-
ing to the wall and some ultrasonic waves will be reflected in other directions
which cant be retrieved by the sensor. In this project, the robot will move
forward and follow a wall with a distance of 300mm. Figure(3.26) shows an
WALL
300mm
0
10
20
30
40
-10
-20
-30
-40
Figure 3.26: Experiment To Test the Behavior of the Sonar 5 .
experiment to test the behavior of the Sonar 5. As shown on the figure, the
wall is about 300mm away from the robot. The AmigoBot will be rotated by
hand. The center of the rotation is the Sonar 5. First the AmigoBot is ro-
tated clock wise with a step of 10 degrees. The measured values corresponding
to theta, equals -10 degrees ,-20 degrees,-30 degrees and -40 degrees. Second
AmigoBot is returned to the original position and rotated counter clock wise.
The measured value corresponding to theta equals 10 degrees ,20 degrees,30
degrees and 40 degrees. Figure(3.27) on Page 56 shows the experimental re-
sults of rotating the sonar 5. The figure shows the useful range of the sonar 5
is between 10 and -10 degrees. This experiment shows that the useful range
of the sonar sensor is very small and cant be used for any control application.
To solve this problem a special material is used to allow more ultrasonic wave
be reflected. The surface of this material is composed of lots of cylindrical
barriers which reflect the ultrasonic waves in every direction.
55
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 50 100 150 200 250 300 3500
500
1000
1500
2000
2500Analysis The Sonar Value With The Angle To Wall
Time(s)
Dis
tance
To
W
all (
mm
)
299.1651 326.9944
0 -10
906.7718 -20
1829 -30
1811 -40
303.8033 10
1222 20
2029 30
2043 40
Figure 3.27: Experimental Results of Rotating the Sonar 5 .
Figure(3.28) on Page 57 shows the comparison of two wall materials
working with the sonar sensors. The upper figure is the new material and it
shows the sonar values are increased and decreased with a slow slope. The
lower figure is the old material and it shows the sonar will always lose the
reflected wave which will cause the maximum sonar values to be returned. In
this control application a wall built by the second material will be used to
improve the controllability of the system.
56
-
3.4 Testing the Communication Block and Analyzing the ServerInformation
0 20 40 60 80 100 120 140-1000
0
1000
2000
3000
4000
5000
6000
7000
Time
Valu
eSonar 5 And Theta
SonarTheta
0 20 40 60 80 100 120 140 160-1000
0
1000
2000
3000
4000
5000
6000
7000
Time
Valu
e
Sonar 5 And Theta SonarTheta
Figure 3.28: Comparison of the Sonar Sensor Reflection with Two Materials.
57
-
Chapter 4
A Real-Time Control
Application for the AmigoBot
In preceding chapters the AmigoBots communication block and its functions
were introduced. In this chapter a control application will use the developed
communication model to control the AmigoBot following a wall with a certain
distance. In the following sections the mathematic model of the robot, the
state space control system, the control loop of the AmigoBots controller and
the test of the controller will be explained.
4.1 Mobile Robot Control
The main part of the following section is taken from [2]. The reason of in-
cluding this section is in order to clarify the basic control model of a mobile
robot. The motion of a typical mobile robot can be controlled by setting the
velocities (v1, v2) of each of the two main wheels. With constant v1, v2 the
center of the robot moves with speed v = 12(v1 + v2) on a circle that has its
center on the wheel axis, see Figure(4.1) on Page 59.
v1 = r1 Left Wheel Speed
v2 = r2 Right Wheel Speed (4.1)
The speed of rotation = of the robot is thus given by
=v2 v1r2 r1 (4.2)
58
-
4.1 Mobile Robot Control
b
r1 r2
v1
v2
L
Center Of Turn
Reference
Direction
Figure 4.1: Rotational Motion of Robot
Assume the average wheel velocity is v and introduce the control variable u
then two velocities can be expresses as:
v1 = v u (4.3)v2 = v + u (4.4)
Thus:
=2
bu (4.5)
where b is the wheel base.
x = v cos
y = v sin (4.6)
Equation(4.6) is a linear first order differential equation relating the orientation
of the robot to the control variable u. To get a full description of the robot
motion, the translational motion also needs to be considered, which is depicted
in Figure(4.2) on Page 60. Here v can be viewed as a constant, which was
chosen in Equation(4.3). As the distance of the robot from the x-axis will be
controlled, the x-equation is not important thus only two differential equations
are useful. These equations can be put into the form of a state space system.
59
-
4.1 Mobile Robot Control
Y
X
.
X
.
Y
V
Figure 4.2: Translational Motion of Robot
More details about the state space system will be given in the next section.
Here the state vector x = (x1, x2) = (, y) is chosen.
x1 =2
bu (4.7)
x2 = v sinx1 (4.8)
y = x2 (4.9)
Here b and v are given constants. This is the so called state space description
of a robot system from ([2]).
4.1.1 State Space System Basics
This section begins by considering the purpose and advantages of using the
state-space design. The advantage of using the state-space control for a mobile
robot is the mathematic model of the robot is a state-space system. In state-
space design, the control engineer designs a dynamic compensation by working
directly with the state-variable description of the system, see [5]. From this
point of view, the idea of state space comes from the state-variable method
of describing differential equations. In this method the differential equations
describing a dynamic system are organized as a set of first-order differential
equations in the vector-valued state of the system. The state-space represen-
60
-
4.1 Mobile Robot Control
tation for a system of linear differential equations is given by the equations:
x(t) = Ax(t) +Bu(t) (4.10)
y(t) = Cx(t) +Du(t) (4.11)
where x is an n by 1 vector, representing the state (commonly position and
velocity variable in mechanical systems), u is a scalar, representing the in-
put (commonly a force or torque in mechanical systems), and y is a scalar
representing the output. The matrices A (n by n), B (n by 1), and C (1
by n) determine the relationships between the state and input and output
variables, see [11]. Figure(4.3) shows the basic structure of a state-space
B
A
C
D
+
+ +
+yu
Figure 4.3: A State Variable Control System
system. Note that there are n first-order differential equations. State-space
representation can also be used for systems with multiple inputs and outputs
(MIMO), but here a single-input, single-output (SISO) systems will be used
for the AmigoBot.
4.1.2 State Space System of the AmigoBot
In this section how to start the state-space control of the AmigoBot will be
given. Equation(4.9) shows the two-wheeled mobile robot itself is a state space
system so the state space control can be used to control this system. From
the introduction in the previous section the first step for a state space control
system is to find the control matrices: A,B,C,D and the state variable. The
Equation(4.9) shows for a two-wheeled mobile robot: x= [ y ] which is the set
of state variables for the system (a 2x1 vector), u is the input(the difference
of the wheel speed), and y is the output (the robots position in the Y axis).
These equations describe that this robot system is a nonlinear system as there
is a sin() function in the equations. As known when the is very small the
61
-
4.1 Mobile Robot Control
sin() can be assumed equal . In order to simplify the system the sin() will
be linearized to . From the linearized equation the system matrix A, the
input matrix B, the output matrix C and the feed throw matrix D have the
following values:
A =
[0 0
v 0
]B =
[2bu
0
]C =
[0 1
]D =
[0]
To discover how the AmigoBot system functions, the A,B,C,D and the con-
stants b,v have to be entered into an m-file. As explained before this system is
linearized by assume that the robot will not move more than a few degrees away
from the horizontal which makes sin(). Figure(4.4) is the non-linearized
Theta
Y
2X
1State Variable
cos
sin
1s
1s
1s
STARTSPEED
2/WIDTH
STARTSPEED
1u
Figure 4.4: Non-linearized Mathematic Model of The AmigoBot.
mathematic model of the AmigoBot. The next step of building the control
system is to compare the behavior of the open loop of the linear system and
the nonlinear system by giving the same input u=10. Figure(4.5) on Page 63
shows the different behavior of the linearized and non-linearized model of the
AmigoBot. The non-linearized model shows the real action of the robot: the
robot will move along a circle. The linearized model only works like the real
action of the robot when the is very small. When the becomes greater the
system will become disturbed. But notice for both cases the output of the sys-
tem is an unstable value which has to be controlled later. In this application
the linearized model will be used in the first phase in order to easily find the
gain of the controller. But the final simulation model is build by replacing the
linearized model with the non-linearized model.
62
-
4.2 Design of the Control Loop of the AmigoBot
0 10 20 30 40 50 60 70 80 90 100-1000
0
1000
2000
3000
4000
5000
6000
7000
8000
9000
Time
Valu
e
Compare The Linear And Non-Linear Model of The Robot
Non-Linear YNon-Linear ThetaLinear ThetaLinear Y /10
Figure 4.5: Linearized and Non-linearized Model of the Open loop Control ofthe AmigoBot
4.2 Design of the Control Loop of the AmigoBot
In this section a state-space controller will be built for the AmigoBot. At first
the interface of the final simulation model will be given and then how to build
this interface will be explained step by step.
4.2.1 Building the Simulation Model
Controller Robotyr yu
-
Figure 4.6: Standard Control Loop for the Mobile Robot.
The control loop for making the robot follow a track with a certain
distance is shown in Figure(4.6). In control theory the object to be controlled
63
-
4.2 Design of the Control Loop of the AmigoBot
is called the plant, here the plant is the robot. u is the actuating signal, plant
input. yr is the reference or command input. y is plant output and measured
signal. This system is a feedback control system. The most elementary feed-
back control system has three components: a plant, a sensor to measure the
output of the plant and a controller to generate the plants input,see [8]. The
basic problem of control design is to choose a suitable controller to obtain a
control loop with acceptable performance. This in particular means that the
control loop should be stable, which means that the control error must be
reduced to zero, at least after some transients. Now the problem is how to
find the controller gain K which will be suitable for the application:
u = K x = [K1 K2] [y
]where state variable:
x =
[
y
]goes to 0.
To implement this controller, a MATLAB simulation model is built to
find the best controller gain K. One of the first things to do with controller
gain K is to find the poles of the system; these are the values of s where det(sI
- A) = 0, or the eigenvalues of the A matrix. Take Laplace transforms with
zero initial conditions on x:
sx(s) = Ax(s) +Bu(s)
y(s) = Cx(s)
Now eliminate x(s) to get:
y(s) = C(sI A)1Bu(s)
and the transfer function from u to y:
G(s) = C(sI A)1B
This transfer function is strictly proper. From the representation
G(s) =1
det(sI A)Cadj(sI A)B
64
-
4.2 Design of the Control Loop of the AmigoBot
it should be clear that the poles of G(s) are included in the eigenvalues of A,
see [8]. For the robot the eigenvalues of A are:
ev =
[0
0
]As mentioned in the open loop control system this system is unstable.
The stability of a feedback system is related to the location of the roots of
the characteristic equation of the system function. Stability in control theory
means that for any bounded input over any amount of time, the output will
also be bounded. Mathematically, that means for a system to be stable, all the
poles of its transfer function must lie in the left half of the complex plane. Or
more simply put, the real part of every complex number that makes the transfer
function become infinite, has to be negative for the whole system to be stable,
see Routh-Hurwitz Stability Criterion in [7]. As seen the eigenvalues of the A
matrix are not in the left half of the complex plane, this proves that the robot
system is unstable. Figure(4.7) starts to build a controller for the system.
u Y0
yr
simout
To Workspace
x' = Ax+Bu y = Cx+DuState-Space
Ks* u
K
C* u
C
Figure 4.7: Building a Controller for the System.
The schematic of a full-state feedback system is the following: Recall that
the characteristic polynomial for this closed-loop system is the determinant