Swarm Robot Project Documentation

Overview

Nullam tincidunt, tellus sit amet elementum imperdiet, nunc arcu imperdiet nisl, vitae hendrerit erat nulla eu erat. Vivamus suscipit ante quis leo volutpat mollis. Proin placerat dui id arcu fringilla aliquam. In nec felis a ligula molestie ultrices. Curabitur dapibus vulputate nulla fermentum sollicitudin. Mauris id dui neque, et ultricies ante. Maecenas ac arcu at mauris posuere ultricies at sit amet magna. Etiam eu eros nunc, non hendrerit dui. Morbi blandit dui sed orci euismod eget dignissim purus fermentum. Curabitur nunc arcu, auctor et semper eu, aliquet a odio. Nam ut leo at sapien dictum luctus eu non nibh.

To quickly get a grasp of how to run the Swarm Consensus Estimation Project, refer to the Swarm e-puck Quickstart Guide. This guide provides instruction on setting up the computer systems, what programs to run, and how to configure the e-pucks. For more detailed information about each step, such as program configuration, etc, please read through this article.

The e-puck

The e-puck is a cylindrical robot from EPFL with a diameter of 70 mm and a height of 53 mm, with a stepper motor driven wheel mounted on each side of the body. The small size of the e-puck made it ideal for the project, and the stepper motor driven wheels offered consistency and accuracy, both of which are highly desirable features for experiments requiring motion planning and dead reckoning. In order to address the requirement for wireless communication, the original extension module on the e-puck which contained peripherals including a speaker, infrared receiver, and mode selection switch, was replaced by a custom-made extension module that held an XBee radio module which connected to the serial port of the microcontroller.

Information about the e-puck can be found at http://www.e-puck.org/, and additional documentation by going to Download>Documentation.

e-puck code

The Swarm Consensus Estimation Project E-puck code can be downloaded here: Swarm Consensus Project e-puck Code

Documentation of the code can be found at Swarm Consensus Project e-puck Code Documentation.

The e-puck code was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version). The student version can be downloaded from Microchip's website. When you set up the project, be sure to add the linker library file (libp30F6014A-coff.a) and linker script (p30f6014A.gld) in the project.

Note that the e-pucks will start out in 'sleep' mode, meaning they do not send out packets, nor do they move. Use the 'wake' command in the vision system command window to wake the e-pucks.

Packet Structure

The send and receive packets (for the XBee radios using their proprietary API mode) are different, e.g. the packet received by one PIC is not the exact copy of the packet what was sent out of the other PIC's serial port. The data frame, or payload, is unchanged, although it may be reformatted in the presence of escape characters. Using API 2 packets is important in order to ensure that corrupted or incomplete packets are dropped. For more information about the API modes (1 and 2), and their uses, refer to this and this as well as the XBee manual and datasheets.

Data Frame

The data frame of the packet contains the data needed for the swarm consensus estimator, as well as any additional statistics that we may wish to piggyback with the packet (for example, position and orientation data for data logging).

The data frame (in its current state) contains 15 floating point numbers. The contents are as follows:

x_1
w_1
x_2
w_2
x_3
w_3
x_4
w_4
x_5
w_5
Robot X coordinate
Robot Y coordinate
Robot Theta orientation
Robot left wheel speed
Robot right wheel speed

Each individual number is a 32-bit floating point value, or 4 bytes each, which means that the data frame is 60 bytes long (15 x 4 = 60), although it could be longer if escape characters are needed (see API Operation in the XBee Manual).

Additional values may be added to the packet data frame. For example, the e-pucks currently are fitted with a color sensor board, allowing the e-puck to detect the color of the environment it is in. Tracking and logging these values (red, green, blue) is important, and thus three additional values are added to the data frame. Consequently in the packet handling (XBeePacket) code, the "additional_num" value must be changed from 5 to 8 to accommodate three additional values. This change must be made across all the packet handling code, on both the computers and e-pucks, as an e-puck will not be able to read code with 15 values if its own code is changed to send/receive 18 value packets. The resulting packet data frame would contain the same values 1 - 15 as above, but also 16, 17 and 18, which would represent the R, G, and B values from the color sensor.

XBee Radios

The XBee radio module is a low-cost, low-power (1mW) radio that uses the IEEE 802.15.4 standard (which specifies the physical layer and medium access control layer of the network) and operates on the 2.4GHz ISM frequency band. Each module contains both a RF transceiver and a microcontroller whose firmware provides a basic implementation of networking capabilities such as addressing, packet, and checksums. The radios together form a peer-to-peer network where each member of the network can broadcast messages to any other member of the network. The XBee module communicates with the PIC microcontroller on the e-puck via the serial port using the RS-232 serial data transfer protocol at 115200 bauds per second.

