Professional Documents
Culture Documents
Realtime Control of A Mobile Robot Using Matlab: Kai Wu, B.Eng
Realtime Control of A Mobile Robot Using Matlab: Kai Wu, B.Eng
Matlab
by
Thesis
Master of Engineering
Oct 2004
Realtime Control of a Mobile Robot Using
Matlab
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
Kai Wu
iii
CONTENTS
Contents
Abstract ii
Acknowledgments iii
Chapter 1 Introduction 1
1.1 Problem Overview . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Organization of the Thesis . . . . . . . . . . . . . . . . . . . . 5
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
Bibliography 92
v
CONTENTS
Index 93
Vita 95
vi
LIST OF TABLES
List of Tables
vii
LIST OF FIGURES
List of Figures
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
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
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:
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:
In the following chapters, how to realize this application and solutions to the
problems will be explained in detail.
5
1.2 Organization of the Thesis
chapters detail the design process, the hardware, the software and the resulting
problems.
Chapter 4 opens with the design of the control block. The mathematical
background 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.
6
Chapter 2
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.
7
2.1 The xPC Target
tasks. xPC Target includes a command-line and graphical user interfaces to
complete these tasks.
MATLAB Control and interaction with the xPC Target software en-
vironment and target application using a command-line interface.
8
2.1 The xPC Target
xPC Target Embedded Option deploys stand-alone target applications
and 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
Run-time interface
support files Make model.mk
model.exe
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
10
2.1 The xPC Target
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.
11
2.1 The xPC Target
Figure 2.4: xPC Target Boots, the Kernel and Display Information on the
Target PC
12
2.2 The AmigoBot
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.
Body
28cm
33cm
Sonar Sensor
shows the physical characteristics of the AmigoBot. Here only the information
13
2.2 The AmigoBot
Client Application
Server Informaiotn
Communication Packets
PWM Encoder
Control Counting
Robot Specific Functions
Sonar I/O
Ranging Control
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.
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.
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.
16
2.2 The AmigoBot
continued from previous page
Status byte = 0x3S; where S = Motors status
sfSTATUSSTOPPED (2) Motors moving
sfSTATUSMOVING (3) Motors stopped
Xpos unsigned integer (15 ls-bits) Wheel-encoder integrated
coordinates; platform-
dependent units; multiply
by 0.5083 to convert to
millimeters
Ypos unsigned integer (15 ls-bits)
Th pos signed integer Orientation in platform-
dependent units multiply by
0.001534 for degrees.
L vel signed integer Wheel velocities (respective
Left and Right) in platform-
dependent units;
R vel signed integer multiply by 0.6154 to con-
vert to mm/sec
Battery byte Battery charge times 10
volts
Bumpers integer Motor stall indicators. Bit 0
of the lsbyte is the left wheel
stall indicator = 1 if stalled;
bit 0 of the msbyte is the
right wheel stall.
Control signed integer Setpoint of the servers an-
gular position multiply by
0.001534 for degrees
PTU unsigned integer bit 0 reflects motors engaged
state (1 of engaged) and bit
1 reflects the sonar toggle (1
if on)
continued on next page
17
2.2 The AmigoBot
continued from previous page
Compass byte Always 0
Sonar read- byte Number of new sonar read-
ings ings included in information
packet; readings follow:
Sonar number byte Sonar number
Sonar range unsigned integer Sonar reading in millimeters
(old AmigOS v1.0 multiply
by 0.555)
rest of the sonar readings
Timer unsigned int Currently selected analog
port number 1-5
Analog byte User analog input (0-255=0-
5 VDC) reading on selected
port
Digin byte User digital input; 6 avail-
able on b0-5; actual results
depend on configuration set-
tings
Digout byte User digital output; 6 avail-
able on b0-5; actual results
depend on configuration set-
tings
Checksum integer Checksum (see previous sec-
tion)
18
2.2 The AmigoBot
send a command at least once every two seconds or so;otherwise, the com-
munication watchdog server will stop the robots onboard drives. Table(2.4)
shows the format of the client commands.
Component Bytes Value Description
Header 2 0xFA, 0xFB Packet header; same for client
and server
Byte Count 1 N+2 Number of subsequent command
bytes plus checksum, not includ-
ing Byte Count. Maximum of
200 bytes.
Command 1 0 - 255 Client command number;
Number
Argument 1 0x3B or 0x1B Required data type of com-
Type (com- or 0x2B mand argument: positive integer
mand depen- (sfARGINT), negative integer or
dent) absolute value (sfARGNINT), or
string (sfARGSTR)
Argument n data Command argument; integer or
(command string
dependent)
Checksum 2 computed Packet integrity checksum
19
2.3 Connection between the xPC Target and AmigoBot
2.2.7 Client Command Argument Types
There are three different types of AmigOS client-command arguments: pos-
itive integers two bytes long, negative integers two bytes long, and NULL-
terminated strings consisting of as many as 196 characters. The byte order is
least-significant byte first. Negative integers are transmitted as their absolute
value, unlike information packets, which use sign extension for negative inte-
gers; see below. The argument is an integer, a string, or nothing, depending
on the command.
RS232
Radio Modem
Target PC
AmigoBot With a
Radio Modem
In this project the radio modem link is chosen, because the radio modem is
more flexible than the serial link. As shown in Figure(2.8), the connection
between the Target PC and AmigoBot.
20
2.4 The AmigoBot Communication Block
2.4 The AmigoBot Communication Block
After the above introduction the next step is to build an interface for the
AmigoBot communication block. The Figure(2.9) shows the AmigoBot com-
Check Sum
Start LeftWheel
RightWheel
AmigoBot Communication Y
Block
Right Wheel Speed X
SonarReading
SonarIndex
SonarRange
21
Chapter 3
Implementation of the
AmigoBot Communication
System
The first step to work with the AmigoBot is to establish a connection with the
AmigoBot. There are lots of different communication interfaces which can be
used for the AmigoBot like the Aria software or self built interfaces. In this
project the MATLAB Real-Time Work Shop and xPC Target are chosen as
the development tools and the communication will be realized by a MATLAB
model file. Before exerting any control, a client application must first establish
a connection to the AmigoBot server. Over that established communication
link, the client then sends commands to and receives operating information
from the server. In the following parts, details of this model file are given.
22
3.1 Analyzing the Synchronization and Initialization Process
time the client has to retrieve the server responses.
The synchronization sequence of bytes are (in hexadecimal notation):
23
3.1 Analyzing the Synchronization and Initialization Process
continued from previous page
0024.954 00 00 7F 4A
0025.032 FA FB 21 32 00 00 00 00
0025.032 00 00 00 00 00 00 6D 00
0025.047 00 00 00 02 00 00 02 04
0025.047 8F 06 05 DC 05 01 00 00
0025.063 00 00 1C 08
24
3.1 Analyzing the Synchronization and Initialization Process
Time(s) Sent Data (hex) Received Data (hex)
0006.890 FA FB 03 00 00 00
0006.890 FA FB 03 01 00 01
0006.906 FA FB 03 02 00 02
0006.906 FA FB 03 01 00 01
0006.922 FA FB 06 04 3B 01 00 05 3B
0006.922 FA FB 03 00 00 00
0006.922 FA FB 03 02 00 02
0007.000 FA FB 03 00 00 00 FA FB
0007.000 03 FF 00 FF FA FB 03 FF
0007.015 00 FF FA FB 03 FF 00 FF
0007.031 FA FB 03 FF 00 FF
The above four tables demonstrate four different cases of the synchro-
nization process. In the following section these four cases will be explained.
The Table(3.1) on Page 23 shows a case, in which a successful synchronization
is completed. This table shows that the client sends the three synchronization
packet one by one and with an interval of about 0.3 seconds. The client also
receives three identical synchronization packets just after each packets is sent.
The Table(3.1) shows that as introduced in [1], once connected the AmigBot
will automatically send the SIPs back to the client every 100 milliseconds.
This means the receive block of the client needs to operate at least with the
same rate.
The Table(3.2) on Page 24 and Table(3.3) on Page 24 illustrate some
strange behavior of the AmigoBot. From the specification, if a SYNC0 packet
is sent by the client, the server should respond with an identical echo packet.
The client should then receive a packet like 0xFA 0xFB 0x03 0x00 0x00 0x00
but the Table(3.2) shows that a 0xFA 0xFB 0x00 packet will be received.
The situation of Table(3.2) happens every time when the AmigoBot is switched
on. After several tests it was proven, that this behavior is a result only of
switching on the AmigoBot. The second erroneous behavior pattern is shown
in Table(3.3). Sometimes when the AmigoBot is switched on, at first the client
will receive a strange packet. There are two reasons for this: The first reason is
that the clients RS232 buffer keeps the old data from the last connection. The
second reason is that the AmigoBot still sends the rest of data of the packet
25
3.1 Analyzing the Synchronization and Initialization Process
which belongs to the last connection, because the AmigoBot is suddenly shut
down before the whole packet is sent. This second behavior pattern leads to a
synchronization error. If a SYNC0 packet is sent during the synchronization
phase, an identical echo packet will be received on the client side. If any packet
is sent at the wrong time, the AmigoBot will recognize that a wrong packet
is received and send back a packet 0xFA 0xFB 0x03 0xFF 0x00 0xFF to
the client. The packet 0xFA 0xFB 0x03 0xFF 0x00 0xFF is always used to
notify the client that some errors occurred during the communication. Because
of this behavior the client should send the SYNC0 packet again in order to
build the synchronization as shown in Table(3.3).
Table (3.4) on Page 24 shows an unsuccessful synchronization. The
reason is that the client sends the command with a higher rate than the
AmigoBots maximum accepted rate. The AmigoBot reads and processes the
client commands only once every 10 ms. If the client sends the commands
faster than this, the AmigoBot will send back an error message 0xFA 0xFB
0x03 0xFF 0x00 0xFF. After these four tests, the behavior of the AmigoBot
during the synchronization phase is clear. In the next section the initialization
process will be explained.
26
3.1 Analyzing the Synchronization and Initialization Process
the POLLING command #3 (string argument,only sonar #0 works; 0xFA,
0xFB, 0x0C, 0x03, 0x2B, 0x01, 0x01, 0x01, 0x01, 0x01 0x01, 0x01,0x01, 0x07,
0x2F) to the AmigoBot. In order to activate the new sequence at the begin-
ning, this command has to be sent before the OPEN command is sent.
Initialization Process: Set the sonar polling sequence ,open the server
and enable the motor.
In the previous section the functionality of the xPC Target and Real-Time
Workshop are introduced, and how these functions are used to build the com-
munication will be given.
Figure(3.2) on Page 29 shows the top level of the communication block.
Together with this block there is an initial MATLAB m file(in Appendix B)
which contains some useful factors. These factors will help the block to work
more flexibly. This initial file should be the first file running under MATLAB.
This model file works with a sampling rate of 100 milliseconds which is the
27
3.1 Analyzing the Synchronization and Initialization Process
Send SYNC0
NO
Received=FA FB 03
NO
YES Received=FA FB 00
S
y Receive 3 Bytes Echo SYNC0 YES
n
c
h Do Nothing Send SYNC0
r
o
n Received=FF 00 FF
i
z NO
a YES
t
i
Send SYNC0 Do Nothing
o
n
Send SYNC1
Received=FA FB XX
NO
Send SYNC2
YES
Received=FA FB XX
I
Set Sonar POLLING Sequence NO
n
YES
i
t Receive XX Bytes Echo SYNC1 or SYNC2 Do Nothing
i
a
l
i Send OPEN
z
a
t
i
o
n
Connect & Send ENABLE Motor
28
3.1 Analyzing the Synchronization and Initialization Process
RS232
Check Sum 1
Check Sum
Left 2
1 Start
LeftWheel
Start
Right 3
RightWheel
Th 4
Th
Y 5
Y
RightWheelSpeed X 1
160 6
R X
SonarReading 7
SonarReading
SonarIndexA 8
SonarIndexA
SonarRangeA 9
SonarRangeA
240 LeftWheelSpeed
SonarIndexB 10
L SonarIndexB
SonarRangeB 11
SonarRangeB
AmigoBot Plant
same as the transmission rate of the SIPs. At the top level there is one RS-
232 Mainboard Setup block, three inputs and eleven outputs. The RS-232
Mainboard Setup block is used to initialize the communication of a COM1
port. The three inputs are used to:
1. Start: Start the AmigoBot with 1 and disconnect the AmigoBot with
0.
2. Right: Set a speed to the right wheel of the AmigoBot with a unit of
mm/sec.
3. Left: Set a speed to the left wheel of the AmigoBot with a unit of
mm/sec.
The unit of speed can be changed by changing the speed factor SPEEDFAC-
TOR in the initial file. For example if the SPEEDFACTOR is set to 1, it
29
3.1 Analyzing the Synchronization and Initialization Process
means the input speed multiplied by 40 will be the output speed in mm/sec.
The eleven outputs are the received values from the AmigoBot:
8. SonarIndexA: The even index of the sonar sensor number in the sonar
polling sequence.
10. SonarIndexB: The odd index of the sonar sensor number in the sonar
polling sequence.
As mentioned in Chapter 2 the AmigoBot will send the SIPs every 100 mil-
lisecond or 50 milliseconds depending on the configuration of the AmigOS. If
the configuration of the SIP is different, then the format of the SIP is also
different. In comparing the 100 millisecond configuration with the 50 millisec-
ond configuration, the sonar information can be different. The sonar firing
rate of the AmigoBot is every 50 milliseconds and the 8 sonar sensors will fire
one after the other. This means every 50 milliseconds only one sonar sensor
is working and all the others are sleeping.If the configuration is set to send
the SIPs every 100 milliseconds, then there will be two sonar sensors putting
their measured values in the SIP. By default the 8 sonar sensor values will be
sent in 4 continuous SIPs while the sonar sensors are firing in a continuous
sequence. If the SIPs are sent every 50 milliseconds then there will be only
30
3.1 Analyzing the Synchronization and Initialization Process
one sonar measured value in the SIP. The 8 sonar sensor values will be sent
in 8 continuous SIPs. The AmigoBot Connecter shown above works only for
the situation in which the SIPs are sent every 100 milliseconds. This is the
default configuration of the robot and it can be changed to 50 milliseconds.
The default polling sequence of the sonar sensors is continuous and this can
be changed by using the POLLING command. The POLLING command
is used to set a new sonar sensor firing sequence(e.g only one sonar sensor
fires every 50 milliseconds). In this application only the sonar sensor #5 is of
interest, so in the initial file the factor SEQUENCE1 to SEQUENCE8 are
all set to 6 and the factor SonarIndex100B is set to 5. This means when
the application starts, the sonar sensor #5 will fire every 50 milliseconds and
the output SonarRangeB will output the received value of sonar sensor #5.
This is the solution to the problem mentioned in the introduction, now the
feed back information comes every 100 milliseconds not in every 400 millisec-
onds as before. Figure(3.3) shows the second level of the whole model. The
1 Start
Start
Right 3
Right
Check Sum auto 1
(double) Th 4
Check Sum
Th
Y 5
Start
Y
X 6
X
Data Received data
SonarReading 7
SonarReading
Enable SonarRangeA 9
SonarRangeA
2 Right
SonarIndexB 10
RightWheelSpeed
SonarIndexB
3 Left
LeftWheelSpeed
SonarRangeB 11
Set Speed
SonarRangeB
Decode SIP
31
3.1 Analyzing the Synchronization and Initialization Process
second level is divided into three parts:
Receive SIP & Decode SIP: In the subsystem Receive SIP & Decode
SIP the functions to receive the SIP and decode the information from
the SIP are implemented.
Set Speed: In the subsystem Set Speed the commands of setting the
velocity of the robot will be sent to the AmigoBot if the correct infor-
mation from the AmigoBot is received.
32
3.1 Analyzing the Synchronization and Initialization Process
case [ 1 ]:
case: { }
Clear the Receive Buffer
case [ 2 ]:
case: { }
Send SYNC0
case [ 3 ]:
case: { }
Receive SYNC0
case [ 4 ]:
case: { }
Send SYNC1
case [ 5 ]:
case: { }
T Receive SYNC1
1 u1 case [ 6 ]:
z-1
Start
case: { }
State Counter
Send SYNC2
case [ 7 ]:
case: { }
Receive SYNC2
NOT auto 2 case [ 8 ]:
(double)
case: { }
Polling Sonar
case [ 9 ]:
case: { }
Send OPEN
case [ 10 ]:
case: { }
Enable Motor
case [ 13 ]:
case: { }
State Machine
Disconnect
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
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
case: { }
Action Port
RS232 Send
[250 251 3 0 0 0]
COM1
SYNC0
RS232
Binary Send
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
case: { }
Length
3
==
==
0
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
case: { }
FA
Length Done
250
== RS232 Receive
COM1
3 Length Done AND auto Enable Data
FB (double)
RS232 Receive
COM1 251
==
1 Enable Data Unpack
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.
T auto
1 u1 case [ 10 ]:
z-1 (double)
Start
Data 2
Data
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-
case: { }
Action Port
Data 2
AmigoBot SIP RS232 Receive 8 Data
COM1
CheckSum 1
CheckSum
AmigoBot SIP Receive Block
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:
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;
}
39
3.2 Receive and Decode the SIPs
if(sum==(buf[*current-3]<<8 | buf[*current-2]))
*output_checksum=1;
else
*output_checksum=0;
//the *current is used to control the output
//packet
*current=0;
//Output the data when the packet is correct
if(*output_checksum==1)
memcpy(ssGetOutputPortSignal(S, 0), buf, width);
*findFAFB=0;
}
//put the byte to the output buffer
buf[(*current)] = tmp & 0xff;
*current=*current+1;
bufCount--;
}
......
Finally the subsystem will give two outputs: the check sum and a whole SIP
packet which shifts to the left by one byte. The SIP packet is then sent to a
subsystem which decodes the whole packet into several useful values.
40
3.2 Receive and Decode the SIPs
1 Received data
4a5 X 5
X
get X
6a7 Y 4
Y
get Y
Target Scope
Id: 1
8a9 Th 3
Th
get Th Scope (xPC) 1
10a11 Left 1
Left
get Left Wheel Speed
12a13 Right 2
Right
get RightWheel Speed
SonarReading 6
SonarReading
SonarIndexA 7
SonarIndexA
22a28 SonarRangeA 8
SonarRangeA
SonartIndexB 9
SonarIndexB
Target Scope
Id: 2
SonarRangeB 10
SonarRangeB Scope (xPC) 2
get Sonar Readings
10a11
auto
1 Data Amigo Converter Value (double) WHEELSPEEDFACTOR 1
Left
Data converter
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] << 8)); break;
// Signed Integer convert 2 bytes to Signed Integer
case 2 :
tmp=(int)((signed short int)(u[start]+((int)u[start+1] << 8)));
break;
// Integer
case 3 : tmp=(int)((short int)u[start]+
((short int)u[start+1] << 8)); break;
default: break;
}
*y=tmp;
...
For example the parameter for the left wheel speed is Start=9 and Mode=2.
This means the left wheel speed is composed by two bytes and is a signed in-
42
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-
22a28
1 Data Amigo Converter Value auto 1
(double)
SonarReading
u1 if(..)
if { }
Data Amigo Converter Value auto In1 Out1 3
(double)
SonarRangeA
u1 if(..)
if { }
Data Amigo Converter Value auto In1 Out1 5
(double)
SonarRangeB
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.
43
3.3 Setting the Wheel Speed
auto
2 SPEEDFACTOR round (int8) Right
Right
AmigoBot
3 SPEEDFACTOR round auto Left Speed Command
(int8) RS232 Send
Left COM1
1 Enable
Enable
Give Speed
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
Example1:
Left Wheel Speed: 1*40mm/sec=40mm/sec
Right Wheel Speed 1*40mm/sec=40mm/sec
+
+
FA FB 06 20 3B 01 01 21 3C
Header1 Header2 Command Argument Right Wheel Left Wheel
Length Check Sum
VEL2 Postive Speed Speed
Integer
Example2:
Left Wheel Speed: 1*40mm/sec=40mm/sec
Right Wheel Speed -1*40mm/sec=-40mm/sec
FA FB 06 20 3B FF 01 1F 3C
Header1 Header2 Command Argument Right Wheel Left Wheel
Length Check Sum
VEL2 Negative Speed Speed
Integer
Example3:
Left Wheel Speed: -3*40mm/sec=-120mm/sec
Right Wheel Speed -3*40mm/sec=-120mm/sec
FA FB 06 20 1B 03 02 23 1D
Header1 Header2 Command Argument Right Wheel Left Wheel
Length Check Sum
VEL2 Negative Speed Speed
Integer
Example4:
Left Wheel Speed: -1*40mm/sec=-40mm/sec
Right Wheel Speed 1*40mm/sec=40mm/sec
FA FB 06 20 1B FF 00 1F 1B
Header1 Header2 Command Argument Right Wheel Left Wheel
Length Check Sum
VEL2 Negative Speed Speed
Integer
Example5:
Left Wheel Speed: -1*40mm/sec=-40mm/sec
Right Wheel Speed 0*40mm/sec=0mm/sec
FA FB 06 20 1B 00 01 20 1C
Header1 Header2 Command Argument Right Wheel Left Wheel
Length Check Sum
VEL2 Negative Speed Speed
Integer
45
3.4 Testing the Communication Block and Analyzing the Server
Information
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.
46
3.4 Testing the Communication Block and Analyzing the Server
Information
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
The first correct SIP has been
receiced then send the first
client command
0 1 2 3 4 5 6 7 8 9 10 11 12
Time Line (seconds)
Clear the Received Buffer
Send SYNC0 The first client command is
Synchronization
POLLING Sonar
Send OPEN
ENABLE Motor
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 Server
Information
Timing Analysis
12
10
Checksum
State Counter1
8 State Counter2
Speed/120
Value
0
0 5 10 15 20 25 30 35 40
Time
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.
X Multiplied With Factor and Original X
6000
X Multiplied With A Factor 0.5083
Original X
5000
4000
Value
3000
2000
1000
0
0 5 10 15 20 25 30 35
Time
The red curve is the original data received from the SIP packet. The blue
48
3.4 Testing the Communication Block and Analyzing the Server
Information
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 Server
Information
Left And Right Wheel Speed
300
Left Wheel Speed 2
Right Wheel Speed 6
250
200
Speed
150
100
50
0
0 5 10 15 20 25 30 35 40 45
Time
X,Y Theta
500
Theta
Y
X
400
300
200
Value
100
-100
-200
-300
0 5 10 15 20 25 30 35 40 45
Time
AmigoBot Moving Track
250
200
150
100
50
Y Position
-50
-100
-150
-200
-250
0 50 100 150 200 250 300 350 400 450 500
X Position
50
3.4 Testing the Communication Block and Analyzing the Server
Information
Left And Right Wheel Speed X Multiplied With Factor and Original X
140 6000
Left Wheel Speed X Multiplied With A Factor 0.5083
Right Wheel Speed Original X
120
5000
100
4000
80
Speed
Value
3000
60
C (30 2154)
2000
40
1000
20 A (20 996.3) B (30 996.3)
0 0
0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 35
Time Time
140
120
Speed
100
80
60
40
20
0
0 20 40 60 80 100 120 140
Time
51
3.4 Testing the Communication Block and Analyzing the Server
Information
X
15000
X
F (120 11608)
10000
Value
E (100 7722)
5000
D (80 4653)
C (60 2301)
B (40 767)
A(20 0)
0
0 20 40 60 80 100 120 140
Time
52
3.4 Testing the Communication Block and Analyzing the Server
Information
Left And Right Wheel Speed
4000
Left Wheel Speed
Right Wheel Speed
Ramp
3500 Set Speed
3000
2500
Speed
2000
1500
1000
500
0
0 20 40 60 80 100 120
Time
2000
50
1500
Speed
Value
1000
-50
500 Left Wheel Speed
Right Wheel Speed
-100
0
-500 -150
0 10 20 30 40 50 60 70 80 0 10 20 30 40 50 60 70 80
Time Time
Figure 3.24: Set the Left and Right Wheel Speed to Positive and Negative
Values
53
3.4 Testing the Communication Block and Analyzing the Server
Information
-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
Analysis The Sonar Working Range
7000
6000
5000
Distance To Wall (mm)
4000
3000
2000
1000
0
0 10 20 30 40 50 60 70 80
Time(s)
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 Server
Information
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
40
30
20
10
0
-10
300mm
-20
-30
-40
WALL
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 Server
Information
Analysis The Sonar Value With The Angle To Wall
2500
2029 2043
2000 30 40
1829 1811
-30 -40
Distance To Wall (mm)
1500
1222
20
1000
906.7718
-20
500
326.9944
299.1651 303.8033
0 -10 10
0
0 50 100 150 200 250 300 350
Time(s)
56
3.4 Testing the Communication Block and Analyzing the Server
Information
5000
4000
Value
3000
2000
1000
-1000
0 20 40 60 80 100 120 140
Time
Sonar 5 And Theta
7000
Sonar
Theta
6000
5000
4000
Value
3000
2000
1000
-1000
0 20 40 60 80 100 120 140 160
Time
Figure 3.28: Comparison of the Sonar Sensor Reflection with Two Materials.
57
Chapter 4
A Real-Time Control
Application for the AmigoBot
58
4.1 Mobile Robot Control
Center Of Turn
Reference
Direction r1 r2
L
v1
b
v2
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
= u (4.5)
b
where b is the wheel base.
x = v cos
y = v sin (4.6)
59
4.1 Mobile Robot Control
Y
.
X
V
.
Y
More details about the state space system will be given in the next section.
Here the state vector x = (x1, x2) = (, y) is chosen.
2
x1 = u (4.7)
b
x2 = v sin x1 (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]).
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)
+ +
u B C y
+ +
A
D
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:
0 0
A=
v 0
2
u
B= b
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
1 Theta
1 2/WIDTH
s
u
1
1 Y State Variable
sin STARTSPEED
s
1
cos STARTSPEED 2
s
X
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
Compare The Linear And Non-Linear Model of The Robot
9000
Non-Linear Y
Non-Linear Theta
8000 Linear Theta
Linear Y /10
7000
6000
5000
Value
4000
3000
2000
1000
-1000
0 10 20 30 40 50 60 70 80 90 100
Time
Figure 4.5: Linearized and Non-linearized Model of the Open loop Control of
the AmigoBot
u
yr - Controller Robot y
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
s
x(s) = A
x(s) + B u(s)
y(s) = C x(s)
G(s) = C(sI A)1 B
1
G(s) = Cadj(sI A)B
det(sI A)
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:
0
ev =
0
u x' = Ax+Bu Y
0 C* u simout
y = Cx+Du
yr C To Workspace
State-Space
Ks* u
65
4.2 Design of the Control Loop of the AmigoBot
All the explanation above shows the control system for the AmigoBot
is a very simple state-space control. To test the linear simulation result the
initial value of the state variable is set to (0.01,10). Figure(4.8) is the result
4
Value
-2
-4
0 1 2 3 4 5 6 7 8 9 10
Time
of the linear simulation, it shows that with the help of the controller gain
both state variables return to zero. This figure also shows the response time is
fast enough and the control error is small enough. Normally for a state-space
control the overshoot and response speed are depending on the chosen values
of the poles. If a smaller overshoot is required, placing the poles further to the
left can result in the transient response improving (this should also make the
response faster). In this thesis the poles are always real values, so there will
be no influence with the overshoot of the system. Only the response speed
of the system can be changed by choosing different poles. Normally a control
system is required to have a reference input, for example a step function. This
is realized by adding a reference gain Nv in front of the input u. The reference
gain is calculated as following, see [10]:
Nv = -1.0/(C*inv(A-B*Ks)*B);
66
4.2 Design of the Control Loop of the AmigoBot
u x' = Ax+Bu Y
Nv C* u simout
y = Cx+Du
Step Gain State-Space C To Workspace
K
K*u
Figure(4.9) shows the linear model with a reference input. Figure(4.10) shows
the simulation results of sending a step function to the system. This figure
shows the final output will not be zero any more but like the reference input
is a step function.
Linear Simulation Results with Reference Input
0
Y
-20 Reference Input
-40
-60
-80
Value
-100
-120
-140
-160
-180
-200
0 10 20 30 40 50 60
Time
Figure 4.10: Simulation Result of the Linear Model with a Step Reference
Input
At this point everything looks fine and it seems the control system is
finished, but there is one other problem , in the case where not all the states
of x can be measured. Actually for most systems not all the states of x can be
measured and they have to be estimated by an observer. For the AmigoBots
controller the same problem occurs. In the AmigoBot control application, the
67
4.2 Design of the Control Loop of the AmigoBot
only value which can be used is the sonar sensor value and this value can
be translated into the state x2 . Figure(4.11) shows that from the detected
Y
7,5 cm
X
7,5 cm
-10,5 cm
-10,5 cm
X
S5'
Y=S5'+10,5
S5
Y=S5+10,5
WAll
Figure 4.11: Derivation of Y Values From The Sonar 5
sonar sensor values during the movement of the robot, the Y position of the
robot can be calculated(e.g. Y=Y-Y ). For the state x1 , how can the be
retrieved? The solution is to use an observer to estimate the state x1 . The
observer is basically a copy of the plant; it has the same input and almost
the same differential equation, see [9],[10]. An extra term compares the actual
measured output y to the estimated output yb; this will cause the estimated
states x
b to approach the values of the actual states x. The error dynamics of
the observer are given by the poles of (A-L*C). To construct an observer, the
observer gain L has to be chosen by the place() function. If the observer needs
to be faster than the system itself, the poles have to be set farther to the left
than the dominant poles of the system. In this application an observer which
is twice as fast as the system has been used in the first instance.
op = 2*clp;
L = place(A,C,op);
L = L;
68
4.2 Design of the Control Loop of the AmigoBot
.
in u + x x +
Nv B C y
+ +
A
D
Ks
+
B C
^
+ ^ -
x y
Observer A
~
y
L
2 Y
u State Variable C* u
Nv u
X C Scope
Step Gain Amigo Simulator
Controller Ks
K*u
2
Observer
x^ u_in
2
~y
y_in
nonlinear model includes a reference input and an observer. The linear model
of the AmigoBot is also replaced by the nonlinear model of the AmigoBot.
Figure(4.14) on Page 70 shows the simulation result of the system with a
reference step input. This figure proves that the observer and controller gain
K also work with the nonlinear model of the mobile robot.
69
4.2 Design of the Control Loop of the AmigoBot
Final Simulation with a Step Input
5
-5
-10
Value
-15
-20
Y /10
Reference Step Function /10
Theta
-25
Estimate Y /10
Estimate Theata
Error Y
u/10
-30
-35
0 5 10 15
Time
Enable
Enable
ReferenceInput u u
Reference Input
Ramp Saturation y 1
y_in
y
Observer & Controller AmigoBot Plant
Figure 4.15: The Top Level of the AmigoBots Final Control System
the AmigoBots control system in real time. Compare this structure with the
70
4.2 Design of the Control Loop of the AmigoBot
standard control loop for a mobile robot shown in Figure(4.3) there are several
differences:
The y position is input to the system separately with the reference value.
2 Nv 1
ReferenceInput u
1 Enable
Enable
3 y_in x^
y_in
u_in
Controller
Ks K*u Observer
2
y_in
A
A* u
1
3 B* u s C* u
u_in
B Integrator C1 Product
1
L
x^
L* u
71
4.2 Design of the Control Loop of the AmigoBot
72
4.3 Experimental Verification of the Robot Controller
of this block will first check if the sonar index is the one required for
the observer and then check if the measured value is a reasonable
value. If the measured value is the correct value, then it is sent to
the observer. If this is not true an Enable switch will switch off
the observer gain L and let the observer use only the old estimated
state variables until the switch is switched on again. Figure(4.19)
1
SonarIndex ==
AND auto
SONARINDEX100B (double) 1
Enable
SONARLIMITATION
Limitation >=
2
SonarRange
2
y
DISTANCE
Initial Sonar Value
73
4.3 Experimental Verification of the Robot Controller
Pi*r=2000mm
R=636.6mm
l=2000mm
r=636.6mm
500mm
300mm
move almost like the step function. In the above experiment, the robot is
first put into the starting position. The distance between the starting point
and the wall is 500mm. The robots sonar sensor 5 should be located just
above the start point. The synchronization and initialization process of the
robot need to use 10 seconds. During the synchronization and initialization
process, the green system and amber user LEDs on the AmigoBot will indicate
the activities of the AmigOS. How these LEDs indicate the activities of the
AmigOS can be seen in [1]. Once the synchronization and the initialization
process is finished the AmigoBot will generate a sound Connected which
indicates to the user that the process is completed. In the next 10 seconds the
AmigoBot will try to move alone the track at a distance of 500mm. During
the time 20 to 30 seconds the AmigoBot will move near to the wall until at a
distance of 300mm. In this time period the robot should move according to a
ramp function like the ramp part of the given step function. After 30 seconds
the robot should always try to keep a distance of 300mm to the track. If the
track is a straight one then the robot should move forward in a straight line.
If the track is a circle then the robot should also move along the circular track.
The behavior of the robot is shown in Figure(4.21) on Page 75. Exam-
ining the Figure(4.21), the control loop takes its effect on the robot and lets
the robot move in the proposed way. After the control loop starts, the robot
is not located exactly on the start point but a little bit nearer. The controller
immediately finds out the distance is less than 500mm so it will give a positive
u to the robot which means decrease the left wheel speed and increase the
right wheel speed. This control command will let the robot turn right and
74
4.3 Experimental Verification of the Robot Controller
Sonar 5 And Theta And U
600
Sonar
Theta
U
500 Step
400
300
Value
200
100
-100
-200
0 20 40 60 80 100 120
Time
Figure 4.21: Experiment A: The Received Sonar,Theta,U and the Given Input
Information.
make the distance larger. It is very difficult to make the robot move exactly to
the expected position and keep the distance of 500mm to the track. Normally
the controller will keep sending the old command which will lead the distance
to be larger than 500mm. After a short while the controller discovers that the
distance is larger than the expected one and will let the robot move nearer to
the track. This situation will go on and the moving track of the robot will
oscillate around the 500mm distance line. This behavior is very normal for the
control system, for a control system there is always some basic requirement
like the control error and response time. In this application the required con-
trol error should be less than 40mm and the response time should be as fast as
possible. Here, except the controller itself, the robot also has some influence
on the final control result. Except the already explained sonar sensor problem
there are still some problems coming from the robot which will influence the
control. Here two more problems will be explained.
The quantization problem of the wheel speed. The wheel speed of the
AmigoBot can only be set with a unit of 40mm/sec. This means if the
75
4.3 Experimental Verification of the Robot Controller
control input u is a small value, it will not have any effect on the system.
For example the robot is just 23mm away from the desired distance,
and the input u may have a value of 510. The average speed of the
wheels is 200mm/sec. Now the new calculated left wheel speed will be
195mm/sec. This value is an arbitrary value of the controller, the actual
speed sent to the AmigoBot will be quantized to 200mm/sec which means
nothing changes the system at this moment. This means an immediate
reaction from the robot only occurs when the absolute value of the input
u is greater than 20. If this situation happens the robot will always keep
this distance of 302 303mm to the track. This problem can be solved
by using a larger pole or faster observer but if the large pole or faster
observer is used then the system will become more unstable. Whether
using a large pole to solve the quantization problem or using a small
pole to keep the system more stable depends on different applications.
In this application a more stable system is more important.
The operating system problem of the AmigoBot. The AmigOS can only
send the SIP every 50 or 100 milliseconds and accept the client com-
mand every 20 milliseconds. In this application the required sampling
rate of the control model is 100 milliseconds so the SIP will be received
only every 100 milliseconds and the client command will also be sent
every 100 milliseconds. When working with this sampling rate the con-
trollability of the control system will be limited. The reason is that the
feedback information will come in every 100 milliseconds and the con-
troller will send the control information every 100 milliseconds but in
this 100 milliseconds the position of the robot is already changed. For
example assume the average speed of the robot is 200mm/sec and the
input u is exactly 40, which means in the next 100 milliseconds the left
and right wheel speed of the robot is 160mm/sec and 240 mm/sec. This
controller input will lead the robot to move along the x position about
20mm, the y position about 2mm and the theta turns left about 1.91
degree. These values are calculated from the mathematical model of the
system. They are smaller than the actual value because there is an ac-
celeration time (The maximum allowable translational (de)acceleration
is 4000 mm/sec). In 100 milliseconds the x position of the robot changes
about 20mm if the track is not a straight one but a circle, and the actual
76
4.3 Experimental Verification of the Robot Controller
distance to the track will change a lot. This will mean the next coming
sonar sensor value may be less or more then the expected value.
All these problems will influence the control of the system. As shown in the
experiment, if the robot following the straight track and the distance is just
23mm less or more than 300mm the robot will keep the distance. Only the
difference between the desired value and the actual value is large enough such
that the controller will take effect. In the above experiment during the time
36 to 58 seconds the robot is following a half circle track. The figure shows
in this period the measured distance is always smaller than 300mm. This is
caused by the slow sampling rate of the model. For example once the robot
finds the distance is 275mm then the controller will allow the robot to move a
further 2mm from the track, but at the same time the robot will move forward
about 20mm which makes the distance 5mm smaller. In the next sample time
the distance becomes 273mm, the controller will allow the robot to move a
further 4mm from the track but the final distance will be 272mm. This means
the input u is not strong enough and the robot will move nearer to the track.
During this period the input u becomes stronger as the distance is shorter.
Now there are two things influencing the robots distance to the track, the
input u will let the robot move further to the track and the forward speed
of the robot lets the robot move nearer to the track. Once the effort from
the input u is larger then the forward speed, the robot will move finally far
away from the track. This is the explanation of why during the time 36 to 58
seconds the distance is lese then 300mm and always oscillating.
Figure(4.22) on Page 78 shows the result of the Experiment B. The
difference between the experiment A and B is the setup of the control vari-
ables. In Experiment A the poles are set to [-1.6 -1.601] and the observer is
twice as fast as the closed loop poles. In Experiment B the poles are set to
[-1.75,-1.751] and the observer is twice as fast as the closed loop poles. Com-
pare the Figure(4.21) and Figure(4.22) the differences are that the system of
Experiment B has a faster reaction then that of Experiment A and during the
circle track part the average distance of Experiment B is nearer 300mm than
Experiment A. From the controlling view of the system, Experiment B has
more oscillations which means not so easy to control.
Figure(4.23) on Page 79 shows the result of Experiment C. In Exper-
iment C the poles are set to [-1.75,-1.751] and the observer is 4 times faster
than the closed loop poles. The figure shows during the half circle track part
77
4.3 Experimental Verification of the Robot Controller
Sonar 5 And Theta And U
600
Sonar
Theta
500 U
Step
400
300
Value
200
100
-100
-200
0 20 40 60 80 100 120
Time
Figure 4.22: Experiment B: The Received Sonar,Theta,U and the Given Input
Information.
the distance can be controlled at nearly 300mm but the system is always os-
cillating. From these experiments the following conclusion can be generated:
Placing the poles further left to the complex plane or using a faster observer
will lead to a faster response but will cause difficulties of the control of the
system. For every control system there may be different requirements so how
to choose the best setup of the controller to serve for the applications will be
done by simulations and the test of real time behavior.
At this point, nearly all the tasks of this thesis are finished: The com-
munication model for the AmigoBot based on the xPC Target is proved to be
very stable. The real-time control model to let the AmigoBot follow a track is
also working now. In the next chapter some conclusions and suggestions will
be given.
78
4.3 Experimental Verification of the Robot Controller
400
200
0
Value
-200
Sonar
-400 Theta
U
Step
-600
-800
0 20 40 60 80 100 120
Time
Figure 4.23: Experiment C: The Received Sonar,Theta,U and the Given Input
Information.
79
Chapter 5
5.1 Conclusions
This thesis has investigated the possibility of building a real-time control model
for a mobile robot using MATLAB.
In Chapter 2, the use of the xPC Target from MATLAB and the
AmigoBot from ActivMedia to support this real-time control application have
been introduced in the beginning of the chapter. The main topic of this chap-
ter is the design of a communication model for the AmigoBot which works
on the xPC Target. The design process starts with analyzing the communi-
cation protocol of the AmigoBot and how to connect the xPC Target to the
AmigoBot. Finally an interface of the AmigoBots communication block was
created. This interface will include the inputs and outputs of the commu-
nication system. These inputs and outputs were used in the control model
later.
In Chapter 3, the task of implementing the AmigoBots communication
block was realized. The communication block of the AmigoBot was divided
into three parts:
The first part was used to finish the synchronization and initialization
task of the communication process. This process works like a state ma-
chine because the AmigoBots synchronization process is built by send-
ing and receiving three synchronization packets. In this part some tests
80
5.1 Conclusions
were done to research the behavior of the AmigoBot and to find the prob-
lems during the synchronization and initialization process. A flowchart
was created to describe the property of the state machine. The final
Synchronization And Initialization model was built by following the
flowchart.
The second part was used to decode the Server Information Packet re-
ceived from the AmigoBot. In this part some S-Functions were created
to help decode several useful information sets from the whole received
SIP. As the received information may be not the real physical value of the
system some conversions were done in order to output the real physical
values to the user.
The third part was used to set the speeds of both wheels of the AmigoBot.
The main functionality of this part was to judge whether the input speed
was negative or positive and then to use the corresponding command to
generate the client command which was sent to the AmigoBot.
At the end of this chapter some experiment were done in order to check if this
communication block worked. By analyzing these experiment the features of
the AmigoBot were also discovered.
In Chapter 4, a real-time control application was designed and imple-
mented. This application allow the AmigoBot to move following a wall with
a certain distance. This distance can be a constant or a mathematic func-
tion(like a step function). The wall can be a straight or curve one. In the
beginning, a mathematic model of the robot was created and the state-space
control system was used to control the AmigoBot. A simulation model was
used at first to test the action of the system, some desired control variables
were discovered by using this simulation model. When using these control vari-
ables on the AmigoBot some problems occurred and some of these variables
had to be corrected. Some experiments were done to indicate the influence of
different control variables and how these variables were changed to reach the
requirement of the system.
During the whole work of this thesis new problems constantly arose
and solutions for these problems were also discovered. Here the problems and
solutions for this application will not be mentioned again. In the next section
some suggestions for the future work will be given.
81
5.2 Suggestions for the Future Work
5.2 Suggestions for the Future Work
In Chapter 4 some experimental results showed that the control part of the
system still can be improved. Here, some suggestions about how to improve
the controller of the system will be given. As explained in the previous section
the AmigoBot can send its SIP every 50 milliseconds or 100 milliseconds. The
communication block of this application is only designed for receiving SIPs
every 100 milliseconds. A model which works with 50 milliseconds will improve
the controllability of the system, the reason is that the sonar information can
be detected twice as fast as the 100 milliseconds system. This is one way
to improve the controllability of the system. Another way to improve the
controllability of the system from a hardware point of view is to try to find a
better material than the one used now. This can improve the quality of the
sonar sensor value. From the view of control theory, the robot needs to use
faster poles during the circle part of the track and slower poles if the track is
straight. An improvement from this view is to use two controllers: one for the
circle part and the other for the straight part. A detector has to be created to
find out at which point to use the faster controller and when to use the slower
one.
Another suggestion from the application side, is that a robot following
application can be created, which means one robot follows another robot. The
interesting point of this application is that two sonar sensors can be used
together and the robot can move more flexibly than following a track which
needs much more places.
82
Appendix A
83
continued from previous page
HEAD 12 int Turn to absolute heading; 0-359 degrees
DHEAD 13 int Turn relative to current heading; de-
grees
SAY 15 int,string Sound duration (20 ms increments)/tone
(half-cycle) pairs; int is string length
CONFIG 18 int Request configuration SIP
ENCODER 19 int Request continuous (0) or stop sending
(=0) encoder SIPs
RVEL 21 signed Rotate at degrees/sec
int
DCHEAD 22 int Colbert relative heading setpoint; de-
grees
SETRA 23 int Sets rotational ()de/ acceleration in
mm/sec2
SONAR 28 int Enable (1) or disable (0) the sonars
STOP 29 none Stops robot (motors remain enabled)
DIGOUT 30 int Msbits is a byte mask that selects output
port(s) for changes; lsbits set (1) or reset
(0) the selected port.
VEL2 32 int Independent wheel velocities; lsb=right
wheel; msb=left wheel; AmigOS in
2cm/sec increments
GRIPPER 33 int Pioneer Gripper server command. See
the Pioneer Gripper manuals for details.
ADSEL 35 int Select the A/D port number for analog
value in SIP. Selected port reported in
SIP Timer value.
GRIPPERVAL 36 int P2 Gripper.
IOREQUEST 40 int Request an IOpac. Set argument=1 for
a single packet; 1 for a packet each info-
Cycle; 0 stop continuous packets
continued on next page
84
continued from previous page
PTUPOS 41 int Msb is the port number (1-4) and lsb is
the pulse width in 100 sec units PSOS
or 10 sec units P2OS
TTY2 42 string Send string argument to serial device con-
nected to AUX port on micro controller
GETAUX 43 int Request to retrieve 1-200 bytes from the
aux serial channel; 0 flushes the aux serial
input buffer.
BUMPSTALL 44 int Stop and register a stall if front (1), rear
(2) or either (3) bump-ring contacted.
Off (default) is 0.
TCM2 45 int TCM2 Module commands; see P2 TCM2
Manual for details.
E STOP 55 none Emergency stop, overrides deceleration
STEP 64 none Single-step mode (simulator only)
TTY3 66 String Send string argument out to serial device
connected to AUX2 serial port
GETAUX2 67 int Request to retrieve 1-200 bytes from the
AUX2 serial port; 0 flushes the buffer.
ROTKP 82 int Change working rotation Proportional
PID value (not FLASH default)
ROTKV 83 int Change working rotation Derivative PID
value (not FLASH default)
TRANSKP 85 int Change working translation Proportional
PID value (not FLASH default)
TRANSKV 86 int Change working translation Derivative
PID value (not FLASH default)
TRANSKI 87 int Change working translation Integral PID
value (not FLASH default)
REVCOUNT 88 int Change working differential encoder
count (not FLASH default)
continued on next page
85
continued from previous page
SOUND 90 int Play stored sound
PLAYLIST 91 int Request playlist packet for sound number
or 0 for all user sounds
SOUNDTOG 92 int Mute (0) or enable (1) sounds
86
Appendix B
Initial File
An example of the initial file, changing the value of these factors will let the
model works more flexible.
% Declare Constants To initialize the model
% Used for the application 50ms, Change this to get the required
% sonar range e.g SONARINDEX50=5 means the model will output
% the value of sonar 5 at output port SonarRange
% only value 0,1,2,3,4,5,6,7 can be chosen
SONARINDEX50=5;
% Used for the application 100ms
% e.g SONARINDEX100A=4 means the model will output the value of
% sonar 4 at output port SonarRangeA
% only value 0,2,4,6 can be choosen
SONARINDEX100A=4;
% Used for the application 100ms
% e.g SONARINDEX100B=5 means the model will output the value of
% sonar 5 at output port SonarRangeB
% only value 1,3,5,7 can be chosen
SONARINDEX100B=5;
87
% SIP factors, these factor is used to convert the received SIP
% information to the real physic values
XPOSITIONFACTOR=0.5083;
YPOSITIONFACTOR=0.5083;
THETAFACTOR=0.001534;
WHEELSPEEDFACTOR=0.6154;
88
Appendix C
% control loop
% getting started
% Declare Constants
WIDTH = 240;
STARTSPEED= 120;
DISTANCE=500;
NEWDISTANCE=300;
STEPTIME=20;
SONARLIMITATION=1000;
WALLINRIGHT = 1;
%
% Part I. Setup base data for the linear model and
% show that the open loop plant is unstable.
% create state space matrices for plant (output state 2 value)
A = [ 0 0;
STARTSPEED 0 ];
B = [2/WIDTH;0];
C = [0 1];
D = [0];
disp(State Space Matrices for the Plant)
89
A, B, C, D
%
% compute eigenvalues of state matrix for open loop plant
disp(Eigenvalues of the "Open Loop Plant"); ev = eig(A)
%
%
% Part II. Add state feedback control to stabilize
% the system and simulate system behavior for a step
% change in the state 2 reference point.
% check for full state controllability
disp(Controllability Matrix for this system), M = ctrb(A,B)
disp(Rank of Controllability Matrix), rank(M)
%
% calculate state feedback gains for specified closed loop poles
clp = [-1.75 -1.751];
Ks = place(A,B,clp);
disp(Desired closed loop poles for state feedback controller);
clp
disp(State feedback gains needed to give desired poles);
Ks
disp(Calculated eigenvalues of system with state feedback);
eig(A-B*Ks)
%
% calculate Nv for zero SS error (see derivation in
% notes - next subsection)
Nv = -1.0/(C*inv(A-B*Ks)*B);
%N=(C*((B*K-A)^(-1))*B)^(-1);
disp(Setpoint gain for zero SS error); Nv
%
%
% Part III. Add state feedback control and a full observer
% to stabilize the system. Simulate system behavior for a
% step change in the state 1 reference point. This should
% give the same simulation as Part II.
% check for full state observability
90
disp(Observability Matrix for this system),
H = obsv(A,C)
disp(Rank of Observability Matrix),
rank(H)
%
% calculate estimator gains for specified observer poles
% estimator dynamics is 0.5 times faster than closed loop poles
op = 2*clp;
L = place(A,C,op); L = L;
disp(Desired observer poles for state feedback controller); op
disp(Estimator gains needed to give desired poles); L
disp(Calculated eigenvalues of estimator system); eig(A-L*C)
91
BIBLIOGRAPHY
Bibliography
[3] The MathWorks: xPC Target For Use with Real-Time Workshop Users
Guide(Version 2)
92
BIBLIOGRAPHY
[12] Li Fang: Camera based track control for a mobile robot, Master Thesis
University of Applied Sciences Hamburg, (2004)
93
INDEX
Index
ActivMedia, 1 SIP
AmigoBot, 2 Server Information Packets, 15, 16, 21,
AmigOS Client Command, 18, 44, 83 25, 29, 30, 37, 38, 40, 76, 82
VEL2, 44 State-Space, 4, 60, 61
ENABLE, 27, 32, 37
OPEN, 26, 32, 36 xPC Target, 2, 7, 34, 78
POLLING, 27, 31, 36 host PC, 3, 8
SYNC0, 22, 25, 32, 34 target PC, 3, 8
SYNC1, 22, 32, 35
SYNC2, 22, 32, 36
AmigOS
Amigo Operating System, 2, 3, 7, 14,
16, 26, 74
ARIA, 1
MATLAB, 1
Real-Time Workshop, 2, 8, 9, 22
S-Function
System-Function, 9, 10, 16, 34, 3840,
44, 81
Simulink, 2, 8, 9, 34
94
VITA
Vita
1 A
LT EX 2 is an extension of LATEX. LATEX is a collection of macros for TEX. TEX is
a trademark of the American Mathematical Society. The macros used in formatting this
thesis were written by Dinesh Das, Department of Computer Sciences, The University of
Texas at Austin, and extended by Bert Kay and James A. Bednar.
95