Upload
digiblood
View
113
Download
4
Tags:
Embed Size (px)
Citation preview
On E-Puck Mobile Robots for Distributed Robotics
Submitted by
Mohamed Isa Bin Mohamed Nasser
Department of Electrical & Computer Engineering
In partial fulfillment of the
requirements for the Degree of
Bachelor of Engineering
National University of Singapore
i
ABSTRACT
This project provides tools to enable the E-Puck to be used for distributed
robotic experiments. The capabilities of the E-Puck robot for distributed robotics in
terms of communication, motion planning and localization has limitations.
For motion planning, tools to implement grid based motion paths have been
created. This includes software for path planning and synchronization codes. A
demonstration of a sample path is presented. For the limitations in communication
and localization, a second robot is developed to provide pose information and a
communication channel. The development of vision based localization is incomplete
and left for future work.
ii
ACKNOWLEDGEMENTS
I am thankful to the patience, understanding and flexibility of my Supervisor Dr.
Hai Lin throughout this project.
I would also like to thank my Graduate Assistant, Mr. Mohammad Karimadini who
has, throughout this project, given much encouragement, constructive inputs and
technical help.
iii
TABLE OF CONTENTS
ABSTRACT........................................................................................................................ I
ACKNOWLEDGEMENTS.................................................................................................... II
TABLE OF CONTENTS ...................................................................................................... III
LIST OF FIGURES............................................................................................................. III
CODE FRAGMENTS ........................................................................................................VII
LIST OF TABLES..............................................................................................................VII
LIST OF ABBREVIATIONS...............................................................................................VIII
CHAPTER 1 INTRODUCTION ....................................................................................... 1
1.1 LITERATURE REVIEW ............................................................................................ 3
CHAPTER 2 CAPABILITY OF THE E-PUCK ROBOT .......................................................... 4
2.1 E-PUCK'S SENSORS .................................................................................................. 4
2.1.1 Infrared Sensor............................................................................................... 4
2.1.2 3d Accelerometer.............................................................................................. 5
2.1.3 CCD Camera ..................................................................................................... 6
2.1.4 Microphones..................................................................................................... 6
2.2 LOCALIZATION OF THE E-PUCK ................................................................................ 7
2.2.1 Global Localization ........................................................................................... 7
2.2.2 Relative Localization ......................................................................................... 7
CHAPTER 3 BLUETOOTH COMMUNICATION OF THE E-PUCK........................................ 9
3.1 STRUCTURE OF A PACKET ...................................................................................... 10
3.2 BASIC COMMUNICATION IN THE E-PUCK ............................................................... 11
3.2.1 Overview of Operation in Transparent Mode............................................ 11
3.2.2 Setting to Transparent Mode ...................................................................... 12
3.2.2.1 Factory Reset ...................................................................................... 13
3.2.2.2 Set Device Name .............................................................................. 15
3.2.2.3 Set Device PIN ..................................................................................... 16
3.2.2.4 Set Transparent Mode......................................................................... 16
3.2.3 Establish Connection....................................................................................... 17
3.2.3.1 Inquiry................................................................................................. 18
3.2.3.2 Issues With The Inquiry........................................................................ 19
3.2.3.3 Selection of Devices............................................................................. 19
3.2.3.4 Establish SPP Link................................................................................ 22
3.2.3.5 Issues With SPP Link ............................................................................ 23
iv
3.2.3.6 Results From Forcing Establish Connection .......................................... 24
3.2.4 Sending and Receiving Data in Transparent Mode .......................................... 24
3.3 OVERVIEW OF OPERATIONS IN COMMAND MODE................................................. 26
3.3.1 Setting to Command Mode ............................................................................. 27
3.3.2 Open Ports...................................................................................................... 28
3.3.3 Establish Connection....................................................................................... 30
3.3.4 Sending and Receiving in the Command Mode................................................ 32
3.3.4.1 Sending ............................................................................................... 33
3.3.4.2 Receiving............................................................................................. 33
3.4 ISSUES WITH BLUETOOTH COMMUNICATION ................................................. 34
CHAPTER 4 MOTION PLANNING OF THE E-PUCK ROBOT ........................................... 35
4.1 DYNAMIC VS STATIC............................................................................................... 35
4.2 KINEMATICS OF THE E-PUCK.................................................................................. 35
4.3 PHYSICAL SPECIFICATION ...................................................................................... 36
4.4 MODEL FOR DIFFERENTIAL DRIVE.......................................................................... 36
4.5 FORWARD KINEMATICS......................................................................................... 37
4.6 INVERSE KINEMATICS............................................................................................ 38
4.6.1 Inverse Kinematics using Curved Trajectories .................................................. 38
4.6.2 Inverse Kinematics using straight Trajectories................................................. 38
4.7 KINEMATICS SOFTWARE FOR THE E-PUCK............................................................. 39
4.7.1 Tracking the E-Pucks ....................................................................................... 39
4.7.2 Forward Kinematics (software) ....................................................................... 40
4.7.3 Inverse Kinematics using straight paths (software) ......................................... 41
4.7.4 Inverse Kinematics using Curved paths (software)........................................... 42
4.7.5 Graphing feature ............................................................................................ 43
4.7.6 Command list ................................................................................................. 44
4.7.7 Sending Command to E-Pucks ......................................................................... 44
4.7.8 Pending Developments ................................................................................... 44
4.8 GRID BASED PATH PLANNING .............................................................................. 45
4.8.1 Simple Software to convert Grid movements into Code ................................... 45
4.8.2 Add................................................................................................................. 46
4.8.3 Move .............................................................................................................. 46
4.8.4 Wait ............................................................................................................... 47
4.8.5 Generate Code................................................................................................ 47
CHAPTER 5 DEVELOPMENT OF A LAPTOP ROBOT...................................................... 49
5.1 DESIGN OF THE LAPTOP ROBOT............................................................................. 49
5.2 IMPLEMENTING STEREO VISION ............................................................................ 50
5.2.1 Camera Calibration......................................................................................... 50
5.2.2 Matching........................................................................................................ 52
5.2.3 Disparity Map................................................................................................. 52
5.3 STEREO SLAM........................................................................................................ 53
v
5.4 COMMUNICATION CHANNEL FOR THE E-PUCK ...................................................... 54
5.5 LOADING TASKS INTO THE E-PUCK......................................................................... 55
5.6 SYNCHRONIZATION OF THE E-PUCK....................................................................... 56
CHAPTER 6 RESULTS ................................................................................................ 58
6.1 DIRECT COMMUNICATION RESULTS FOR E-PUCK................................................... 58
6.1.1 Single Point Through Transparent Mode ......................................................... 58
6.1.2 Multipoint Through Command Mode .............................................................. 59
6.1.3 Quality of Communication Through the Bluetooth Module.............................. 59
6.1.4 Mirroring using a Single point connection ....................................................... 60
6.1.5 Mirroring using a Multipoint Connection......................................................... 60
6.1.6 Slave to Master communication in Command Mode ....................................... 61
6.2 COMMUNICATION THROUGH THE LAPTOP ROBOT ............................................... 61
6.2.1 Grid Based Motion Planning Results................................................................ 62
6.3 RESULTS OF THE LAPTOP ROBOT’S VISION ............................................................ 64
6.3.1 Stereo Vision................................................................................................... 64
6.3.1.1 Matching ............................................................................................ 64
6.3.1.2 Disparity Map ..................................................................................... 66
6.3.1.3 Object Recognition .............................................................................. 69
6.3.1.4 Camera calibration parameters........................................................... 71
6.3.1.5 Triangulation ...................................................................................... 71
CHAPTER 7 CONCLUSION ......................................................................................... 73
7.1 FUTURE WORK...................................................................................................... 73
REFERENCES.................................................................................................................. 75
APPENDIX A - E-PUCK BLUETOOTH MODULE CODES....................................................... 77
APPENDIX B – VISION DATA AND CODES........................................................................ 81
APPENDIX C – E-PUCK SYNCHRONIZATION CODE............................................................ 77
APPENDIX A - E-PUCK BLUETOOTH MODULE CODES....................................................... 77
LIST OF FIGURES
Figure 1.1 : E-Puck IR Sensor Position............................................................................... 5
Figure 2.2 E-Puck 3d Accelerometer Position................................................................... 5
Figure 2.3 E-Puck Microphone Position ........................................................................... 6
Figure 3.1 : Overview of transparent mode .................................................................... 12
Figure 3.2 : Initializing to Transparent Mode .................................................................. 13
vi
Figure 3.3 : Actual Packet for Factory Reset.................................................................... 14
Figure 3.4 : Flowchart for Establishing Connection.......................................................... 17
Figure 3.5 : Flowchart for Establishing Connection.......................................................... 20
Figure 3.6 : Flowchart for Forcing SPP Link Connection ................................................... 23
Figure 3.7 : Flowchart of operation in Command Mode .................................................. 26
Figure 3.8 : Initialization for Command Mode................................................................. 27
Figure 3.9 : Establishing Connection in Command Mode................................................. 31
Figure 4.1 : Wheel Base Length ...................................................................................... 36
Figure 4.2 : Model of Differential Drive .......................................................................... 37
Figure 4.3 : Overhead Camera Tracking.......................................................................... 40
Figure 4.4 : Forward Kinematic Controls......................................................................... 40
Figure 4.5: Inverse Kinematics Controls (straight path) ................................................... 41
Figure 4.6 : Straight Line Path ........................................................................................ 42
Figure 4.7 : Inverse Kinematics using Curved Paths......................................................... 42
Figure 4.8 : Trajectory From inverse kinematics using Curve Path ................................... 43
Figure 2.17 NyARToolkit Control Flow............................................................................ 37
Figure 2.18 NyARToolkit Flowchart ................................................................................ 38
Figure 3.1 Android running in SDK Emulator................................................................... 41
Figure 3.2 Android Architecture Protocol stack .............................................................. 43
Figure 4.9 : Grid based Path Planning Software .............................................................. 46
Figure 4.10 : Generated Command from Software.......................................................... 48
Figure 5.1 : Setting up Bluetooth Connection for both E-Puck ......................................... 55
Figure 5.2 : Synchronization flowchart ........................................................................... 57
Figure 6.1 : Mirroring using Multipoint connection......................................................... 61
Figure 6.2 : Grid Movement Sequence for Experiment.................................................... 62
Figure 6.3 : Actual Movement by the E-Pucks................................................................. 63
Figure 6.4 : Matching using SIFT descriptors ................................................................... 65
Figure 6.5 : Matching using Multi Scale algorithm .......................................................... 65
Figure 6.6 : Dense Disparity Map using SIFT matches...................................................... 67
Figure 6.7 : Dense disparity map using multiscale matching........................................... 68
vii
Figure 6.8 : Objects selected from corresponding Scene ................................................. 68
Figure 6.8 : Object Recognition, Left image is taken at an earlier time frame................... 70
Figure 6.9 : Stereo Triangulation.................................................................................... 72
CODE FRAGMENTS
Code Fragment 2.1 Get Infrared Sensor ........................................................................... 5
Code Fragment 2.2 Get 3d Accelerometer........................................................................ 5
Code Fragment 2.3 Get CCD Camera ................................................................................ 6
Code Fragment 2.4 Get Microphone ................................................................................ 6
Code Fragment 3.1: Inquiry ........................................................................................... 21
Code Fragment 3.2: Get friendly names of Devices......................................................... 21
Code Fragment 3.3: Select E-Pucks From List.................................................................. 22
Code Fragment 3.4: Sending of the request packet......................................................... 24
Code Fragment 3.5: Send data in Transparent Mode ...................................................... 25
Code Fragment 3.6: Receive data in Transparent Mode.................................................. 25
Code Fragment 3.7: Receive data in Transparent Mode.................................................. 25
Code Fragment 3.8: Establishing multiple SPP link.......................................................... 32
Code Fragment 3.5: Send data in Transparent Mode ...................................................... 25
Code Fragment 3.6: Receive data in Transparent Mode.................................................. 25
Code Fragment 3.7: Receive data in Transparent Mode.................................................. 25
LIST OF TABLES
Table 3.1 : Values in a Packet Structure.......................................................................... 10
Table 3.2 : Restore Factory Settings ............................................................................... 13
Table 3.3 : Write Local Name ......................................................................................... 15
Table 3.4 : Write Device PIN .......................................................................................... 16
Table 3.5 : Set to Transparent Mode .............................................................................. 17
viii
Table 3.6 : Inquiry Packets............................................................................................. 18
Table 3.7 : Get device friendly Name.............................................................................. 21
Table 3.8 : Establish SPP Link ......................................................................................... 23
Table 3.9 : Write Operation Mode ................................................................................. 28
Table 3.10 : Opening of Ports......................................................................................... 30
Table 3.11 : SPP link for Command Mode....................................................................... 31
Table 3.12 : SPP Send Data ............................................................................................ 33
Table 5.1 : Grid Based Task Codes .................................................................................. 56
Table 6.1 : Generated Code for the Sequence................................................................. 63
Table 6.2 : Comparison between Disparity Map Value with Manual Measurements........ 69
List of Abbreviations
RSSI Radio Signal Strength Indicator
SIFT Scale Invariant Feature Transform
SLAM Synchronous Localization and mapping
SPP Serial Port Profile
1
1
Introduction
The E-Puck robot is a differential drive robot developed by Ecole Polytechnique
Fédérale de Lausanne (EPFL) that is originally used for educational purposes [1].
The E-Puck’s relative low cost and its many sensors have made it popular in
experiments for swarm robotics [2].
Research in distributed robotics has been very active in recent years. Solutions
using multiple agents are often more robust and scalable than using a single agent
solution. The occurrence of emergence in a swarm of robot creates complex
behaviours from simple rules followed by every agent [3]. The hardware
requirements of these robots are relative low, resulting in low cost solutions.
The objective of this project is to develop tools to enable easy implementation of
distributed robotics experiments using the E-Puck robots. This platform
concentrates on providing the E-Puck with robust communication, motion
planning and localization capabilities.
The main constraints that are present in the E-Puck robots are its low processing
power of 16 MHz [1] and Bluetooth communication that can accommodate three
slaves at a time [2]. The extents to which the Bluetooth module can accommodate
a network have been studied.
It is found that the resending of commands to the Bluetooth module needs to be
made to establish connection between E-Pucks which is not described in the
Bluetooth manual [2]. Furthermore, while single point to point connection works
2
flawlessly, a multipoint connection results in the Slave unable to send back to the
Master.
For the localization problem, sensors-wise the E-Puck can accommodate relative
localization [4]. This is achieved using the 3 onboard microphones and speaker.
Using the code provided for locating the bearing of the sound source as well as
Bluetooth communication, it is found that even though the bearing of the sound
source is located well, its distance cannot be determined accurately.
As a solution to these constraints, a second robot is designed which uses a laptop
mounted on a wheel base and equipped with two webcams. The role of this robot
is to provide the E-Puck with global localization and a communication channel.
The communication channel has been written in MATLAB. This channel has been
used in the implementation of grid based motion planning. To enable the grid
based movement to be coordinated, synchronization codes is developed for the E-
Pucks.
A demonstration of an example of this movement is presented.
To enable this system to be implemented in the real world, the laptop robot will
use vision based techniques to achieve SLAM. Algorithms to achieve stereo
vision using two regular webcams are explored. For the correspondence problem,
the Scale Invariant Features Transform (SIFT) [5] is compared with another Multi
Scale Feature matching technique that uses Fisher Information Analysis to decide
on correspondence [6].
3
The sparse disparity maps from this matching are converted into dense map by
weighting a segments of the original image. These segments are achieved using
mean shift segmentation algorithm over the original pixels in greyscale. The
accuracy of this dense map is assessed.
1.1 Literature Review
An approach to solving these constraints is by increasing the capability of the E-
Pucks by building hardware extensions. Increasing the capability of the E-Puck
has been explored by creating extension hardware that is fitted in the free UART
provided. Gutiérrez [7] creates an extension that provides situated
communication. This refers to communication that contains information of both
the message and also the physical property of its location. This solves certain
constraints the E-Puck has in communication and localization.
Xiaolei and Changbin [8] have created a testbed for swarm robotics for the E-
Pucks which have similar goals to this project. This testbed gives the E-Puck
global positioning through an overhead camera. Using this information, control
algorithms for navigation is studied.
The testbed focuses on providing the global position to the E-Pucks through a
static world camera. In this project the camera are to be mounted on a laptop robot
which localizes itself globally through vision. Also, the synchronization of the E-
Pucks are achieved through inter E-Puck communication and not coordinated by
the laptop robot.
4
2
Capability of the E-Puck Robot
The E-Puck robot is a 2 wheeled differential drive robot running on the DSPic
microcontroller developed by Ecole Polytechnique Fédérale de Lausanne (EPFL),
France. The main objective for the design of the robot is as an educational tool
[1]. Due to its relative low cost and sensor capabilities, it has also been used for
research in swarm robotics.
In the following section, the sensors that the E-Pucks are equipped with will be
discussed and the corresponding codes to extract its values listed.
2.1 E-Puck’s sensors
In this section, the details of the E-Puck sensors are discussed.
2.1.1 Infrared Sensor
8 Infrared proximity sensors with an effective range of about 20 cm are positioned
around the E-Puck. The position is illustrated in Figure 1.1. These sensors like the
Khapera robot [9] is not positioned in an equidistant manner around the robot.
Therefore a fusion method needs to be used to assess a sensed obstacle position
relative to the robot’s orientation. The weights used in Breitenberg fashion is
provided in the library.
5
Figure 2.1 : E-Puck IR Sensor Position
Code to extract Value
int e_get_prox(unsigned int sensor_number)
Code Fragment 1.1: Get Infrared Sensor
Where sensor_number is an integer from 0 to 7 that is reflected on the figure.
2.1.2 3d accelerometer
The accelerometer in the E-Puck is placed in the position illustrated in the Figure
1.2. The convention used of the axis is also shown.
Figure 2.2 : E-Puck 3d Accelerometer Position
Code to extract Value
int e_get_acc(unsigned int captor)
Code Fragment 2.2: Get 3d Accelerometer
Where captor is 0 for the x axis, 1 for the y axis and 2 for the z axis
6
2.1.3 CCD Camera
The CCD colour camera has a resolution of 640x480 however the fidelity of the
pictures fairly low. Due to the clock speed of the E-Puck, complex image
processing is not possible. This camera is often used for basic colour extractions.
Code to extract value
e_po3030k_launch_capture(char *buffer);
Code Fragment 2.3: Get CCD Camera
Where buffer is a character array to store the image.
2.1.4 Microphones
There are three microphones on the E-Puck. They are located in a symmetrical
manner on the E-Puck in an isosceles triangle configuration as shown in Figure
1.3. The reason for the availability of three microphones is to enable trilateration
of a sound source. The time difference in the detection of the sound source for the
three microphones can be used to calculate the bearing of the sound source [4].
Figure 2.3 : E-Puck Microphone Position
Code to extract value
e_po3030k_launch_capture(char *buffer);
Code Fragment 2.4: Get Microphone
Where buffer is a character array to store the image.
7
2.2 Localization of the E-Puck
In order to carry out distributed robotics experiments, the robot has to be aware of
its pose. This information of its pose can either be relative to each other, and
hence relative localization, or a global one, which is global localization.
2.2.1 Global Localization
Global localization of the E-Puck is not implementable. The 3d accelerometer and
the IR proximity sensors are not capable of tracking features over distances.
Although there are techniques for landmark localization and SLAM for a single
camera, the 16MHz microcontroller is not powerful enough for the involved
calculations.
2.2.2 Relative Localization
Relative localization of the E-Puck can be achieved in a few ways. There are three
microphones located at different points on the E-Puck. A sound wave travelling
from a certain direction will be detected by the microphones at different times.
The difference of these times can determine the bearing of the sound source. The
first E-Puck will send a Bluetooth signal to the second E-Puck which commands it
to emit the sound. A second E-Puck will emit this sound wave from its speaker.
Upon sending the Bluetooth signal, the first E-Puck immediately starts counting.
It will stop counting the moment it detects the sound. The number of counts
divide by its clock speed determines the time for which the sound wave travels.
This time multiplied by the speed of sound gives the distance between the two E-
Pucks. With the bearing and distance, these two E-Pucks can change roles to
8
determine the second E-Puck’s orientation. This whole process achieves relative
localization for the two E-Pucks [4].
If there are three or more E-Pucks with at least the initial distance between two E-
Pucks known, we can simply find the bearing of the third E-Puck from the two E-
Pucks. The location of the intersection of the line of the two bearings can be
calculated since 2 angles and 1 length of the triangle is known.
Lastly, received signal strength indication of the Bluetooth communication
module can be used. The larger the distance between two communicating E-
Pucks, the lower the value of RSSI of the communication link [10]. With a
configuration of 4 E-Pucks, 3 of which with known position, the position of the
fourth E-Puck can be determined through trilateration. This methods will not be
used since only 3 E-Pucks is available currently.
9
3
Bluetooth Communication of the E-Puck
The E-Puck communicates using the LMX9820A Bluetooth module. It runs on
two main operating modes. The transparent mode is designed for single point to
single point link. It emulates a cable between two E-Pucks; every message pushed
into the Tx buffer of one E-Puck will be sent directly to the Rx buffer of the
linked E-Puck. Bluetooth is designed in a master-slave structure. Using
transparent mode, this structure is ignored.
The command mode is for multi point communication. In conventional Bluetooth
modules, a multi point connection of 8 agents can be established at one time. This
means that the master is connected to 7 slaves in a piconet. This enables for
scatternet formation when bridge nodes links one piconet to another. Due to
memory constraints, the Bluetooth module is able to accommodate a piconet of 1
master and 3 slaves. This makes ad hoc networks based on scatternet structures
being more impractical since one of the slaves in the piconet has to function as a
bridge node. A higher number of bridge nodes generally imply slower network
performance [11]. Also, since two piconets are asynchronous, scheduling a high
number of bridge nodes can be difficult.
In order to use any of the module’s function, a packet needs to be written
manually according to the format listed in the software user guide. However this
guide is verbose and does not have any implementation in any specific language.
10
In the following section, a step by step guide to the format of the packet, as well
as fragments of the actual code in C, is presented.
3.1 Structure of a packet
Byte Value Desciption
01 0x02 Start delimiter
02 0x52(REQ)/0x43(CFM)/0x69(IND)/0x72(RES) Packet type
03 <unique command identifier> Opcode
04 – 05 Data Length
06 <Packet type> + <Op code> + <Data Length> Checksum
07 - <last
byte -1>
Command
specific values
<last byte> 0x03 End delimiter
Table 3.1 : Values in a Packet Structure
Referring to table 3.1 for this section, the start delimiter is always 0x02. It marks
the start of the packet. There are three types of packet, namely request (REQ),
confirm (CFM), indication (IND), response (RES). Most operations are made up
of sequential sending and receiving of different packet types. For example, to
establish an SPP connection, a request type followed by a confirm type packet is
required. These sequences are found on page 102 onwards o in the software user
guide (SWUG).
The opcode is a commands identifier. It is therefore unique for each command and
the list of available commands can be found on page 100 of the SWUG.
11
The checksum is a Block Check Character (BCC) checksum of the packet type,
opcode and data length.
Byte 7 onwards is called the “packet data” area and it holds command specific
values that is stated in the corresponding command in the list on page 102
onwards of the SWUG. Finally an end delimiter marks the end of the packet.
There are many commands that are listed for the Bluetooth module. However, for
normal use, there are a few important commands that are more important than the
rest. These commands are those related to the actual sending and receiving of
packet, as well as the establishing of connection. From the experience of
implementing these commands, certain problems and workarounds that are not
explicitly defined in the guide have been encountered. Therefore, the proceeding
section will discuss these functions as well as code fragments of the actual
implementation.
3.2 Basic communication in the E-Puck
Basic communication here refers to communication that does not have any energy
saving or security requirements that is not already set in the factory settings.
As mentioned earlier, the Bluetooth module runs on 2 operating modes, namely
the transparent mode and the command mode.
3.2.1 Overview of Operation in Transparent Mode
The Figure 3.1 illustrates a high level view of the operation in transparent mode.
Both the E-Pucks involved must be set to transparent mode. It follows by either of
12
the E-Pucks initiating a establishing a connection command sequence after which
both E-Pucks has equal ability to transmit data.
Figure 3.1 : Overview of transparent mode
3.2.2 Setting to transparent mode
The Bluetooth module has an EEPROM memory to store its configuration even
after the E-Puck has been switched off. If the state of the configurations is not
known at any point of time, a quick way to ensure that the right configurations are
being used for the operation is by using a factory reset command. This command
is then followed by the setting of the required configuration parameters.
In the case of transparent mode, the required configuration after the factory reset
is the setting of the device name and PIN, followed by the setting of operation
mode to transparent mode. This sequence of configuration is shown in Figure 3.2.
13
Figure 3.2 : Initializing to Transparent Mode
Now we will proceed to the actual low level command to achieve this flowchart.
3.2.2.1 Factory Reset
The factory reset involves a request followed by a confirm.
Packet Transmissions
Table 3.2 : Restore Factory Settings
As the starting example, some parameters here shall be clarified. From Table 3.2,
under the “Opcode” row, “RESTORE_FACTORY_SETTINGS” is provided
instead of the hexadecimal number required for programmatic implementation.
This phrase is actually referring to the name of the opcode from the list of
opcodes on page 98 of the SWUG. In the same way ERROR_OK and
14
ERROR_INVALID_NO_OF_PARAMETERS refers to error codes on page 180
of the SWUG.
Since the second packet is a confirmation type packet, these error codes refer to
the possible data that can be in the corresponding byte. ERROR_OK refers to a
successful operation and takes a value 0x00. To clarify this operation, Figure 3.3
shows the actual request packet that are being sent, and the important parts of the
received packet.
Figure 3.3 : Actual Packet for Factory Reset
The confirm packet is as discussed. Notice that for the received packet, the
important portion is the error code. Also the datalength and end delimiter can be
used to check that the full packet has been received.
15
The rest of the codes for initializing the E-Puck are done in a similar manner by
referring to the SWUG for the format. To make this report concise, the subsequent
functions will have its importance described, followed by a table with the sending
and receiving packets needed that can be implemented in the same manner as the
factory reset example. The full codes used to achieve all the commands for a
normal connection has been included in the appendix.
3.2.2.2 Set Device Name
Device name is the name of the E-Puck used when it is being discovered. By
convention, the E-Pucks are named as “epuck_<ID>” where ID refers to the
corresponding ID physically labelled on the E-Puck. Setting the device name is
important so that the E-Pucks can differentiate themselves in the network.
Packet Transmissions
To set the device name, the packet transmission in Table 3.3 has to be followed.
Send request:
Receive confirm:
Table 3.3 : Write Local Name
16
3.2.2.3 Set device PIN
The Bluetooth device is connected to each other through a process called pairing.
This involves the discovery of the Bluetooth device followed by the entering of a
PIN. This PIN acts as a security mechanism to protect the device from
unauthorized access. By convention, the E-Puck’s PIN is its ID. Setting this after
factory reset is useful to ensure the right device is being connected to.
Packet Transmission
To set the device PIN, the packet transmission in Table 3.4 has to be followed.
Send Request:
Receive Confirm:
Table 3.4 : Write Device PIN
3.2.2.4 Set Transparent Mode
In order to operate in transparent mode, this configuration has to be set on both E-
Pucks involved in the communication.
Packet Transmission
To set to Transparent Mode, the packet transmission in Table 3.5 has to be
followed.
Send Request:
17
Receive Confirm:
Table 3.5 : Set to Transparent Mode
3.2.3 Establish connection
Since Bluetooth uses frequency hopping to provide a connection between two
devices, the mechanism for establishing of connection is relatively complex.
Figure 3.4 : Flowchart for Establishing Connection
From Figure 3.4, it shown that E-Puck 1 is connecting to E-Puck 2. After
configuring its settings, E-Puck 2 has to simply wait for a connection from E-Puck
1. There are three steps in order to establish a connection in the transparent mode.
Firstly, the inquiry step is the “finding” step where Bluetooth devices within the
18
network range is inquired and their details being saved in the E-Puck. This list of
Bluetooth devices then have to be parsed in the second step and only the E-Puck 2
have to be selected. Lastly, with the right E-Pucks known, we can then make a
connection. This process is shown in the Figure 3.4.
3.2.3.1 Inquiry
The inquiry command has three steps; a request followed by a confirm and then
the indicator packets which gives the information of the actual devices found.
Packet transmission
Send Request:
Receive Confirm:
Receive Indicator:
Table 3.6 : Inquiry Packets
19
3.2.3.2 Issues with the Inquiry
One issue with the inquiry process is that sometimes devices within range are not
found. This problem can solved by checking if a particular device has been found
and running the inquiry again if it has not. However, in scenarios where the
targeted devices within range are not known exactly, this error must be taken into
consideration.
Alternatively multiple runs of the inquiry command can be made and any new
devices found will be saved. Since this process is time consuming and the
discussion involves connection between known E-Pucks, this approach is not
covered. Instead a check for the targeted E-Puck will be done. This will be
covered in the selection section.
3.2.3.3 Selection of Devices
Among the ways to identify a Bluetooth device using information from the
inquiry scan, using the friendly device name has been chosen. Friendly device
name refers to the same device name covered in the preceding configuration
section. One requirement for selection through this process is that the devices of
interest must be named uniquely. Since the E-Pucks are named uniquely, this
requirement is met.
The first step is to parse the list of devices for E-Puck types. This will be a useful
process since the list of E-Pucks can be used to either find a particular E-Puck, or
updated when new runs of the inquiry is made. Updating all the non E-Puck
devices found is computationally wasteful since they are not of interest.
20
Figure 3.5 : Flowchart for Establishing Connection
The packet transmission that is involved to acquire the device friendly name is
covered next.
Packet Transmission
Send Request:
Receive Confirm:
21
Table 3.7 : Get device friendly Name
Firstly, an inquiry is made as shown in the code fragments 3.1.
int device_find=e_bt_inquiry(e_bt_present_device);
Code Fragment 3.1: Inquiry
Where device_find is the number of Bluetooth devices found and
e_bt_present_device is an array of a structure in the library called BtDevice. The
exact specification of this structure is less important in this discussion; only the
required components will be discussed. This method now populates
e_bt_present_device with information of the devices found.
Next, the array of devices e_bt_present_device is used to get the friendly names of
all the devices found as shown in Code Fragment 3.2. This is of course using the
command discussed on the previous section.
For (i=0;i<device_find;i++) { e_bt_get_friendly_name(&e_bt_present_device[i]); } Code Fragment 3.2: Get friendly names of Devices
Where device_find is the same variable from the inquiry process holding the
number of devices found though the inquiry. e_bt_get_friendly_name is the command
to acquire the friendly name of the Bluetooth device pointed to be the array
_bt_present_device[i].
It will assigned the friendly_name char array inside the BtDevice structure with a
character array of the device’s name. This array can therefore be checked if it
starts with the characters ‘e’, ‘-‘, ‘p’ , ‘u’, ‘c’, ‘k’ . In most cases the first three
22
letter is sufficient. This condition is met using the a conditional statement shown
in Code Fragment 3.3.
if((e_bt_present_device[i].friendly_name[0]=='e')& (e_bt_present_device[i].friendly_name[1]=='-')& (e_bt_present_device[i].friendly_name[2]=='p')) { // It is an E-Puck. Store the device information } Code Fragment 3.3: Select E-Pucks From List
Where i is referring to the counting variable in the For loop. When this condition
is met, it means that the device is indeed an E-Puck and the next step is to store
the device information. This step is trivial and will not be covered. The codes are
in the appendix.
3.2.3.4 Establish SPP Link
Now that the information (in the form of the BtDevice structure) of the device that
is being connected to is known, the next step is to use the command to establish
Serial Port Profile (SPP) link. This command does the actual connection to the E-
Puck itself.
The term Serial Port Profile is used because it emulates a serial connection
between the connected devices.
Packet Transmission
Send Request:
23
Receive Confirm:
Table 3.8 : Establish SPP Link
3.2.3.5 Issues with SPP Link
Similar to the problem in the inquiry command, this command often receives an
error from the confirmation. This occurs when the E-Puck referred to by the
BtDevice structure can not be found for a particular connection attempt.
A way to handle this is to keep sending the request packet until the confirmation
has no errors (ERROR_OK).
Figure 3.6 : Flowchart for Forcing SPP Link Connection
24
Code snippets for forcing establish SPP link command
The Code Fragment 3.4 shows the sending of the request packet followed by the
receiving of the confirm packet.
do{ e_send_uart1_char(send,15); // send the request packet i=0; c=0; do{ i=0; if (e_getchar_uart1(&read[i])) // read the confirm packet { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); // while the end of the confirm packet // hasn’t been reached }while(read[6] !=0x00); // keep sending request packet until the // confirm packet has ERROR_OK (0x00) Code Fragment 3.4: Sending of the request packet
Where e_send_uart1_char is the command to send the packet to the Bluetooth
module through the UART. send is a character array of the request array and read
is the character array of the confirm packet.
Notice that the last line of the snippet, “while(read[6]!= 0x00)”. The seventh byte
of the confirm packet refers to the error code. Therefore this code keep sending
the request packet (send) until ERROR_OK is received.
3.2.3.6 Results from forcing establish connection
Initially this command was not successful even once out of a total of 50 tries.
After the addition of this code, it never fails to establish connection. Therefore
this is a crucial code fragment to establish connection.
3.2.4 Sending and Receiving data in Transparent Mode
25
The transparent mode emulates using a cable link between to devices. Pushing a
packet into the Tx of the UART will automatically be sent to the Rx of the
corrresponding linked UART. In this sense, the transmission in this mode is
simple.
Code snippets for data transmission in transparent mode
Sending
void e_send_uart1_char(char *send, unsigned int datalength );
Code Fragment 3.5: Send data in Transparent Mode
Where send is a character array and datalength is its length.
This method in Code Fragment 3.5 will push the data in the send array into the Tx
of the UART.
Receiving
bool e_getchar_uart1(char *read)
Code Fragment 3.6: Receive data in Transparent Mode
The method in Code Fragment 3.6 receives 1 byte of the data in the Rx channel of
the UART. It return true when a data is received and zero if there is no data to be
received. Therefore a loop is required and a check on the end delimiter OR the
datalength information in the packet must be used to separate one received packet
from another in the Rx. Code fragment 3.7 shows this operation.
do { if (e_getchar_uart1(&read[i])) // read the confirm packet { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); // check if the packet has ended Code Fragment 3.7: Receive data in Transparent Mode
Here either the end delimiter of value 0x03 OR datalength + header length has
been received will mean the packet end has been met. Header length is always 6
bytes. The data length can be read from the fourth byte of confirm packet.
26
The use of an OR statement instead of an AND statement is to be conservative in
the sense that either of the condition is made sufficient. This is to prevent
deadlocks in the case some packet loss may cause either of the condition not met.
3.3 Overview of Operation in Command Mode
The transparent mode is ideal for single point to point connection. For multipoint
connections, the command mode has to be used. Since most of the methods used
in the transparent mode is also used in the command mode, this section will focus
on the additional methods used for operation in this mode. The following
flowchart shows a high level representation of communication in this mode.
Figure 3.7 : Flowchart of operation in Command Mode
The similarities of the operation under both modes are many. Both modes require
the initializing of EEPROM configuration to their respective modes for all the
27
involved devices. Both require only the Master E-Puck to establish connection
with the rest of the E-Pucks.
The main differences, command-wise, between the modes are that the command
mode has to open up multiple ports for connection with multiple E-Pucks. The
establishing of connection to these E-Pucks requires a free port for each E-Puck.
The sending and receiving of data use the port number corresponding to the
connection to a specific E-Puck specify its destination and to differentiate the
received packets.
3.3.1 Setting to command mode
The procedure to set the E-Puck to command mode is almost identical to that of
setting it to transparent mode.
Figure 3.8 : Initialization for Command Mode
28
The only difference lies in the last block where instead of setting it to transparent
mode, the appropriate command for command mode is used. For completeness,
the packet transmission to achieve this is covered.
Packet Transmission
Send Request:
Here the data in the seventh byte of the packet must be 0x00 for command mode.
Receive Confirm:
Table 3.9 : Write Operation Mode
The seventh byte is checked for ERROR_OK.
3.3.2 Open Ports
In the Bluetooth communication, each SPP link is assigned with a unique port.
These ports acts channels to identify the E-Puck involved in the transmission.
These ports have 2 states; they can either be open or closed. Hence, before
connection can be established, a command to open the required ports must be
executed first.
29
There are 30 available ports in the Bluetooth module. It is assigned using a 32-bit
mask (4 bytes). The 31st and 32nd bit must be ‘0’. The opening of the ports is best
explained with an example.
The 4 bytes of hexadecimal data used to open the ports is shown is
0x00 0x00 0x00 0x07
This is equivalent to 0x00000007. It is represented as elements of a character
array which is 1 byte each.
The binary representations of all the bytes are zeros except for the right-most byte
which is
000001112
These binary digits correspond to a port that ascends from right to left.
Binary digit 0 0 0 0 0 1 1 1
Corresponding Port
Port 8 Port 7 Port 6 Port 5 Port 4 Port 3 Port 2 Port 1
This code, therefore, opens port 1 to port 3 while closing the rest of the ports.
There’s a last twist to be aware of. The 4 bytes must be byte inverted in the
packet. Hence, it must be sent to the UART in the following byte inverted order:
0x07 0x00 0x00 0x00
Packet transmission
Send Request:
30
The “PORTS 4 bytes” is the 32 bit mask described.
Receive Confirm:
Table 3.10 : Opening of Ports
3.3.3 Establish Connection
The difference in the establishing of connection in the command mode compared
to the transparent mode is the number of SPP links to be made and the port
numbers to be specified in the SPP Link.
31
Figure 3.9 : Establishing Connection in Command Mode
In Figure 3.8, the inquiry and selection blocks is identical to the one is transparent
mode. The difference lies in the establishing of SPP link.
Table 3.11 : SPP link for Command Mode
In the request packet shown in table 3.11, there are two port parameters. On the 7th
byte, there is the local port number which defines the port of the master E-Puck
that is used for the connection. On the 14th byte, there is the remote port number
that defines the port of the slave E-Puck.
32
Assuming the slave E-Pucks has only 1 master, the remote port number can
always be set to port 1 or 0x01. Since the Master E-Puck has to connect to
multiple slaves, the local port number has to correspond to those which are set to
“open” earlier. They must also be unique for each E-Puck connection.
The SPP connections, which involves a request and a confirm packet as before,
are done sequentially for every E-Puck to be connected to by the Master E-Puck.
Code fragment 3.8 shows the establishing of multiple SPP links.
e_bt_write_local_pin_number(&e_bt_present_epuck[0].number[0]); error=e_bt_establish_SPP_link(&e_bt_present_epuck[0].address[0], 0x01); e_bt_write_local_pin_number(&e_bt_present_epuck[1].number[0]); error=e_bt_establish_SPP_link(&e_bt_present_epuck[1].address[0], 0x02); Code Fragment 3.8: Establishing multiple SPP link
Where e_bt_present_epuck[i].address[n] is the nth byte of the 4 byte address for
the ith E-Puck of the inquiry list. e_bt_present_epuck[i]. number [n] is the nth byte
of the 4 byte PIN for the ith E-Puck of the inquiry list.
In order to authenticate the PIN for every slave, the Master E-Puck change its own
PIN to that of the slave’s and revert it at the end of the code. The
e_bt_write_local_pin_number method writes the PIN of the slave E-Puck
temporarily to the master E-Puck for the establish SPP link command to occur.
The e_bt_establish_SPP_link method, handles the packet transmissions with using
the 2nd parameter as the port for the connection. It always sets the remote port to
port 1.
3.3.4 Sending and Receiving in the Command Mode
Unlike in the transparent, direct sending linking of the Tx and Rx channels of the
UART between two E-Pucks is not possible due to multiple connections from the
33
Master to the Slave. A packet needs to be constructed with enough information to
distinguish its source, when receiving, or destination, when sending.
3.3.4.1 Sending
In the transparent mode, there is no designated command to send data between E-
Pucks. The command mode requires a command due the reasons mentioned and
this command is SPP_SEND_DATA.
Packet Transmission
Send Request:
Here the payload is the character array to be sent and the payload size is its
corresponding size. Sample code has been included in the appendix to clarify the
syntax of constructing this packet.
Receive Confirm:
Table 3.12 : SPP Send Data
3.3.4.2 Receiving
34
The data received in transparent mode is the actual raw data sent by the sender.
For example when sending the character array {‘t’, ‘e’, ‘s’, ‘t’}, the data in the Tx
of the sender is directly sent to the receiver:
However in command mode, using the SPP_SEND_DATA command, The data is
sent in inverted order byte by byte in the form of packets:
3.4 Issues with Bluetooth communication
In command mode, the data that is received by the slaves, contrary to the manual,
does not contain the delimiters or comes in byte by byte. The data that is received
are the raw data itself just as in transparent mode. Master to multiple Slaves
communication is successful. The master is running an obstacle avoidance code
while the slaves are mirroring its movements.
However a persisting problem is getting the Slaves to send data back to the
master. The master is unable to receive packets from slaves in command mode
using the command SEND_SPP_DATA. This is a major problem since scatternets
cannot be formed and a robust communication network for multi agents
communication.
35
4
Motion Planning of the E-Puck Robot
Motion planning or the navigation problem is the process of translating a task that
involves movement into actual motor commands.
4.1 Dynamic vs Static
Motion planning can be divided into two categories, static and dynamic. Static
motion planning is often computed offline. The term offline computations refers
to computations that are done before the task starts often by a central unit which is
then transferred to the agents involved in the task. Static motion planning does not
take into account changes in the environment after the commencement of the task.
Dynamic motion planning is carried out online during the task itself. It is more
versatile and takes into account changes in the environment. In most cases, robots
used in distributed robotics, including the E-Pucks, have processors with low
clock speeds. This is to keep cost low for the duplication of the robot.
Consequently, dynamic motion planning algorithm often has to be kept simple
and computationally low.
4.2 Kinematics of the E-Puck
The E-Puck is a differential drive robot. Differential drive refers to two wheels
that can be driven independently in both directions, clockwise and anticlockwise.
It is important to study the kinematics of the E-Puck to identify useful models that
36
can be used for motion planning. We first look at physical specifications of the E-
Puck that affects its kinematics.
4.3 Physical Specification
The following are the physical specifications of the E-Puck that affects its
kinematics.
Motor type: Stepper Motor
Motor Speed: 1000 steps / second, 1000 steps / revolution
Wheel Diameter: 41 mm
Wheelbase length: 53 mm
The wheelbase length is defined as the length between two points of contact that
the two wheels make with the ground. This is very close to the length shown in
the following picture:
Figure 4.1 : Wheel Base Length
4.4 Model for Differential Drive
37
Figure 4.2 : Model of Differential Drive
Where l is the wheelbase length, Vl is the left wheel velocity, Vr is the right wheel
velocity, R is the turning radius, ICC is the instantaneous center of curvature, ω is
the angular velocity, Figure 4.2 shows the model for a differential drive trajectory.
The wheel input values are integers from -1000 to 1000 where negative value
steer “backwards”, zero stop the wheel and positive value steers “forward”. This
input value corresponds conveniently to physical value of steps/second. Therefore
to obtain the Vl and Vr value, from the input values, the following can be used:
Vl = Il / 1000 × π × 41
VR = IR / 1000 × π × 41
Where the resulting unit is in millimetre per second. With this, we can proceed to
use the rest of the equations that can be used to model the differential drive
trajectory.
4.5 Forward Kinematics
If the current position and orientation of the robot is known, forward kinematic is
the calculation of the new position and new orientation of the robot after a set of
known inputs is carried out at known time intervals.
38
The above equation relates the turning radius and the angular velocity to the input
wheel velocities.
Where x is the initial horizontal position, y is the vertical position and θ is the
orientation at time t. A set of (x,y, θ) is collectively known as the pose of the
robot. (x’, y’, θ’) is the pose at time (t + δt).
These equations can be used to predict the motion and pose of the E-Puck after an
array of given inputs [12].
4.6 Inverse Kinematics
Inverse kinematics is the process of determining the inputs in order to achieve a
desired pose.
4.6.1 Inverse Kinematics using Curved Trajectories
There are only 20002 combinations of left and right wheel speeds. This can be
reduced by taking steps of 10. With this, solutions for a certain destination points
can be obtained by exhaustively running through these trajectories.
4.6.2 Inverse Kinematics using straight Trajectories
39
A more practical method of dead reckoning is through straight trajectories since it
is less error prone especially from slippage. The formulas for the trajectories can
be derived from simple trigonometry.
T1 = atan2 (y’ – y, x’ – x) rad
T2 = ’ – y – T1 rad
D = ( ( x’ – x ) 2 + ( y’ – y ) 2 )1/2 mm
The process will be is hence a rotation T1 rad to face destination � Translate D
mm to destination � Rotation T2 to achieve orientation of destination pose.
4.7 Kinematics software for the E-Puck
A software has been partially developed for offline path planning. This will be
useful in order to automate the calculations especially for inverse kinematics and
also visually selecting destination points by using an overhead camera. This
section will be dedicated to the current working features of the software and its
pending features that can be further developed by later project groups.
4.7.1 Tracking the E-Pucks
In order to track the E-Pucks, a toolkit for tangible multi-touch surfaces developed
by Reactivision is used. This toolkit uses fudicial markers and tracks them using
an overhead webcam.
40
Figure 4.3 : Overhead Camera Tracking
The E-Pucks that are detected are reflected in the basic program provided as black
boxes as shown in figure 4.3. Since this display is now used for robotics, certain
visuals have been made so that analysis of the movement is enhanced. Error is
represented in red text. Error is the difference between the calculated position
based on the differential drive model and the actual position of the E-Puck. Also,
a green line is drawn to represent the velocity vector of the robots. This is make
assessment of how fast the E-Pucks are moving relative to each other, easier.
4.7.2 Forward Kinematics (software)
Figure 4.4 : Forward Kinematic Controls
The controls used for forward kinematics is rather intuitive. It is shown in Figure
4.4. One will have to use the drop down boxes to set the right wheel speed (RW
41
speed), left wheel speed (LW speed) and time at which the wheels run at the
speed. The trajectory that the E-Puck will travel at will be drawn on the display
area as a green line.
4.7.3 Inverse Kinematics using straight paths (software)
Figure 4.5: Inverse Kinematics Controls (straight path)
The algorithm used for this box is the same as the preceding section on inverse
kinematics. The control is also intuitive as shown in Figure 4.5. Destination X, Y
and angle defines the final pose required. The “Get from Marker” dropbox uses a
marker already detected by the camera as the destination pose. Figure 4.6 shows a
straight path planned using this control. The green line defines this path.
42
Figure 4.6 : Straight Line Path
4.7.4 Inverse Kinematics using Curved paths (software)
Figure 4.7 : Inverse Kinematics using Curved Paths
The algorithm used for inverse kinematics using curved path is an exhaustive
search through all the left and right wheel speed with the maximum time for each
combination defined in the “time for each route” field. A dropbox of possible
43
solutions is presented at the end of the search. This dropbox is shown in Figure
4.7 with the label “Choose Trajectory”.
Alternatively, the user can simply click on the display to define the destination
point. The destination angle will be set to zero using this method. This method is
used in the generation of trajectory in Figure 4.8.
Figure 4.8 : Trajectory From inverse kinematics using Curve Path
4.7.5 Graphing feature
With the information on the movement agents on screen, it would be useful to
save the data and represent it graphically as X, Y and angle.
The Zedgraph library for C#.net is used to keep track of the position of 1 agent
currently. This feature is not fully functional since the task organization for
multiple markers has not been developed yet. However the code for 1 robot can be
extended to multiple agents once task organization for multiple agent has been
coded.
44
4.7.6 Command list
The command list saves the commands that have been made from the three
movement options. This list is made up of two arrays, one which it displays,
showing the motor speed required for every task. And the other an array not
displayed which holds the actual message to be sent to the E-Pucks.
4.7.7 Sending Command to E-Pucks
Termie is a serial RS232 terminal developed in C#.net. The code for this terminal
implementation has been integrated into the motion planning software in order to
send the planned movement commands to the respective E-Pucks through
Bluetooth.
Although the software currently can occasionally send commands to a single E-
Puck, the sending is still buggy. This is an issue of coordinating the thread that
runs Termie. Data from the main form needs to be sent to the terminal’s thread
and this has caused certain possible deadlock situations.
4.7.8 Pending Developments
The main pending features that are to be developed are:
• Task management for multiple agents
• Organize the visual for multiple agents (which task to show at a certain
point of time for each agent)
• Replacing termie with a reliable way to send data to the E-Pucks
45
4.8 Grid based path planning
Due to the lab’s interest to developed grid based navigation algorithms by
decomposing global tasks, a big emphasis of the project is to develop a way to
plan, coordinate and implement quickly grid based paths into the E-Puck robots.
This was not implementable due to the issues of multipoint connection in
Bluetooth communication. The synchronization code between the E-Pucks has
also yet to be developed. Lastly, a quick way to convert a certain sequence of grid
based movement into an easy to enter code into the E-Puck is not available yet.
4.8.1 Simple Software to convert Grid movements into Code
The idea of keying in repetitive code for a repetitive translation and rotation
motion is very inefficient. Therefore software needs to be developed to automate
this code generation. The main requirement of the software is that it must be quick
and intuitive. The screen shot of the software can be seen in Figure 4.9.
46
Figure 4.9 : Grid based Path Planning Software
4.8.2 Add
Based on the paper, there are four types of squares. Empty squares, squares
containing an E-Puck, avoid squares and goal squares. Squares are empty by
default. Each of these squares can be added by selecting the appropriate radio
button followed by a double click on its desired placement.
E-Puck squares are in blue, avoid squares in grey, goal squares in yellow and
empty squares in white.
4.8.3 Move
Movement is input using 4 directional buttons. First, select the move radio button.
Then click on a blue box which represents an E-Puck robot. Use the 4 directional
button the move square around the grid.
The move feature only uses simple forward motion with the distant of 1 square. A
reverse motion is omitted so that the issue of deciding optimal places to rotate and
47
reverse is avoided. Also it provides better clarity to the motion such that the
desired motion is always head by the front of the robot.
In the software, rotations are handled automatically. It is always represented by a
clockwise rotation of multiples of 90 degrees. Shorter anticlockwise rotations, in
the case there is a clockwise rotation of 270 degrees, is used and handled by the
rotation algorithm in the E-Puck itself.
4.8.3 Wait
The wait command is for synchronization. At any point of time when a certain E-
Puck needs to synchronize with another on the next movement for both E-Pucks,
this command can be used. The command is simple.
Select the wait radio button. Double click on E-Pucks that need to sync with the
current selected E-Puck. And then click add button. After this choose any
movement to carry out and this movement to the next square will be synchronized
between the selected E-Pucks.
4.8.4 Generate Code
The generate code button generate codes that can be used to program into the E-
Puck. This can be seen in Figure 4.10. Alternatively, number combinations are
also generated to be used with the communication and synchronization program
discussed in the Chapter 5.
48
Figure 4.10 : Generated Command from Software
49
5
Development of a Laptop Robot
Although the original aim of this project is to create tools for distributed robotics
using a homogeneous group of robot, difficulties in communication and
localization of the E-Puck robots prompted the design of a second robot to fill in
these requirements.
This second robot has to be able to carry out global localization and also act as a
communication channel for the E-Pucks. The roles that the E-Pucks and the laptop
robots take may be different. These exact roles will not be discussed in this report.
We shall only discuss the accommodation of localization and communication and
its extents from the addition of the laptop robot.
In order to accommodate experiments in Multi Agent Systems and swarm
robotics, the cost of the build of the laptop robot must be kept low. An analysis of
the cost is given after the assessment of the robot’s components.
Besides low cost, the robot has to be easy to build. Large numbers of the robot
may be needed and hence the structural design of the robot has to be simple for
duplication. These robots are designed to be hand built with aluminium sheets,
low cost and easy to machine, being the primary building material.
5.1 Design of the laptop robot
50
Laptops shall be used as the brain of this robot to due to its availability,
computing power for image processing as well as the available software available
under the PC platform.
Sensors used in this robot must be able to be used to localize the robot, provide
basic collision avoidance and track the E-Pucks.
One obvious sensor is a webcam. Using one webcam, global localization can be
achieved using monocular SLAM. This technique involves tracking of features
and formation of a sparse map.
Two webcams for stereo vision has been chosen instead. This will enable us to
determine the pose of the features directly. Also, the depth map can be used for
collision avoidance.
5.2 Implementing stereo vision
Two cheap CCD webcams with a maximum resolution of 800x600 are used. The
first step is calibration which involves determining intrinsic and extrinsic
parameters of the camera.
5.2.1 Camera Calibration
Camera calibration is the process of finding the camera’s intrinsic and extrinsic
parameter in order to correctly map point in world space into a pixel in the image.
The intrinsic parameters describe the mapping from the camera plane to the image
plane. The parameters are as follows.
51
u0,v0 on the image plane is the principal point
γ is the shear factor
f is the focal length
m is the scale factor that relates pixels to distance
This matrix corrects the camera matrix which maps a point on the world plane
into the image plane. For stereo triangulation, we need to know where the camera
is in relation with each other which is represented as a transformation consisting
of a rotational and a translational matrix. These are the extrinsic parameters.
The intrinsic parameters can be found easily using an automated process by using
the GML C++ Camera Calibration Toolbox [13]. It uses a checkerboard pattern
with known dimensions which it detects automatically in the image.
After calibration using the toolbox, each calibration image will have its own set of
extrinsic parameters. This relates the pose of the camera relative to the checker
box pattern. Therefore the calibration images taken from the two cameras must be
taken at the same time. Two corresponding images will have extrinsic parameters
that correspond to each other. The inverse of one of the transformation matrix can
be used to set one of the cameras as the world origin by a matrix multiplication.
52
5.2.2 Matching
In order to triangulate, we need to match a feature which appears in the two
images which exist in the same world space. This is the correspondence problem.
A common solution to this problem is by first extracting keypoints on both images
using Scale Invariant Feature Transform (SIFT). SIFT features are obtained by a
difference of Gaussians and scaling [5]. The resulting features are robust and
invariant to scaling and some degree of rotation [14]. This makes it ideal for
stereo matching where the projection transform on both cameras may scale the
keypoints differently. A SIFT feature identifier, which is essentially gradient data
of the region around keypoint, is attached to each keypoint. These identifiers are
then compared with each other between the keypoints in the two images to find
correspondence. The implementation used for SIFT is VLFeat [15] in MATLAB.
From the method so far, the matching is still rather poor. The next step is
commonly using the epipolar constraints to eliminate bad matches. However for
this implementation a multiscale feature matching program written by Lorenzo
Sorgi is used which matches using variance of position estimate error, extracted
from the Fisher information matrix.
5.2.3 Disparity Map
The difference between the horizontal components of the match points forms the
disparity map. There are two types of disparity maps, sparse disparity maps and
dense disparity maps. Dense maps can be obtained directly from rectified images,
53
which lines up the epipolar lines between two images, by using pixel by pixel
comparisons. The map that we obtain from keypoint matching are sparse maps.
Using a sparse map for collision avoidance is inconvenient since depth
information of only a small number of points on the images are known. This
makes avoiding a body of obstacle hard. To tackle this, conversion of the sparse
map into a dense one is necessary.
The approach for this is by firstly converting the colour image into a grayscale
image. We then use mean shift segmentation on the image to segment it into
regions. These regions are then weighted by the points in the disparity map.
The policy for weighting can be designed to be conservative for collision
avoidance in the sense that points that indicate a near obstacle will be given more
weight. Also, one good property of dense disparity maps obtained in this manner
is that the ground planes are often on an average distance. This is due to
segmentation of the ground as one piece and being weighted along many
distances. This removes it as a near obstacle which might occur in more accurate
dense maps approximations [16].
Although this method imposes the false assumption that a certain colour will have
the same depth everywhere, it does give good and fast results in most cases.
5.3 Stereo SLAM
54
With the intrinsic and extrinsic parameters, stereo triangulation of the disparity
map can be achieved using the method included in the MATLAB Camera
Calibration Toolbox.
In order to track features between stereo image pairs, SIFT features can be
attached to keypoints in either of the images. These keypoints are then matched
between stereo pairs. Horn method for absolute orientation using unit quaternion
has already been written in MATLAB. It finds the rotation, translation and
optionally scaling that maps a set of 3d coordinates to another in a least square
way. This transformation is the same transformation that the robot has undergone.
This method will accumulate error from the sensed pose of the features.
Therefore a database of features can be constructed containing its global positions.
Once the robot revisit a place and detects a previous keypoint, it can update with
an earlier measurement which has less error. This project will only focus on object
recognition. Further implementation like a sophisticated but similar
implementation by (Se, Lowe and Little) [17] can build upon the results.
5.4 Communication channel for the E-Puck
The laptop comes with an integrated Bluetooth module. The Bluetooth module is
able to connect to multiple E-Pucks at the same time. It can therefore acts as a hub
that routes packet to its right recipients.
To do this, the laptop has to first pair up with the E-Puck. Each E-Puck has its on
Bluetooth device name as well as its own PIN, which by convention is its 4 digit
55
number. Figure 4.11 shows the Bluetooth window panes after the pairing of the E-
Pucks. The E-Pucks are assigned two COM ports each and appears on the list of
devices.
Figure 5.1 : Setting up Bluetooth Connection for both E-Puck
This had been implemented in MATLAB. The format of the packets for
communication is a character array of the form “C,<recipient id>(message)\r\0”
where the recipient id is a pre-assigned unique number given to each E-Puck, ‘\r’
is the carriage return character and \0 is the null character.
5.5 Loading Tasks in to the E-Puck
Tasks are loaded into the E-Puck, from the PC, as integers. Table 5.1 shows the
tasks that have been created for the implementation of grid based path planning:
Task Type Task Id No of
Parameters
Task Parameters
Move Forward 1 0 -
56
Rotation 2 1 <no of 90 degrees clockwise
rotation>
Synchronize 3 <number of
E-Pucks>
<E-Puck1>, <E-Puck2>, <E-
Puck3>, … ,<E-Puck n>
Table 5.1 : Grid Based Task Codes
For example, “2,2” indicates a turn of 180 degrees clockwise. Note that the
internal E-Puck function written to handle this task will turn anticlockwise by 90
degrees if “2,3” is encountered. This is done instead of the inefficient move of
turning clockwise by 270 degrees.
An example for the synchronization command for three E-Pucks is “3,1,1,1” for
E-Puck1. The synchronization will ensure that the next task is done synchronously
with E-Puck2 and E-Puck3 where the corresponding command of the same string
exists.
Notice that the first parameter ‘1’ is redundant; the fact that this task is loaded to
E-Puck1 already implies that E-Puck1 is part of the synchronization. However to
keep the format uniform and in order to use the same code for multiple E-Pucks,
this format is kept. The digit corresponding to the programmed E-Puck is
automatically overwritten to be 1 in the E-Puck online. Therefore the task 3,1,1,1
and 3,0,1,1 for E-Puck1 are equivalent.
5.6 Synchronization of the E-Puck
Synchronization is an important component of distributed systems. Since the
communication channel is now using the laptop robot as a “hub”, the network no
57
longer has to tackle the inter-piconet synchronization and scatternet formation
problem.
Figure 5.2 : Synchronization flowchart
When an E-Puck executes a synchronization task, it will send a “waiting” signal
to other E-Pucks that are involved in the synchronization. Each E-Puck holds an
integer variable for all the E-Pucks in the network which keep track of E-Pucks
that are waiting for them at that point of time. When all the E-Pucks involved in
the synchronization is “waiting”, these variables are reset and the next task
continues.
58
6
Results
In this project, there have been efforts in a diverse range of problems to give the
E-Pucks the ability to localize itself, communicate robustly and do coordinated
grid based motion paths. From E-Puck communication codes for the Bluetooth
module to the vision algorithms of the laptop robot, these efforts has a diverse
range of results some more significant than the other.
Therefore, in this section, we will divide it into results relating to the E-Puck first
in terms of communications, motion planning and localization. This is followed
by results related to the laptop robot which are the vision algorithms to achieve
localization.
6.1 Direct Communication Results for E-Puck
There are two methods of communication which have been tested. The first is
directly through the Bluetooth communication between E-Pucks. There are two
modes within this method namely transparent mode and command mode.
The second method is through the laptop robot acting as a “hub” to receive
packets and send it to the right E-Puck.
6.1.1 Single point through transparent mode
59
The last part of the communication procedure is the establishing of an SPP link.
Using the standard method of sending one request packet is not effective. The SPP
link is not made and no packets could be transferred.
Transparent mode single point connection is achieved by forcing the SPP
connection to persistently send “request” packets until the received “confirm”
packet carries no error (refer to chapter 3 for details). After this code change
which is discovered by trial and error, an E-Puck is always able to connect to
another target E-Puck.
6.1.2 Multipoint through Command mode
The establishment of SPP link between multiple E-Puck is successful using the
same persistent sending method described in the transparent mode section.
Initially, following the SWUG that states that the slave E-Pucks will not receive in
raw data but rather packets that contain byte by byte data, the Slave E-Puck is not
successful in receiving the sent message.
Through trial and error, it is discovered that the data that are received by the slave
E-Pucks are indeed raw data. This approach has been used in the proceeding
experiment to test the quality of this multipoint connection.
6.1.3 Quality of Communication through the Bluetooth module
Since the UART is used for Bluetooth communication between E-Pucks, there is
no mean to probe into the E-Puck directly. A test is therefore designed in using
this connection in a dynamic motion planning algorithm. It is tested by running an
60
obstacle avoidance algorithm on one of the E-Puck. For every update on motor
speeds from this algorithm, the updated speed is sent to the second E-Puck in a
form of motion update command. Upon receiving this command the second E-
Puck will set its motor to the same speed. This causes a mirroring action.
The more latency and the packet loss there is to the connection, the less
coordinated the mirroring will be.
6.1.4 Mirroring using a Single point connection
The mirroring for the transparent mode between two E-Pucks is very good. There
is no observable difference in movement between the E-Pucks. The only failures
occur when the E-Pucks are too far away from each other such that the connection
breaks. This result is however less useful since distributed robotics will require
synchronization between E-Pucks which requires a multipoint connection.
6.1.5 Mirroring using a Multipoint Connection
In command mode, the Master E-Puck runs the obstacle avoidance algorithm. It
then sends its motor update data to the two slave E-Pucks sequentially; first to
slave 1 and then to slave 2. Figure 6.1 shows the movement sequence.
61
Figure 6.1 : Mirroring using Multipoint connection
It can be observed that as the mirroring motion carries on, the slave E-Pucks gets
increasingly less synchronized with the master, especially for slave 2. This can be
attributed to the loss of packets since certain rotational motion of the Master E-
Pucks did not occur on the slave E-Puck which cannot be attributed to latency.
6.1.6 Slave to Master communication in Command Mode
Using the same Bluetooth module command that has been used by the Master to
send to the slaves in the preceding experiment, the Master is unable to receive any
packet from the Slave. The SWUG approach of byte by byte receiving of data as
packets has also failed. This leaves a gap for any form of multipoint
communication for the E-Pucks.
6.2 Communication through the laptop robot
Master
Slave 2
Slave 1
62
Due to the lack of a multipoint communication method, this is the only viable
method of communication.
The mirroring experiment has not been adapted to be used with the
communication through the laptop robot due to time constraints for this project. It
is, however, extensively tested for synchronization in the grid based path planning
which result is discussed in the next section.
6.2.1 Grid Based Motion Planning Results
Due to the research interest in the lab currently, one of the emphases of this
project is to enable fast implementation of grid-based motion paths that are
planned offline. These motions require synchronization of multiple E-Pucks.
The example path that was chosen to test the communication channel, the
movement and the synchronization codes in the E-Puck that has been developed is
given in (Kloetzer and Belta 08) [18].
Figure 6.2 : Grid Movement Sequence for Experiment
The next step is to form this scenario and motion in the grid based motion planner
software as shown in the following Figure 6.3. The code generated is in Table 1.
63
Figure 6.2 : Sequence entered into Software
E-Puck 1 3,1,1,1,2,1,3,1,1,1,1,3,1,1,1,1,2,3,3,1,1,1,1,2,1,3,1,
1,1,1
E-Puck 2 3,1,1,1,1,3,1,1,1,1,2,1,3,1,1,1,1,3,1,1,1,1,2,1,3,1,1,
1,1 E-Puck 3 3,1,1,3,1,1,1,1,2,1,3,1,1,1,1,3,1,1,1,1,2,1,3,1,1,1,1,
2,3,3,1,1,1,1
Table 6.1 : Generated Code for the Sequence
The length of the square is 12 cm and the speed to run this motion is set to 500.
Therefore 12 and 500 is appended to the front of the number sequence. This is
then copied to the MATLAB code that loads the sequences into the E-Pucks and
then acts as a communication channel. The resulting movement is shown in
Figure 6.3 in the same form as Figure 6.1.
Figure 6.3 : Actual Movement by the E-Pucks
The synchronization and movement codes perform very well. The movement code
to move from one square forward is designed to stop after travelling the distance.
1
2
3
64
In frame 2 to 3, E-Puck 3 is making two consecutive “move forward” movements
and there is no observable stoppage. Even from frame 3 to 4 where the same E-
Puck has to synchronize with the rest of the E-Pucks to move synchronously to
the next position, there is no observable delay in the two forward motions
involved. This means that the latency of the synchronization process as well as the
transmissions in the communication channel is negligible relative to the robot’s
motor response.
6.3 Results of the Laptop Robot’s Vision
The requirement of the laptop robot is the ability to localize itself. The approach
to tackle is this is a vision-based localization using landmarks. It must also be able
to tell its relative distance and bearing from the E-Pucks in order to send the E-
Pucks its global pose.
6.3.1 Stereo Vision
Experiments in stereo vision are conducted for matching, sparse to disparity map
estimation for object avoidance, object recognition and triangulation. These
experiments are geared to achieving global localization by landmark ultimately.
6.3.1.1 Matching
The correspondence problem is approached by first using the famous Scale
Invariant Features Transform (SIFT) and matching SIFT features between two
images. This has been described in chapter 6. The results for the matching using
SIFT is shown in Figure
65
Figure 6.4 : Matching using SIFT descriptors
The left image shows the left camera image with the corresponding SIFT
keypoints marked in blue. The right image shows the right camera image with the
matched keypoints between the both images connected by a blue line.
It is clear that many of the keypoints are matched wrongly. The obvious ones are
long lines that cannot be true due to the relative small distance of the two cameras
and zig zag lines that cannot be true since the image has no large distortions.
The multi scale feature matching algorithm that uses the Fischer Information to
identify matches is then used. The results are as follows:
Figure 6.5 : Matching using Multi Scale algorithm
66
Clearly more features are matched. 1887 keypoints are matched for this case of a
640x380 image. This is compared to only 131 matches using comparisons of SIFT
identifiers. The more points there are, the denser the disparity map and
consequently the weighting of image segments used to achieve a dense map will
be more accurate.
The matching is visibly accurate. Since the image is not rectified, the epipolar
lines are not aligned between the images. However it is clear that most of the
points are matched correctly by the manner that the blue lines connecting matched
features are parallel to each other for each portion of the map. The directions of
these lines are dependent on mainly the extrinsic parameters of the stereo cameras.
In this case, the cameras are not well aligned.
6.3.1.2 Disparity Map
The sparse disparity map is proportional to the radial distances of the
corresponding objects to the camera [5]. This factor can be computed as a
function of its focal length and scaling factors from pixel to focal plane. However
in this experiment, this factor is computed experimentally by measuring actual
object distances to the camera and its corresponding disparity value. The result is
a scale factor of 2100 for measurements in the units of centimetres.
After matching, a sparse disparity map is obtained. A sparse disparity map refers
to a map that does not cover almost every pixel in the image. Mean shift
segmentation is used to segment the image based on its grayscale pixel values.
These segments are weighted by the disparity map to form a dense map.
67
The dense disparity map for the SIFT matches is show in Figure 6.5.
X: 241 Y: 312
Index: NaN
RGB: 0.5, 0, 0
X: 147 Y: 184
Index: NaN
RGB: 0.5, 0, 0
100 200 300 400 500 600
50
100
150
200
250
300
350
400
450
Figure 6.5 : Dense Disparity Map using SIFT matches
Due to the low number of matches, most of the segments are not weighted. The
NaN value refers to pixels that have not been weighted. Even with enough
matches, the disparity map will clearly be inaccurate due to the visible
mismatches earlier.
68
Figure 6.6 shows the dense disparity map using the multi scale method.
X: 225 Y: 72
Index: 56.7
RGB: 0, 0, 0.625 X: 386 Y: 136
Index: 350
RGB: 0, 0.813, 1
X: 92 Y: 222
Index: 110.5
RGB: 0, 0, 0.875
X: 364 Y: 348
Index: 35.59
RGB: 0, 0, 0.563
100 200 300 400 500 600
50
100
150
200
250
300
350
400
450
Figure 6.7 : Dense disparity map using multiscale matching
X: 364 Y: 348
RGB: 232, 250, 225
X: 386 Y: 136
RGB: 9, 12, 0
X: 92 Y: 222
RGB: 225, 182, 25
X: 225 Y: 72
RGB: 237, 255, 250
Figure 6.8 : Objects selected from corresponding Scene
The lighter blue regions are regions that are further way while the darker blue
regions are those that are nearer. 4 objects are selected which can be seen with the
69
corresponding image of the scene in Figure 6.7. A can, curtain, drum shell and fan
(in grey) have been selected and the distance to the corresponding points are
measured using a tape measure manually.
The result is tabulated in the following table:
Manual measurement
(cm)
From Disparity Map (cm)
Can 35 35.6
Curtain 200 350.0
Drum shell 149 110.5
Fan 79 56.7
Table 6.2 : Comparison between Disparity Map Value with Manual Measurements
The results are not too far from the manual measurement. This segmentation
method cannot be used for localization however it can be used for obstacle
avoidance where a fuller map is required.
6.3.1.3 Object Recognition
The main aimed method for localization is through landmarks. In order to carry
out landmark localization, the laptop robot must be able to recognize the
landmarks. One way to achieve this is to have a database of objects and its
corresponding SIFT features.
Keypoints, divided in segments, are compared to a database of features which are
grouped in the segments that they are originally found. If the keypoints in one of
70
the segments in the current image does not match any from the database, it is
saved into the database.
One problem is the growing database and a growing amount of comparisons
which results from it. Therefore the maintenance of the database should also be
implemented such that objects that are less likely to be prominent should be
deleted. Due to the constraint of time, this maintenance of the database has not
implemented. The focus shall be on object recognition simply using a fixed size
database that is maintained in a first in first out fashion.
objects1.jpg
objects24.jpg
Figure 6.8 : Object Recognition, Left image is taken at an earlier time frame
This method does have some good object matches. However the segmentation
changes for every viewpoint. This causes certain segments that are selected from a
segment already in the database, but a subset of it. Because the features in the
subset are small, that segment is not recognized. In the Figure 6.8, this situation
has occured as the second segment being a subset of the first segment is not
recognized as the same object. The object is saved as object 21 instead of being
recognized as object 1.
One way to solved this is by checking whether the segments around it has been
recognized and if they have been, it implies that the segment has not been
matched well and should not be saved.
71
However this project will not delve into the policy used for matching segments
and the database maintenance. These issues will be left for future work.
6.3.1.5 Camera calibration parameters
The GML Calibration toolbox has been used to calibrate the cameras. There is no
option to find extrinsic parameter for two cameras. The left extrinsic parameters
obtained during the calibration process using a stereo pair is inverted and
multiplied with the right extrinsic parameters. Although both of these parameters
correspond to the same world before this inversion, they are often in a
inconvenient arbitrary axis based on the free position of the camera from the
calibration pattern.
6.3.1.6 Triangulation
Stereo triangulation is the process of transforming the matched points in
perspective space to Cartesian distances in Euclidean space. The stereo
triangulation function included in the MATLAB calibration toolbox has been
used. The matched points from the multi scale feature matching algorithm are
used with the calibration parameters. The results are in millimetres and shown in
Figure 6.9.
72
Figure 6.9 : Stereo Triangulation
The results are bad. The coordinates obtained does not resemble any of the
physical positions of the objects in the scene from solely the scale. Due to this
result, stereo SLAM cannot be achieved at this stage. An algorithm that finds the
transformation of points in Euclidean Space between two frames can be used to
update the pose of the robot once this issue has been resolved.
-500-400
-300
-500
0
500-500
0
500
73
7
Conclusion
This project has provided significant tools and hardware to solve the limitation of
the E-Puck robots in communication, motion planning and localization.
The step by step guide to achieving Bluetooth communication using the E-Puck
had been important. Crucial tweaks to the sending of commands in order to
achieve both multipoint and single point connection had been discovered. These
commands are not reflected in the Software User Guide of the Bluetooth module.
In motion planning the synchronization method to coordinate the grid based
movement is shown to be fast and robust. This implies the communication
channel is functional and fast. More importantly, it solves the issue of the poor
transmission quality in multipoint connection through Bluetooth which was
demonstrated in the mirroring algorithm.
For localization, significant progress has been made in the realization of a SLAM
algorithm in order to provide the E-Pucks with positional information. A method
to convert a sparse disparity into a dense one has shown decent results which can
be used for obstacle avoidance.
7.1 Future Work
Future work primarily focuses on the vision based localization techniques.
74
The laptop robot must also be able to calculate the relative pose of the E-Puck
from itself. One solution is using the Augmented Reality Toolkit (ARtookit). It
uses fudicial markers and the transformation matrix that maps the camera axis to
the marker’s position is obtained. These fudicial markers can directly obtain the
relative pose by using the translation matrix as the position and the Euler angles of
the rotational matrix as orientation.
75
REFERENCE
[1] Mondada, F., Bonani, M., Raemy, X., Pugh, J., Cianci, C., Klaptocz, A., Magnenat, S., Zufferey, J.-C., Floreano, D. and Martinoli, A. (2009) The e-puck, a Robot Designed for Education in Engineering. Proceedings of the 9th Conference on Autonomous Robot Systems and Competitions, 1(1) pp. 59-65.
[2] LMX9820/LMX9820A Bluetooth Serial Port Module - Software Users Guide, National Semiconductor, October 2006, Revision 1.6.4
[3] Sharkey, A. J. 2007. “Swarm robotics and minimalism” Connect. Sci 19, 3 (Sep. 2007), 245-260
[4] Jean-Marc Valin and François Michaud and Jean Rouat and Dominic Létourneau, “Robust Sound Source Localization Using a Microphone Array on a Mobile Robot”, Proceedings International Conference on Intelligent Robots and Systems (2003)
[5] David G. Lowe, "Distinctive image features from scale-invariant keypoints," International Journal of Computer Vision, 60, 2 (2004), pp. 91-110.
[6] Zhijun Pei, Ping Zhang, Shoumei Sun, Jinqing Gu, "Fisher Information Analysis for Matching Feature Extraction," itcs, vol. 1, pp.425-428, 2009 International Conference on Information Technology and Computer Science, 2009
[7] Gutiérrez, Á., Campo, A., Dorigo, M., Donate, J., Monasterio-Huelin, F., and Magdalena, L. 2009. Open E-puck range & bearing miniaturized board for local communication in swarm robotics. In Proceedings of the 2009 IEEE international Conference on Robotics and Automation (Kobe, Japan, May 12 - 17, 2009). IEEE Press, Piscataway, NJ, 1745-1750.
[8] Xiaolei Hou, Changbin Yu, “On the implementation of a robotic SWARM testbed”, Autonomous Robots and Agents, 2009. ICARA 2009. 4th International , pp. 27 – 32
[9] Narongdech Keeratipranon, Joaquin Sitte,”Beginners Guide to Khepera Robot Soccer”, Queensland University of Technology, 2003
[10] A.K.M. Mahtab Hossain and Wee-Seng Soh, “A Comprehensive Study Of Bluetooth Signal Parameters for Localization”, 18th Annual IEEE International Symposium on Personal, Indoor and Mobile Radio Communications (PIMRC’07)
[11] Wang, Z., Thomas, R. J., and Haas, Z. J. 2009. Performance comparison of Bluetooth scatternet formation protocols for multi-hop networks. Wirel. Netw. 15, 2 (Feb. 2009), 209-226
[12] Dudek and Jenkin, Computational Principles of Mobile Robotics. [13] V.Vezhnevets, A.Velizhev "GML C++ Camera Calibration Toolbox",
http://research.graphicon.ru/calibration/gml-c++-camera-calibration-toolbox.html, 2005
[14] Scale Invariant Feature Transform (SIFT):Performance and Application [15] Vedaldi and B. Fulkerson, “VLFeat: An Open and Portable Library of
Computer Vision Algorithms”, 2008 [16] Wided Miled, Jean Christophe Pesquet and Michel Parent , “Robust
Obstacle Detection Based on Dense Disparity Maps”, Lecture Notes in Computer Science, Computer Aided Systems Theory, EUROCAST 2007
76
[17] Stephen Se and David Lowe and Jim Little, “Local and Global Localization for Mobile Robots using Visual Landmarks”, 2001
[18] Marius Kloetzer and Calin Belta, “Distributed implementations of global temporal logic motion specifications”, ICRA 2008, pp.393-398
[19] Luke Fletcher, “An Introduction to Computer Vision”, http://users.cecs.anu.edu.au/~luke/cvcourse_files/online_notes/lectures_RV_1_dmaps_6up.pdf
77
Appendix A - E-Puck Bluetooth Module Codes
Code for Factory Reset
char e_bt_factory_reset(void) { char send[8]; char read[10]; int i; char c; send[0]=0x02; send[1]=0x52; send[2]=0x1A; send[3]=0x00; send[4]=0x00; send[5]=0x6c; send[6]=0x03;//link number 1-30 e_send_uart1_char(send,7); i=0; c=0; do { if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); read[i]='\0'; return read[6]; //return error 0=no error }
Code for setting Transparent Mode
char e_bt_tranparent_mode(void) { char send[8]; char read[10]; int i; char c; send[0]=0x02; send[1]=0x52; send[2]=0x11; send[3]=0x01; send[4]=0x00; send[5]=0x64; send[6]=0x01;//link number 1-30 send[7]=0x03; e_send_uart1_char(send,8); i=0;
78
c=0; do { if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); read[i]='\0'; return read[6]; //return error 0=no error }
Code for setting Device Name
char e_bt_read_local_name(char *name) { char send[7]; char read[40]; int i; char c; send[0]=0x02; //send Name request send[1]=0x52; send[2]=0x03; send[3]=0x00; send[4]=0x00; send[5]=0x55; send[6]=0x03; e_send_uart1_char(send,7); i=0; c=0; do { if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); read[i]='\0'; for(i=0;i<read[7];i++) //extract Name name[i]=read[i+8]; return read[6];//return error 0=no error }
Code for setting Device PIN
char e_bt_write_local_PIN (char *PIN)
79
{ char send[30]; char read[10]; int i; char c; int numberlenght; numberlenght=strlen(PIN); //send_uart2(PIN,numberlenght); send[0]=0x02; send[1]=0x52; send[2]=0x17; send[3]=numberlenght+1; send[4]=0x00; send[5]=(send[1]+send[2]+send[3]); send[6]=numberlenght; for (i=0;i<numberlenght;i++) send[i+7]=PIN[i]; send[7+numberlenght]=0x03; e_send_uart1_char(send,numberlenght+8); i=0; c=0; do { if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); read[i]='\0'; return read[6];//return error 0=no error }
Code for setting Command Mode
char e_bt_set_commandmode(void) { char send[120]; char read[10]; int i; char c, received; //send_uart2(PIN,numberlenght); send[0]=0x02; //send PIN request send[1]=0x52; send[2]=0x4A; send[3]=0x01; send[4]=0x00; send[5]=(send[1]+send[2]+send[3]); send[6]=0x00; send[7]=0x03; e_send_uart1_char(send,8);
80
i=0; c=0; do{ if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); return read[6]; }
Code for setting open ports
char e_bt_read_local_name(char *name) { char send[7]; char read[40]; int i; char c; send[0]=0x02; //send Name request send[1]=0x52; send[2]=0x03; send[3]=0x00; send[4]=0x00; send[5]=0x55; send[6]=0x03; e_send_uart1_char(send,7); i=0; c=0; do { if (e_getchar_uart1(&read[i])) //read response { c=read[i]; i++; } } while (((char)c != 0x03)||(i<(read[3]+6))); read[i]='\0'; for(i=0;i<read[7];i++) //extract Name name[i]=read[i+8]; return read[6];//return error 0=no error }
81
Appendix B – Vision Data and Codes
Calibration parameter in MATLAB syntax
%Left Camera Extrinsic Parameters [-0.994216 0.041057 0.099245 59.725145; 0.001058 0.927750 -0.373201 80.114263; -0.107397 -0.370937 -0.922427 833.387468];
%Right Camera Extrinsic Parameters
[-0.999576 -0.024518 0.015704 138.503738; -0.029009 0.884785 -0.465095 159.633384; -0.002492 -0.465354 -0.885121 777.938168;];
%Left Camera Intrinsic Parameters [ 436.120192 0 184.507674;
0 435.466024 85.620939;
0 0 1 ];
%Right Camera Intrinsic Parameters
[ 458.547601 0 175.314928;
0 459.008550 46.938338;
0 0 1 ];
Object Recognition MATLAB Code clear all;
webcamL = videoinput('winvideo',3,'YUY2_320x240');
set(webcamL,'ReturnedColorSpace','rgb');
preview(webcamL);
pause(2);
count1 = 1; %total number of objects in dictionary
count = 1;
nooffeatures = 3;
library_size = 30;
library_features = cell(library_size);
library_keypoints = cell(library_size);
library_hits = cell(library_size);
library_h = cell(library_size);
while(1)
im1c = double(getsnapshot(webcamL))/255;
im1c = CoherenceFilter(im1c);
im1c = imresize(im1c,.4);
im1c = uint8(im1c * 255);
im1 = single(rgb2gray(im1c)) ;
%We compute the SIFT frames (keypoints) and descriptors by
[segs labels] = msseg(im1c,10,7,1000); %-- mean shift
segmentation
imcompare = zeros(size(im1));
im1temp = zeros(320,20);
count = count1;
82
%========================================
% Extract out a segment from the image
%========================================
for(i = 0:length(unique(labels))-1) %for segment i
lab_idx =find((labels == i)); %extract out index of segment
imcompare = ones(size(im1));
imcompare = single(imcompare .* 255); %put white background
imcompare(lab_idx)= single(im1(lab_idx)); %paste segment over
background
flag = 0;
matches = [0;0]; %initialize matches
count3 = 1; %number of "decent matches"
historymatch = zeros(1,100); % number of features in ith
"decent match"
historyobject = zeros(1,100); % library index of "decent
match" ie. object<index>.jpg
[fa, da] = vl_sift(imcompare) ;
%========================================
% compare segment all of the dictionary
%========================================
for (j = 1:count)
if(count>1 && j < count)
filename = strcat('objects',int2str(j),'.jpg');
%load dictionary image
imdatabase = imread(filename);
[matches, scores] = vl_ubcmatch(da,
library_features{j});
%[x1,x2,corr_vec,
match]=features_matching(imdatabase,uint8(imcompare), 0, 1, 3, 5e-3);
%compare with segment
%found 1 "decent match"
if (length(matches(1,:))>nooffeatures)
flag = 1;
historymatch(1,count3) =
length(matches(1,:));
historyobject(1,count3) = j;
count3= count3+1;
% pause(2);
end
end
end
%========================================
% if there is at least 1 decent match
%========================================
if (flag ==1)
[numberofmatches, matchid] = max(historymatch(1,:));
%get max match
fprintf('\nObject %d found. %d features
\n',historyobject(matchid),numberofmatches);
filename =
strcat('objects',int2str(historyobject(matchid)),'.jpg')
%========================================
% Traces the object
%========================================
BW = imread(filename);
indx1 = find(BW == 255);
indx2 = find(BW ~= 255);
83
BW(indx1) = 0; % getoutline
BW(indx2) = 1;
%subplot(1,1,1);
imshow(im1c);
s=size(BW);
for row = 2:55:s(1)
for col=1:s(2)
if BW(row,col),
break;
end
end
contour = bwtraceboundary(BW, [row, col], 'W', 8,
inf,...
'counterclockwise');
if(~isempty(contour))
hold on;
%subplot(1,1,1);
plot(contour(:,2),contour(:,1),'g','LineWidth',2);
hold on;
%subplot(1,1,1);
plot(col, row,'gx','LineWidth',2);
end
end
text(col,row,filename,'FontSize',20,'Color','w');
hold off;
pause(.1);
end
%========================================
%========================================
% Nothing Found. Save to Dictionary
%========================================
if (length(lab_idx)>3 && flag ==0)
imdatabase = ones(size(im1));
imdatabase = imdatabase .* 255;
imdatabase(lab_idx)= im1(lab_idx);
fprintf('\nObject not found \n');
library_features{count1} = da;
library_keypoints{count1} = fa;
filename = strcat('objects',int2str(count1),'.jpg');
imwrite(uint8(imdatabase),filename,'jpg');
count1 = mod(count1,library_size)+1;
end
%=========================================
end
end
84
Appendix C – E-Puck Synchronization Code
This code is mainly BTcom.c from the E-Puck code library with appended codes for Synchronization. /********************************************************************************
The reference programm to control the e-puck with the PC
Version 2.0
Michael Bonani
This file is part of the e-puck library license.
See http://www.e-puck.org/index.php?option=com_content&task=view&id=18&Itemid=45
(c) 2004-2007 Michael Bonani
Robotics system laboratory http://lsro.epfl.ch
Laboratory of intelligent systems http://lis.epfl.ch
Swarm intelligent systems group http://swis.epfl.ch
EPFL Ecole polytechnique federale de Lausanne http://www.epfl.ch
**********************************************************************************/
#include <p30f6014A.h>
//#define FLOOR_SENSORS // define to enable floor sensors
//#define LIS_SENSORS_TURRET
#define IR_RECIEVER
#include <string.h>
#include <ctype.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <motor_led/e_epuck_ports.h>
#include <motor_led/e_init_port.h>
#include <motor_led/advance_one_timer/e_led.h>
#include <motor_led/advance_one_timer/e_motors.h>
#include <uart/e_uart_char.h>
#include <a_d/advance_ad_scan/e_ad_conv.h>
#include <a_d/advance_ad_scan/e_acc.h>
#include <a_d/advance_ad_scan/e_prox.h>
#include <a_d/advance_ad_scan/e_micro.h>
#include <motor_led/advance_one_timer/e_agenda.h>
#include <camera/fast_2_timer/e_poxxxx.h>
#include <codec/e_sound.h>
#ifdef LIS_SENSORS_TURRET
//#include <contrib/LIS_sensors_turret/e_devantech.h>
#include <contrib/LIS_sensors_turret/e_sharp.h>
#include <contrib/LIS_sensors_turret/e_sensext.h>
#include <I2C/e_I2C_master_module.h>
#include <I2C/e_I2C_protocol.h>
#endif
#ifdef FLOOR_SENSORS
#include <./I2C/e_I2C_protocol.h>
#endif
#ifdef IR_RECIEVER
#include <motor_led/advance_one_timer/e_remote_control.h>
#define SPEED_IR 600
#endif
#define uart_send_static_text(msg) do { e_send_uart1_char(msg,sizeof(msg)-1); while(e_uart1_sending()); } while(0)
#define uart_send_text(msg) do { e_send_uart1_char(msg,strlen(msg)); while(e_uart1_sending()); } while(0)
static float PI = 3.14159265f;
85
static char buffer[39*52*2+3+80];
static int waitEpuck[3];
static int count = 0;
static int stillwaiting = 0;
static int tempEpuck = 0;
static int waitEpuck[3] = {0,0,0};
static int epucktowaitfor[3] = {0,0,0};
static float lengthofsquare = 7.0f;
static int travelspeed = 500;
static int bypass = 0;
static int gotopositionX = 0;
static int gotopositionY = 0;
static int gotoorientation = 0;
static int positionX = 0;
static int positionY = 0;
static int orientation = 0;
static float distance = 0;
static float angle = 0;
static int tasklist[100];
static int tasksize = 0;
static int currenttask = 0;
static float threshold = 50.0f;
extern int e_mic_scan[3][MIC_SAMP_NB];
extern unsigned int e_last_mic_scan_id;
void loadtask(char *buffer);
void nexttask(void);
void loadtask(char *buffer)
{
int count1 = 0;
int myConvertedInt = 0;
char * tok = strtok(buffer, ",");
while (tok != NULL)
{
myConvertedInt = atoi(tok);
tasklist[count1] = myConvertedInt; //fill tasklist with integers.
count1++;
tok = strtok(NULL,",");
}
tasksize = count1;
currenttask = 3;
lengthofsquare = (float)tasklist[1];
travelspeed = tasklist[2];
nexttask();
}
void nexttask(void)
{
int i = 0 ;
for (i=currenttask;i<tasksize;i++)
{
switch (tasklist[i])
{
case 1: //move forward
move_cm(lengthofsquare,travelspeed);
uart_send_static_text("m(move cm activated)\r\n");
break;
case 2: //turn
i++;
if(tasklist[i] != 4)
turn_to_direction(PI/2 * tasklist[i]);
uart_send_static_text("m(turn to direction activated)\r\n");
break;
case 3: //wait
bypass = 1;
86
sprintf(buffer,"J,%d,%d,%d\r",tasklist[i+1],tasklist[i+2],tasklist[i+3]);
i+=4;
currenttask = i;
uart_send_static_text("m(wait activated)\r\n");
break;
default:
break;
}
if(bypass==1) break;
}
}
/* \brief The main function of the programm */
int main(void) {
char c,c1,c2,wait_cam=0;
int i,j,n,speedr,speedl,positionr,positionl,LED_nbr,LED_action,accx,accy,accz,selector,sound;
int cam_mode,cam_width,cam_heigth,cam_zoom,cam_size;
static char first=0;
char *address;
char *ptr;
#ifdef LIS_SENSORS_TURRET
int sensext_pres=1, sensext_param[2], sensext_debug=0;
unsigned int sensext_value[2];
#endif
#ifdef IR_RECIEVER
char ir_move = 0,ir_address= 0, ir_last_move = 0;
#endif
TypeAccSpheric accelero;
e_init_port(); // configure port pins
e_start_agendas_processing();
e_init_motors();
e_init_uart1(); // initialize UART to 115200 Kbaud
e_init_ad_scan(ALL_ADC);
#ifdef FLOOR_SENSORS
#ifndef LIS_SENSORS_TURRET
e_i2cp_init();
#endif
#endif
#ifdef IR_RECIEVER
e_init_remote_control();
#endif
if(RCONbits.POR) { // reset if power on (some problem for few robots)
RCONbits.POR=0;
RESET();
}
/*Cam default parameter*/
cam_mode=RGB_565_MODE;
cam_width=40;
cam_heigth=40;
cam_zoom=8;
cam_size=cam_width*cam_heigth*2;
e_poxxxx_init_cam();
e_poxxxx_config_cam((ARRAY_WIDTH -cam_width*cam_zoom)/2,(ARRAY_HEIGHT-
cam_heigth*cam_zoom)/2,cam_width*cam_zoom,cam_heigth*cam_zoom,cam_zoom,cam_zoom,cam_mode);
e_poxxxx_set_mirror(1,1);
e_poxxxx_write_cam_registers();
#ifdef LIS_SENSORS_TURRET //check if sensor extension is present and initalizes ports accordingly
e_i2cp_init();
e_i2cp_enable();
sensext_debug = e_i2cp_write (I2C_ADDR_SENSEXT , 0, 49);
e_i2cp_disable();
// Wait for I2C eeprom on tourret to answer.
e_activate_agenda(e_stop_sensext_wait,0);
e_start_sensext_wait();
e_set_agenda_cycle(e_stop_sensext_wait, 100);
while(e_get_sensext_wait());
e_set_agenda_cycle(e_stop_sensext_wait, 0);
87
e_i2cp_enable();
sensext_debug = e_i2cp_read(I2C_ADDR_SENSEXT, 0);
e_i2cp_disable();
if(sensext_debug!=49) // no SENSORS_TURRET reply
{
sensext_pres=0;
}
else
{
sensext_pres=1;
e_init_sensext();
e_init_sharp(); //a executer s'il y a la
tourelle
// uart_send_static_text("ePic\r\n");
}
#endif
e_acc_calibr();
e_calibrate_ir();
uart_send_static_text("\f\a"
"WELCOME to the SerCom protocol on e-Puck\r\n"
"the EPFL education robot type \"H\" for help\r\n");
while(1) {
if (bypass == 1 ) {c = buffer[0]; }
else{
while (e_getchar_uart1(&c)==0)
#ifdef IR_RECIEVER
{
ir_move = e_get_data();
ir_address = e_get_address();
if (((ir_address == 0)||(ir_address == 8))&&(ir_move!=ir_last_move)){
switch(ir_move)
{
case 1:
speedr = SPEED_IR;
speedl = SPEED_IR/2;
break;
case 2:
speedr = SPEED_IR;
speedl = SPEED_IR;
break;
case 3:
speedr = SPEED_IR/2;
speedl = SPEED_IR;
break;
case 4:
speedr = SPEED_IR;
speedl = -SPEED_IR;
break;
case 5:
speedr = 0;
speedl = 0;
break;
case 6:
speedr = -SPEED_IR;
speedl = SPEED_IR;
break;
case 7:
speedr = -SPEED_IR;
speedl = -SPEED_IR/2;
break;
case 8:
speedr = -SPEED_IR;
speedl = -SPEED_IR;
break;
case 9:
speedr = -SPEED_IR/2;
speedl = -SPEED_IR;
break;
case 0:
88
if(first==0){
e_init_sound();
first=1;
}
e_play_sound(11028,8016);
break;
default:
speedr = speedl = 0;
}
ir_last_move = ir_move;
e_set_speed_left(speedl);
e_set_speed_right(speedr);
}
}
#else
;
#endif
}
if (c<0) { // binary mode (big endian)
i=0;
do {
switch(-c) {
case 'a': // Read acceleration sensors in a non filtered way, some as ASCII
accx=e_get_acc(0);
accy=e_get_acc(1);
accz=e_get_acc(2);
ptr=(char *)&accx;
buffer[i++]=accx & 0xff;
buffer[i++]=accx>>8;
buffer[i++]=accy & 0xff;
buffer[i++]=accy>>8;
buffer[i++]=accz & 0xff;
buffer[i++]=accz>>8;
break;
case 'A': // read acceleration sensors
accelero=e_read_acc_spheric();
ptr=(char *)&accelero.acceleration;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.orientation;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr=(char *)&accelero.inclination;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
ptr++;
buffer[i++]=(*ptr);
break;
case 'D': // set motor speed
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
speedl=(unsigned char)c1+((unsigned int)c2<<8);
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
speedr=(unsigned char)c1+((unsigned int)c2<<8);
89
e_set_speed_left(speedl);
e_set_speed_right(speedr);
break;
case 'E': // get motor speed
buffer[i++]=speedl & 0xff;
buffer[i++]=speedl >> 8;
buffer[i++]=speedr & 0xff;
buffer[i++]=speedr >> 8;
break;
case 'I': // get camera image
e_poxxxx_launch_capture(&buffer[i+3]);
wait_cam=1;
buffer[i++]=(char)cam_mode&0xff;//send image parameter
buffer[i++]=(char)cam_width&0xff;
buffer[i++]=(char)cam_heigth&0xff;
i+=cam_size;
break;
case 'L': // set LED
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
switch(c1) {
case 8:
e_set_body_led(c2);
break;
case 9:
e_set_front_led(c2);
break;
default:
e_set_led(c1,c2);
break;
}
break;
case 'M': // optional floor sensors
#ifdef FLOOR_SENSORS
e_i2cp_enable();
for (j=0; j<3; ++j) {
buffer[i++] = e_i2cp_read(0xC0,2*j+1);
buffer[i++] = e_i2cp_read(0xC0,2*j);
}
e_i2cp_disable();
#else
for(j=0;j<6;j++) buffer[i++]=0;
#endif
break;
case 'N': // read proximity sensors
for(j=0;j<8;j++) {
n=e_get_calibrated_prox(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
break;
case 'O': // read light sensors
for(j=0;j<8;j++) {
n=e_get_ambient_light(j);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
}
break;
case 'Q': // read encoders
n=e_get_steps_left();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
n=e_get_steps_right();
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
break;
case 'u': // get last micro volumes
n=e_get_micro_volume(0);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
90
n=e_get_micro_volume(1);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
n=e_get_micro_volume(2);
buffer[i++]=n&0xff;
buffer[i++]=n>>8;
break;
case 'U': // get micro buffer
address=(char *)e_mic_scan;
e_send_uart1_char(address,600);//send sound buffer
n=e_last_mic_scan_id;//send last scan
buffer[i++]=n&0xff;
break;
case 'W': // read Devantec ultrasonic range sensor or Sharp Ir sensor (optional)
#ifdef LIS_SENSORS_TURRET
while (e_getchar_uart1(&c1)==0);
while (e_getchar_uart1(&c2)==0);
sensext_param[0] = (unsigned char)c1+((unsigned int)c2<<8);
// sscanf(buffer,"W,%d\r",&sensext_param[0]);
if (sensext_pres) // If the TP_sensors
tourret is present
{
if(e_sensext_process(sensext_param, sensext_value))
{
switch (sensext_param[0])
{
case -1: //i2c SRFxxx
buffer[i++] =
sensext_value[0]&0xff;
buffer[i++] =
sensext_value[0]>>8;
buffer[i++] =
sensext_value[1]&0xff;
buffer[i++] =
sensext_value[1]>>8;
// sprintf(buffer,"w,%u,%u\r\n",
sensext_value[0], sensext_value[1]);
break;
case -2: // i2c cmps03
buffer[i++] =
sensext_value[0]&0xff;
buffer[i++] =
sensext_value[0]>>8;
buffer[i++] =
sensext_value[1]&0xff;
buffer[i++] =
sensext_value[1]>>8;
// sprintf(buffer,"w,%d,%d\r\n",
sensext_value[0], sensext_value[1]);
break;
default: //analog (sharp,...)
buffer[i++] =
(sensext_value[0]&0xff);
buffer[i++] =
(sensext_value[0]>>8);
// sprintf(buffer,"w,%d\r\n",
sensext_value[0]);
break;
}
// uart_send_text(buffer);
}
else
{
switch(sensext_param[0])
{
case -1:
case -2:
sensext_value[0] = -1;
sensext_value[1] = -1;
buffer[i++] =
sensext_value[0]&0xff;
buffer[i++] =
91
sensext_value[0]>>8;
buffer[i++] =
sensext_value[1]&0xff;
buffer[i++] =
sensext_value[1]>>8;
break;
default:
sensext_value[0] = -1;
sensext_value[1] = -1;
buffer[i++] =
sensext_value[0]&0xff;
buffer[i++] =
sensext_value[0]>>8;
break;
}
// uart_send_static_text("wrong parameter\r\n");
}
}
else
{
uart_send_static_text("LIS sensors turret not present\r\n");
}
#endif
break;
default: // silently ignored
break;
}
while (e_getchar_uart1(&c)==0); // get next command
} while(c);
if (i!=0){
if (wait_cam) {
wait_cam=0;
while(!e_poxxxx_is_img_ready());
}
e_send_uart1_char(buffer,i); // send answer
while(e_uart1_sending());
}
} else if (c>0) { // ascii mode
if (bypass == 0) // bypass for wait command
{
while (c=='\n' || c=='\r')
e_getchar_uart1(&c);
buffer[0]=c;
i = 1;
do if (e_getchar_uart1(&c))
buffer[i++]=c;
while (c!='\n' && c!='\r');
buffer[i++]='\0';
buffer[0]=toupper(buffer[0]); // we also accept lowercase letters
}
switch (buffer[0]) {
case 'A': // read accelerometer
accx=e_get_acc(0);
accy=e_get_acc(1);
accz=e_get_acc(2);
sprintf(buffer,"a,%d,%d,%d\r\n",accx,accy,accz);
uart_send_text(buffer);
/* accelero=e_read_acc_spheric();
sprintf(buffer,"a,%f,%f,%f\r\n",accelero.acceleration,accelero.orientation,accelero.inclination);
uart_send_text(buffer);*/
break;
case 'B': // set body led
sscanf(buffer,"B,%d\r",&LED_action);
e_set_body_led(LED_action);
uart_send_static_text("b\r\n");
break;
case 'C': // read selector position
92
selector = SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 +
8*SELECTOR3;
sprintf(buffer,"c,%d\r\n",selector);
uart_send_text(buffer);
break;
case 'D': // set motor speed
sscanf(buffer, "D,%d,%d\r", &speedl, &speedr);
e_set_speed_left(speedl);
e_set_speed_right(speedr);
uart_send_static_text("d\r\n");
break;
case 'E': // read motor speed
sprintf(buffer,"e,%d,%d\r\n",speedl,speedr);
uart_send_text(buffer);
break;
case 'F': // set front led
sscanf(buffer,"F,%d\r",&LED_action);
e_set_front_led(LED_action);
uart_send_static_text("f\r\n");
break;
#ifdef IR_RECIEVER
case 'G':
sprintf(buffer,"g IR check : 0x%x, address : 0x%x, data : 0x%x\r\n", e_get_check(), e_get_address(),
e_get_data());
uart_send_text(buffer);
break;
#endif
case 'H': // ask for help
uart_send_static_text("\n");
uart_send_static_text("\"A\" Accelerometer\r\n");
uart_send_static_text("\"B,#\" Body led 0=off 1=on 2=inverse\r\n");
uart_send_static_text("\"C\" Selector position\r\n");
uart_send_static_text("\"D,#,#\" Set motor speed left,right\r\n");
uart_send_static_text("\"E\" Get motor speed left,right\r\n");
uart_send_static_text("\"F,#\" Front led 0=off 1=on 2=inverse\r\n");
#ifdef IR_RECIEVER
uart_send_static_text("\"G\" IR receiver\r\n");
#endif
uart_send_static_text("\"H\" Help\r\n");
uart_send_static_text("\"I\" Get camera parameter\r\n");
uart_send_static_text("\"J,#,#,#,#\" Set camera parameter
mode,width,heigth,zoom(1,4 or 8)\r\n");
uart_send_static_text("\"K\" Calibrate proximity sensors\r\n");
uart_send_static_text("\"L,#,#\" Led number,0=off 1=on
2=inverse\r\n");
#ifdef FLOOR_SENSORS
uart_send_static_text("\"M\" Floor sensors\r\n");
#endif
uart_send_static_text("\"N\" Proximity\r\n");
uart_send_static_text("\"O\" Light sensors\r\n");
uart_send_static_text("\"P,#,#\" Set motor position left,right\r\n");
uart_send_static_text("\"Q\" Get motor position left,right\r\n");
uart_send_static_text("\"R\" Reset e-puck\r\n");
uart_send_static_text("\"S\" Stop e-puck and turn off leds\r\n");
uart_send_static_text("\"T,#\" Play sound 1-5 else stop sound\r\n");
uart_send_static_text("\"U\" Get microphone amplitude\r\n");
uart_send_static_text("\"V\" Version of SerCom\r\n");
#ifdef LIS_SENSORS_TURRET
if (sensext_pres) // If the TP_sensors
tourret is present
uart_send_static_text("\"W,#\" Sensor extension turret.
Analog: W,0; analog 5 LEDs: W,0->31; i2c dist: W,-1; i2c comp: W,-2\r\n");
else
uart_send_static_text("\"W,#\" Sensor extension turret not
detected. Function deactivated.");
#endif
break;
case 'I': // receive ready epuck (in waiting operation)
sscanf(buffer,"I,%d\r", &tempEpuck);
tempEpuck--;
if (stillwaiting == 0) // if we are not waiting yet
{
waitEpuck[tempEpuck] = 1; // just remember that this epuck is
waiting
93
uart_send_text("m(E-Puck is not waiting yet)\r\n");
break;
}
stillwaiting = 0;
waitEpuck[tempEpuck] = 1;
for(count = 0; count<3; count++) // check if all epucks we are waiting for
has arrived
{
if ( epucktowaitfor[count] == 1) // if we are waiting for this
epuck
{
if (waitEpuck[count] == 0) // if it hasn't
arrived
stillwaiting = 1; // we have to
wait
}
}
if (stillwaiting == 0) // if all has arrived
{
uart_send_text("m(E-Puck finished waiting)\r\n");
for(count = 0; count<3; count++)
{
if ( epucktowaitfor[count] == 1) // if the
epuck was waiting for us
{
waitEpuck[count] =
0; // reset its value to 'not waiting'
}
epucktowaitfor[count] = 0; // reset to we
are not waiting for any epuck
}
nexttask();
break;
}
else
{
uart_send_text("m(E-Puck still waiting) \r\n");
break;
}
break;
case 'J':// start wait
bypass = 0;
sscanf(buffer,"J,%d,%d,%d\r",&epucktowaitfor[0],&epucktowaitfor[1],&epucktowaitfor[2]);
epucktowaitfor[0] = 1;
waitEpuck[0] = 1;
uart_send_text("m(Commencing waiting now)\r\n");
uart_send_text("C,2(I,1)\r\n"); // send to waiting epucks
uart_send_text("C,3(I,1)\r\n");
stillwaiting = 1;
break;
case 'K': // calibrate proximity sensors
uart_send_static_text("k, Starting calibration - Remove any object in
sensors range\r\n");
e_calibrate_ir();
uart_send_static_text("k, Calibration finished\r\n");
break;
case 'L': // set led
sscanf(buffer,"L,%d,%d\r",&LED_nbr,&LED_action);
e_set_led(LED_nbr,LED_action);
uart_send_static_text("l\r\n");
break;
case 'M': // read floor sensors (optional)
#ifdef FLOOR_SENSORS
e_i2cp_enable();
for (i=0; i<6; i++) buffer[i] = e_i2cp_read(0xC0,i);
e_i2cp_disable();
sprintf(buffer,"m,%d,%d,%d\r\n",
94
(unsigned int)buffer[1] | ((unsigned int)buffer[0] << 8),
(unsigned int)buffer[3] | ((unsigned int)buffer[2] << 8),
(unsigned int)buffer[5] | ((unsigned int)buffer[4] << 8));
uart_send_text(buffer);
#else
uart_send_static_text("m,0,0,0\r\n");
#endif
break;
case 'N': // read proximity sensors
sprintf(buffer,"n,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
e_get_calibrated_prox(0),e_get_calibrated_prox(1),e_get_calibrated_prox(2),e_get_calibrated_prox(3),
e_get_calibrated_prox(4),e_get_calibrated_prox(5),e_get_calibrated_prox(6),e_get_calibrated_prox(7));
uart_send_text(buffer);
break;
case 'O': // read ambient light sensors
sprintf(buffer,"o,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
e_get_ambient_light(0),e_get_ambient_light(1),e_get_ambient_light(2),e_get_ambient_light(3),
e_get_ambient_light(4),e_get_ambient_light(5),e_get_ambient_light(6),e_get_ambient_light(7));
uart_send_text(buffer);
break;
case 'P': // set motor position
sscanf(buffer,"P,%d,%d\r",&positionl,&positionr);
e_set_steps_left(positionl);
e_set_steps_right(positionr);
uart_send_static_text("p\r\n");
break;
case 'Q': // read motor position
sprintf(buffer,"q,%d,%d\r\n",e_get_steps_left(),e_get_steps_right());
uart_send_text(buffer);
break;
case 'R': // reset
uart_send_static_text("r\r\n");
RESET();
break;
case 'S': // stop
e_set_speed_left(0);
e_set_speed_right(0);
e_set_led(8,0);
uart_send_static_text("s\r\n");
break;
case 'T': // stop
sscanf(buffer,"T,%d",&sound);
if(first==0){
e_init_sound();
first=1;
}
switch(sound)
{
case 1: e_play_sound(0,2112);break;
case 2: e_play_sound(2116,1760);break;
case 3: e_play_sound(3878,3412);break;
case 4: e_play_sound(7294,3732);break;
case 5: e_play_sound(11028,8016);break;
default:
e_close_sound();
first=0;
break;
}
uart_send_static_text("t\r\n");
break;
case 'U':
sprintf(buffer,"u,%d,%d,%d\r\n",e_get_micro_volume(0),e_get_micro_volume(1),e_get_micro_volume(
2));
uart_send_text(buffer);
break;
case 'V': // goto position
sscanf(buffer,"V,%d,%d,%d\r",&positionX,&positionY, &orientation);
if((gotopositionX - positionX) < threshold && (gotopositionY - positionY)
< threshold)
95
{
gotopositionX = 0;
gotopositionY = 0;
nexttask();
break;
}
distance = sqrt((float)((gotopositionX - positionX)*(gotopositionX -
positionX) + (gotopositionY - positionY)*(gotopositionX - positionX)));
angle = (float)atan2(gotopositionY - positionY, gotopositionX - positionX
);
turn_to_direction(angle-(orientation/100.0f));
move_cm(distance*10.0f, travelspeed);
uart_send_text("P\r\n");
break;
case 'W': // read Devantec ultrasonic range sensor or Sharp Ir sensor (optional)
#ifdef LIS_SENSORS_TURRET
sscanf(buffer,"W,%d\r",&sensext_param[0]);
if (sensext_pres) // If the TP_sensors
tourret is present
{
if(e_sensext_process(sensext_param, sensext_value))
{
switch (sensext_param[0])
{
case -1: //i2c SRFxxx
sprintf(buffer,"w,%u,%u\r\n",
sensext_value[0], sensext_value[1]);
break;
case -2: // i2c cmps03
sprintf(buffer,"w,%d,%d\r\n",
sensext_value[0], sensext_value[1]);
break;
default: //analog (sharp,...)
sprintf(buffer,"w,%d\r\n",
sensext_value[0]);
}
uart_send_text(buffer);
}
else
{
uart_send_static_text("wrong parameter\r\n");
}
}
else
{
uart_send_static_text("LIS sensors turret not present\r\n");
}
#endif
break;
case 'X': // Dummy command returning a number of bytes given as parameter
loadtask(buffer);
break;
default:
uart_send_static_text("m(z,Command not found)\r\n");
break;
}
}
}
}