Due to the robust nature of the swarming algorithm, some packet loss was acceptable; packet recovery schemes were foregone in favor of simplicity and lower power consumption.

XBee Radio Configuration

The XBee radio has 20 input/output pins whose signals and functions can be found in section 1.5 in the user’s manual. The only pins that are of particular interest us are:

Pin 1: 3.3V Power
Pin 2 (output): UART Data Out
Pin 3 (input): UART Data In
Pin 10: Power Ground
Pin 12 (output): CTS flow control
Pin 16 (input): RTS flow control

The RTS and CTS signals are for flow control. When the RTS line is pulled low, the XBee module will hold any data waiting to be sent to the microcontroller in a buffer until the line is pulled high again. When the buffer is almost full, the XBee module will pull the CTS signal high.

Details about the XBee radio and its operation can be found in the user’s manual.

Also see the Using the XBee Radio page.

Two firmware options for the microcontroller are available from Digi. The first option implements a suite of networking capabilities including packets, checksums, addressing, and diagnostics; this implementation is specific to the XBee radios and not compatible with other wireless devices. The second option is a ZigBee protocol stack, which is a mesh networking standard for low data rate networks. Because routing and mesh networking capabilities were not needed for this project, the first option was used for simplicity.

The XBee module can be configured using the X-CTU terminal program from Digi, either by sending commands from the terminal or by using the configuration utility found under the “Modem Configuration” tab. Entering the +++ string into the terminal will make the radio enter command mode. Wait until the XBee return The functions of the commands are described below:

Configuration for e-puck XBee radios

For this project, each of the radios on the e-pucks were configured by typing the following commands into the terminal:

+++
atre
atmy <ID>
atap 2
atd6 1
atbd 7
atwr

where <ID> is an ID number used to differentiate the radios. The firmware on the e-puck requires that the IDs range from 0 to 31. In our case, the XBee radios on the robots were given ID numbers 1 through 8, and the base station radio was given an ID of 0.

Configuration for base station/data logger XBee radios

In order to distinguish the radios used by the vision system, real-time display, and data logger, we give them the ID 0 (which means that no robot should have an XBee with ID 0. When a robot received a packet from radio ID 0, then it knows that it is from the vision system computer.). The configuration is the same, except that we do not need flow control, and the ID is always 0.

+++
atre
atmy 0
atap 2
atbd 7
atwr

**XBee AT Commands**
Command	Description
ATRE	Resets the radio parameters to their factory default.
ATMY <ID>	Sets the ID of the radio.
ATAP 2	Enables mode 2 of the radio’s API to enable advanced features such as packets and addressing
ATD6 1	Enables the RTS flow control pin. If this pin is pulled low, the XBee will hold bytes to be transmitted to the microcontroller in its buffer.
ATBD 7	Sets to baud rate to 115200 bps.
ATWR	Writes the new setting to non-volatile memory.

In order for the radios to be able to communicate, they must all be on the same channel and have the same network ID. If the radios experience interference from other XBee radios, the channel or network ID can be changed.

XBee Interface Extension Board

The XBee Interface Extension Board was created with Traxmaker. The extension modules plug into the e-puck via Samtec BTE-020-02-L-D-A connectors, which can be obtained directly from Samtec or one of their distributors. The Traxmaker parts library, which contains the connector and can be downloaded here: Media:Traxmaker_XBee_Lbrary.zip.

Current Version

The Traxmaker file for the current version of the XBee extension board can be downloaded here :Media:e-puck_xbee_board_v1.PCB. Note that the CTS and RTS pins were connected to the sel2 and sel3 pins (instead of y0 and y1) by soldering on jumper wires.

Board In Development

A version of the e-puck with a color sensor circuit built in can be downloaded here: Xbee Interface Extension Board Version 2. This version uses a high-impedance op-amp to amplify signals from three photodiodes (for red, green, and blue), and feeds the outputs into the ADC channels formerly used by the X,Y, and Z axis accelerometers. A 10k potentiometer adjusts the sensitivity for each channel of the amplifier. The RTS flow control line on the XBee is connected to the sel3 line of the e-puck. The CTS line is not hardwired to the sel2 pin, but can easily be connected with a jumper.

Assembling the Boards

Parts:

2x 10 pos. 2 mm pitch socket (Digikey S5751-10-ND)
LE-33 low dropout voltage regulator (Digikey 497-4258-1-ND)
2.2uF tantalum capacitor (Digikey 399-3536-ND)
2x Samtec BTE-020-02-L-D-A (Order directly from Samtec)
0.1"header pins for RTS and CTS pins (you can also use wire for a permanent connection.
2x 0.1" jumpers for connecting RTS and CTS pins if you used header pins(Digikey S9000-ND)

Machine Vision Localization System

A vision system was developed for robot localization, similar to GPS for each individual robot. The system uses cameras mounted overhead and pattern recognition algorithms to track the position and orientation of various targets (robots) in a workspace, and radios the data to the respective robots. The description of the system can be found at Machine_Vision_Localization_System.

Simulator

A MATLAB simulation of the swarm can be found here: Swarm Robot Project Simulator.

Here's early version implementing the formation control algorithm: File:Swarm robot simulation.zip.

Analysis Tools

There are some useful tools that can help you visualize the system, log data, and debug software. They interface with an XBee radio through the serial port, and should be configured like the rest of the radios.

Real-time Display

This is a real-time visualization system that displays the state of the system while it is running. It will draw the ellipse representing the target (the black ellipse)It is written in MATLAB. You can get the files here:File:Swarm RT display.zip.

Connect a configured XBee radio to the computer. Be sure to give the radio ID 0. If you are running another program that uses a serial port, such as the vision system, you will have to use another port and XBee radio. Each serial port can only be accessed by one serial port at a time (therefore, you could have multiple radios connected to the same computer).
Run the open_serial.m script after replacing the 'COM1' parameter in the initXBeeSerial function call with the serial that you are using.
run the RT_Swarm_Plotter.m script. Hit 'q' to stop the logger. Run the script again if you want to resume.
run the close_serial.m script to close the serial port when you are done.

Using the Data Logger with Timestamp

Download the project here: File:Swarm data logger.zip

This program connects to a configured XBee radio (make sure the ID is 0) at the serial port, and reads and parses any XBee packets it receives. It will output two MATLAB files, main_log.m and run_me.m. The file main_log.m contains all of the information in the received packets with a real-time timestamp (in seconds since the start of the program) added, and the file run_me.m contains a short example on how to plot the data.

It is important that you don't close the window while the parser is running, or it will not format the output files property and MATLAB won't be able to read it. To stop logger, press 'a'. This will make it close the files correctly.

The main_log.m output file, when run, will create several arrays in the work space. It will also have a struct called agents which contains copies of these arrays--putting them in a struct makes it possible to access the data by manipulating indexes instead of variable names. The vector agent_list contains the IDs of the agents whose data corresponds to the data in the agents struct.

Packet Data Viewer

Download the project here: File:Swarm packet data viewer.zip

This program will display the raw data in the packets; it is useful for debugging. Written with VC++.

Packet Sender

Download the project here: File:Swarm XBee packet sender.zip

This program sends out XBee formatted packets in an infinite loop. It is useful for debugging.

Making Videos with Overlays

This section explains how to make a video with overlaid figures in MATLAB. To make the video, you'll need the footage, and the output file from the data logger. To align the plotting coordinates with the real-world coordinates, we will use nine equally spaced points arranged in a rectangle centered at the origin for reference points (for example, we can use the center 9 of the 25 dots used to calibrate the camera. Therefore, we need at least a frame where the dots can be seen.

This script assumes that your camera is mounted above the workspace, pointed directly down. It also assumes that your camera has no distortions, and that the x and y axes are perfectly horizontal and vertical, respectively.

You can download the MATLAB files you need here: File:Swarm plotting files.zip

Procedure for Taking the Video:

Turn on all the robots.
Start the video camera.
Turn on and start the data logger.
Turn on the vision system.
Send commands to the swarm to change the settings if needed, and send the wake command to start the swarm.
Send the sleep command the stop the swarm when done.
Press 'a' at the data logger window to close the file, and 'q' to close the program.
Turn off the video camera and robots.

Procedure for generating the video:

Extract the frames of the video with VirtualDub, starting at the moment the robots start moving from the wake command.
Align the plotting axis and the real-world axis of video in Matlab.
Run the script to generate the uncompressed video file.
Compress the video.

Recording the Data

To make a video with Matlab plots overlaid on top of the original footage, you need to log the messages from the base station as well as the robots while the video is running. You should start the logger before sending out commands with the vision system, so that the logger can record the commands as well.

Extracting Frames with VirtualDub

You can extract the frames from a video with a program called VirtualDub (http://www.virtualdub.org). VirtualDub is a free video processing program for splitting, compressing, and processing videos.

Install VirtualDub, and open the video recording (VirtualDub can't open all file types, but most .avi or .mpeg files should be fine).

Move the slider to the frame where the final "wake" command is given (this should be when the robots start moving).
Click the Mark In button to indicate the start of the clip.
Move the slider to where you wish to end the video and click the Mark Out button to indicate the end.
Go to File>Export>Image Sequence...
Enter frame for filename, .jpeg for the filename suffix, and 1 for the minimum number of digits.
Make a folder named "Frames" and set it to be the output directory.
Select JPEG as the file type, set quality to 100.
Click OK to start extracting frames.

Generating the Video

Aligning the Axes

To generate the video, you first need to download the Matlab .m files: File:Swarm plotting files.zip.

Extract the files, and take the Frames folder holding the movie frames and main_log.m generated by the data logger, and put them in the extracted folder which also holds VIDEO_OVERLAY.m

We must now align the plotting axes with the real world axes in the video, so that the position and SCALE will be correct when we overly the figures. To do this, we will use nine equally spaced points arranged in a rectangle centered at the origin. The alignment parameters are saved, so you only need to do this once if you don't move the camera. If need be, you can make a short video clip with marked points on the testbed just for alignment.

To perform the alignment,

Make sure RES is set to the width and height of the resolution of the frames, in pixels (e.g. [width height]).
Go to DOT_X and DOT_Y and set these to the x-axis and y-axis spacing between each of the nine dots in the real world.
Set TEST_OVERLAY_FITTING = 1 and GEN_MOV = 0 and run the script. This will plot a square and nine '+' points in on top of the first frame of the video. Make sure the square is a true square, and not distorted.
Adjust X_OFFSET and Y_OFFSET until the center '+' is lined up with the center dot (the origin of the real world coordinate system).
Adjust SCALE until the other 8 dots are well matched up. SCALE will scale the axis, so increasing SCALE will increase the range of the axis, thus decreasing the unit distance.
The overlay axes should how match the real world axes in the video.
Reset TEST_OVERLAY_FITTING = 0.

Setting Configurations

If you run the VIDEO_OVERLAY.m script with 'GEN_MOV=0', you can preview what the video will look like, but the video will not be saved.

To generate the video, you need to enter the settings that you want.

VIDEO_TIME=TIME_IN_SECONDS: (default = 75) This is the desired video duration in seconds. You must have enough data and video frames to generate the video of this duration.
FPS=FRAMES_PER_SECOND (default = 30) This is the frames per second of the video footage.
GOAL =GOAL STATISTICS (default = [100 300 160000 40000 40000]) This is an array that contains the goal's initial first moments and second central moments ([x y xx xy yy]). If you change the goal from the command console and the data logger recorded it, these values will be overwritten.
COMMR =COMMUNICATION_RADIUS (default = Inf) This is the default communication radius in millimeters. If you change the radius from the command console and the data logger recorded it, this value will be overwritten.

Draw Options

DRAW_COMM_LINKS: =1 to draw communication links, =0 otherwise.
DRAW_SWARM_ELLIPSE: =1 to draw actual ellipse representing swarm, =0 otherwise.
DRAW_GOAL_ELLIPSE: =1 to draw the goal ellipse, =0 otherwise.
DRAW_INDIVIDUAL_ESTIMATES: =1 to draw the individual estimates of the swarm of each robot, =0 otherwise.
DRAW_ROBOT_MARKER_DOT: =1 to draw colored dots on top of the robots, = 0 otherwise.
COMM_LINE_THICKNESS: the thickness of the lines if communication links are drawn.
DOT_MARKER_SIZE: the size of dot marker if the dots are drawn.

Generate Movie File

GEN_MOV: =1 to generate and save the movie (could take a long time), =0 for preview only (much faster, but video won't be saved).

Repairing Damaged Videos

Sometimes, the video generated by MATLAB will be damaged. You can try to repair the file by opening it with VirtualDub, selecting Video>Direct Steam Copy, and then File>Save as AVI.

Compressing the Video

There are many ways to compress the video, including using VirtualDub or Quicktime. To compress the video with Quicktime, open the video and select File>Export.

To use VirtualDub, select Video>Full Processing Mode, go to Video>Compression and select the compression you want to use (you must have the codecs installed on your system), and the go to File>Save as AVI.