Project 5 - Roomba Mapping System

1.0 Objective

The objective of this project is to implement the Roomba mapping system, as described in project 4.

2.0 Hardware

2.1 Boards

The Arduino Mega 2560 board was used to control the Roombas and the base station. The Mega 2560 has an ATmega2560 microcontroller, which uses 17-bit addressing as compared to the 16-bit addressing of the previously used Xplained board. Documentation for the Mega 2560 is available here. An image of the board is shown below.

Arduino Mega 2560 Board

2.2 Roomba

Multiple iRobot Roombas are used as mobile platforms for the mapping hardware. The Roomba is a small, circular robot, about 13.5 inches in diameter, used for automated cleaning. It has three wheels (two drive wheels on the sides and one center wheel for balance), a vacuum and brush, LEDs that indicate its current status, and a number of sensors that help the Roomba navigate as it cleans.

A board communicates with a Roomba through its serial command interface (SCI), which allows UART communication at TTL levels. The Roomba has a mini-DIN port, with pin assignment shown below.

Roomba Mini-Din Connector

The other end of the mini-DIN cable is connected to the breadboard. The cable end, split into pins, is shown below.

Labelled Roomba Pins

2.3 Sonar

A sonar measures distances by emitting pulses of sound that reflect off nearby objects and recording the time for the pulses to return. Each Roomba is equipped with an LV-MaxSonar-EZ0 to measure distances to obstacles. The MaxSonar is capable of ranging objects from 6 to 254 inches away with 1 inch resolution.


The MaxSonar has 7 pins, including Vcc (2.5V - 5.5V DC) and GND. The remaining 5 pins are described below. The sonar datasheet is available here.

MaxSonar Pin Out Function
TX BW high: single sonar pulse for chaining
BW low: serial output in RS232 format
RX Hold high for 20us: start ranging
Hold high: continue ranging
Hold low: stop ranging
AN Analog voltage output (Vcc / 512 volts per inch of distance)
PW Pulse width, scaled at 147us per inch of distance
BW Hold high: chaining
Hold low: serial output on TX

2.4 Servo

Each sonar is also mounted on a servo so that it can be rotated to take measurements without turning the Roomba. A servo is a type of actuator that uses feedback to enable accurate position control. The servo used is the Futaba S3004 (datasheet available here). The servo has 3 pins: Vcc, GND, and Control. A PWM signal is sent over the Control pin to set the servo position. Pulses are between 500µs and 2500µs wide, and sent 20ms apart.

Futaba S3004 Servo

2.5 Radio

The Roombas and base station communicate using wireless radio. The Radio used is the Nordic Semiconductor nRF24L01+ (datasheet available here). The following pins are used in this project.

Pin Function
Vcc Power supply (1.9V - 3.6V DC)
GND Ground
CE Chip Enable; switches between Rx and Tx mode
CSN SPI chip select
SCK SPI clock
MOSI Master Out Slave In; SPI slave data input
MISO Master In Slave Out; SPI slave data output
IRQ Interrupt pin, active low

The nRF24L01+ chip is shown below, with pins labelled.

nRF24L01+ Radio with Pins Labelled

2.6 Bluetooth

The base station communicates with the Android smartphone using Bluetooth. The smartphone has Bluetooth built in, but the base station uses the JY-MCU v1.4 Bluetooth chip (datasheet available here). Pins used are listed below.

Pin Function
EN Enable; set to 1 to enable
VCC Power supply (3.3V - 5V)
GND Ground
TXD UART data output
RXD UART data input

The actual chip is shown below, with pins labelled.

JY-MCU Bluetooth v1.4 with Pins Labelled

2.7 Android Smartphone

A smartphone/tablet is used to display the map data collected. Because of availability, the smartphone used is the Samsung Galaxy Nexus (GT-I9250) and the tablet is the Google Nexus 7 (2012 model).

Samsung Galaxy Nexus

2.8 Wiring

Wiring of the Roomba is shown below. The Roomba is controlled by a Mega2560 board, and has a sonar and servo, as well as a radio.

Roomba Wiring Diagram

The Roomba breadboard is shown below, fully wired up.

Roomba Breadboard

Wiring of the base station is shown below. The base station is also controlled by a Mega2560 board, but it is powered from a laptop instead of a Roomba. The base station has a Bluetooth chip and a radio.

Base Station Wiring Diagram

The constructed base station is shown below.

Base Station

3.0 RTOS Migration

In order to deal with the 17-bit addressing scheme of the ATmega2560, the RTOS implemented in project 3 had to undergo some slight modifications. Specifically, some extra data had to be considered when saving and restoring context. The ATmega2560 still uses SPH and SPL for the stack pointer, but the 17th bit of the stack pointer comes from the Extended Indirect Register (EIND), located at 0x3c. The EIND is concatenated with ZH and ZL when using the EICALL or EIJMP instructions, as shown below.

Indirect Pointer for EICALL/EIJMP

An array of 3 8-bit integers was used to store the stack pointer instead of the previous single 16-bit number. Saving and restoring the current process's stack pointer is shown below.

// Save stack pointer
cur_process->sp[0] = EIND;
cur_process->sp[1] = *(&SP + 1);
cur_process->sp[2] = SP;

// Restore stack pointer
EIND = kernel_sp[0];
*(&SP + 1) = kernel_sp[1];
SP = kernel_sp[2];

The EIND also had to be accounted for when saving and restoring context. It was dealt with in largely the same way as SREG. The new assembly for saving context is shown below; EIND is pushed onto the stack following SREG.

push   r31           ; save r31 first so we can use r31 to save EIND/SREG
in     r31,__SREG__  ; load SREG into r31
cli                  ; disable interrupts
push   r31           ; push r31 (now containing SREG)
in     r31,0x3C      ; load EIND into r31
push   r31           ; push r31 (now containing SREG)
push   r30           ; push the remaining registers
push   r29
push   r28
push   r3
push   r2
push   r1
push   r0

Saving Context with EIND

Assembly for restoring context is shown below. EIND is popped immediately before SREG.

pop    r0
pop    r1
pop    r2
pop    r3
pop    r28
pop    r29
pop    r30
pop    r31            ; pop the saved EIND into r31
out    0x3C, r31      ; restore EIND
pop    r31            ; pop the saved SREG into r31
out    __SREG__, r31  ; restore the saved SREG
pop    r31            ; restore the actual r31

Restoring Context with EIND

The final modification that had to be made was using 17-bit addresses when preparing the stack for a new process. The ATmega2560 expects 3 bytes for the new PC value when returning from a call, so one 0 byte was prepended to each of the addresses of the task function and the Task_Terminate function when they are pushed onto the newly created stack. This byte corresponds to the EIND, and is assumed to be 0 because all RTOS operations are performed in the lower half of memory.

4.0 Sensors and Actuators

Drivers were written to operate the servo and sonar and to query the Roomba sensor data. Drivers were kept modular and separate from one another, and tested individually to iron out errors.

4.1 Servo

The servo requires a PWM signal to set its position, so timer/counter 3 was chosen to generate the signal. The timer was used in fast PWM mode because phase-correct PWM was not required. In fast PWM mode, the timer counts from bottom to top, then loops over and starts counting from bottom up again. The pulse is generated while the timer is between the compare value and the top.

Fast PWM Timing Diagram

The following function initializes the timer for fast PWM operation. An inverted PWM signal is required, so the timer is configured to set the pulse signal at the bottom and clear it when it reaches the compare value. Fast PWM is enabled, and the top is set to the value of OCR3A, which is set to give a pulse period of 20ms. The servo is centered to begin with. Finally, the timer is started by setting the prescaler to 8. This prescaler was chosen for maximum resolution given that pulses are sent with a period of 20ms.

void InitServo() {
    // PE4 = OC3B = Pin #2
    DDRE = _BV(PE4);

    // Clear OC3B on compare match, set at BOTTOM
    TCCR3A = _BV(COM3B1);

    // Fast PWM mode, TOP = OCR3A
    TCCR3A |= _BV(WGM31) | _BV(WGM30);
    TCCR3B |= _BV(WGM33) | _BV(WGM32);

    // Set the PWM period using the TOP compare register

    // Set the servo to the center

    // Start timer with 8 prescaler
    TCCR3B |= _BV(CS31);

    // Turn on the servo

To move the servo, the SetServoAngle function was created. The function maps the angle into the approximate range [480µs, 2220µs], which corresponds to the extremes of the servo range, and the compare value for fast PWM is set accordingly. The pulse width corresponding to the center value of the servo was determined to be approximately 1350µs from experimentation. A delta of 870µs on either side of this center point was found to yield angles very close to +/-90 degrees, leading to the interval mentioned previously. SetServoAngle applies a linear scaling to map an angle into this range; while not exact, the mapping is quite close and good enough for this project. Before returning, it delays for the approximate time it takes for the servo to move to the specified position. The function also caps input angles at 90 or -90 degrees to prevent going past the range of the servo.

void ServoSetAngle(int angle) {
    // Check for errors, cap the possible angle
    if (angle > 90) {
        angle = 90;
    } else if (angle < -90) {
        angle = -90;

    if (angle == 0) {
    } else {
        int delta_pwm = ((int32_t) angle) * PULSE_WIDTH_DELTA / 90;
        set_pulse_width(PULSE_WIDTH_CENTER - delta_pwm);

    // Block while the servo is turning
    uint8_t abs_delta_angle = ABS(angle > current_servo_angle ? angle - current_servo_angle : current_servo_angle - angle);
    for (; abs_delta_angle > 0; abs_delta_angle--) {
        _delay_us(SERVO_SPEED_US_D * 2);

    current_servo_angle = angle;

4.2 Sonar

The sonar code used is very similar to that used in projects 1 and 2. Timer 5 is used for input capture since the other timers are either already used or too small. Translating the input capture timer value into a meaningful distance requires taking a variety of factors into account:

  • The CPU frequency of the board is 16 MHz
  • The speed of sound in air is 340.29 m/s
  • The time measured is actually the time for the sound to go out and come back.

Given the range of expected measurements (up to approximately the sonar's maximum range of 254 inches), a conservative timer prescaler of 64 was chosen to prevent timer overflows while maintaining accurate resolution. This yields the following equation to derive distance from the timer value:

distance = timer_value * (64 * 34'029 cm/s / (16'000'000 ticks/s * 2))
distance = timer_value * 0.068058

The sonar was mounted on top of the servo, and both were raised from the base of the Roomba to get more accurate readings. The mounting is shown in the image below.

4.3 Roomba Wheel Sensors

The Roomba tracks its distance and angle by measuring the number of turns each of the Roomba's drive wheels have travelled. Both of these values are available as part of the second sensor packet, shown below. Distance is the distance travelled since the sensor packet was last requested, and angle is the angle the Roomba has turned through, also since the packet was last requested. These values are used to determine the current position of the Roomba using dead reckoning.

Opcode Packet Code Data 1 Data 2 Data 3 Data 4 Data 5 Data 6
142 2 Remote Control Buttons Distance Angle

Roomba SCI Sensor Packet #2

Since the values are cleared whenever sensor packets are requested, accuracy is lost if packets are requested often. The main loop of the Roomba logic avoids querying the sensor data too often for this reason.

5.0 Communication

The base station, Roomba, and smartphone all communicate to transfer map, state, and command data between them. The base station and Roombas communicate using wireless radio (protocol nRF24). The base station and the Android smartphone communicate using Bluetooth. Bluetooth was chosen because most Android smartphones have built-in support for it. Because Bluetooth only supports paired communication, the smartphone cannot communicate with the Roombas, and only has contact with the base station. The diagram below shows the important channels of communication with their respective protocols.

Communication channels and protocols

5.1 Radio

Packets were implemented as described in project 4, with the addition of a bump packet. This extra packet was used to indicate that the Roomba had hit a wall, and simply contained the Roomba's ID number. See project 4 for a more detailed description of the other packets.

// Packet format for Roomba -> Base Station position information
typedef struct {
    uint8_t roomba_id;
    int16_t delta_x;
    int16_t delta_y;
    int16_t alpha;
} pf_position_t;

// Packet format for Roomba -> Base Station sonar information
typedef struct {
    uint8_t roomba_id;
    int16_t theta;
    int16_t distance;
} pf_sonar_t;

// Packet format for Roomba -> Base Station bump information
typedef struct {
    uint8_t roomba_id;
} pf_bump_t;

// Packet format for Roomba -> Base Station heading direction query
typedef struct {
    uint8_t roomba_id;
} pf_heading_req_t;

// Packet format for Base Station -> Roomba heading direction reply
typedef struct {
    uint8_t roomba_id;
    int16_t theta;
} pf_heading_reply_t;

Packet structs

The provided radio driver was used to send and receive radio packets. In the case of the Roomba, the radio_rxhandler handler signalled an event that read in the packet from the radio. The base station constantly checked for radio packets, so it used radio_rxhandler to set a flag to indicate when a packet was available.

The Roomba code defined a set of helper functions to send radio packets, listed below. The last three are critical functions that continually retry sending until successful. The functions for sending position packets and bump packets are designated as critical because if those packets are not sent, the base station can fall out of sync with the Roomba, and the map becomes invalid. The function for sending the heading request is also critical because the Roomba will not know how to proceed unless it gets a new heading from the base station.

Small delays were inserted between packet transmissions to avoid overflowing the buffer.

inline void transmit_sonar_packet(int16_t distance, int16_t theta);

// Blocking transmission, only returns upon successful transmit
inline void transmit_position_packet_blocking(int16_t delta_x, int16_t delta_y, int16_t alpha);

// Blocking transmission, only returns upon successful transmit
inline void transmit_heading_request_packet_blocking();

// Blocking transmission, only returns upon successful transmit
inline void transmit_bump_packet_blocking();

Radio packet transmission functions

5.2 Bluetooth

Bluetooth was used with the Serial Port Profile (SPP) to emulate RS-232 over a serial cable. After connecting an RX and TX pin to the TXD and RXD pins of the JY-MCU chip, the previously written UART code could be used to faciliate wireless communication. As a demonstration, an Android smartphone was used to send movements commands to a Roomba.

The only special concern regarding Bluetooth was pairing. Pairing was accomplished using the Android smartphone's Bluetooth settings. To communicate, the smartphone searches through its paired Bluetooth devices for one matching the device ID of the JY-MCU chip, then transmits and receives through Android libraries.

Pairing From the Android Smartphone

The rigid radio packet structures were discarded in favour of a more simple format, given that Bluetooth communication happens over a serial port. Three functions were created to send Bluetooth data from the base station to the smartphone, listed below. Data formats are an ASCII character indicating the type of data being sent, followed by the appropriate number of data arguments matching the corresponding radio packet format.

void android_send_position_data(uint8_t roomba_id, int16_t delta_x, 
                                int16_t delta_y, int16_t alpha) {
    UART_print(ANDROID_BT_UART_CHANNEL, "p,%d,%d,%d,%d\n", roomba_id, delta_x, delta_y, alpha);

void android_send_sonar_data(uint8_t roomba_id, int16_t theta, int16_t distance) {
    UART_print(ANDROID_BT_UART_CHANNEL, "s,%d,%d,%d\n", roomba_id, theta, distance);

void android_send_bump_data(uint8_t roomba_id) {
    UART_print(ANDROID_BT_UART_CHANNEL, "b,%d\n", roomba_id);

Bluetooth communication functions

6.0 System Behaviour

6.1 Roomba

The Roomba logic of the mapping system had no precise timing constraints, so periodic tasks were not used. Instead, main logic was implemented in a round robin task, and specialized logic was split between system tasks that were activated by signalling on events. The tasks created are shown below.

Task Priority
task_main_logic Round robin
task_sonar_sweep System
task_read_packet System

The task_main_logic task was essentially the main loop of the Roomba, determining when the other tasks ran by signalling events when required.The task_sonar_sweep task completed a sonar sweep (full 180 degrees) and transmitted the sonar packets. The task_read_packet task was used to process incoming radio packets. The main task goes through the process below, signalling other tasks when they are needed.

Finite state machine for a Roomba

The only changes to the design from project 4 are the distance travelled and the addition of a special case for when the Roomba hits a wall. The distance travelled between measurements was reduced to 50cm from 1m because 1m left gaps between data points, while travelling only 50cm created a more complete map. When the Roomba hits a wall, it transmits a special bump packet to the base station. This is like a sonar packet, but indicates that there is an obstacle directly in front of the Roomba. Data points from bumps are displayed in red on the Android smartphone.

The number of sonar measurements taken was chosen to be 37 (-90 to 90 degrees in 5 degree increments). Originally, measurements were taken from 90 to -90 degrees in 10 degree increments, but this provided poor resolution when measurements were detecting obstacles far away. Measurements at each position were taken until three identical measurements were taken, then the servo was moved to the next position.

6.2 Base Station

The base station only had one system-level task, receive_task. This task continually listened for new radio packets and processed them accordingly. While the task is somewhat complex, there was no way to decompose it.

Task Priority
receive_task System

The main task of the base station switched on the type of packet received. It took action according to the process described below.

Finite state machine for the base station

One variation from the design in project 4 was the handling of bump packets sent from the Roomba when it hits a wall. These were handled like sonar packets, but the obstacle location is implied to be directly in front of the Roomba.

Another notable change is the algorithm used to determine the Roomba's next heading. The least recently visited algorithm described in project 4 was abandoned because position and sonar data was too erroneous for the algorithm to perform successfully. Once all the sonar readings are received, the base station scans through the readings in order and finds the largest group of readings that did not detect obstacles. The base station selects the middle of this group as the Roomba's next heading. This algorithm was fairly successful, but like least recently visited depended heavily on the sonar readings being accurate, just not to the same degree.

6.3 Android Application

The Android application was written using the Google Android Studio IDE on a Linux environment. To connect to the base station, the Bluetooth API was used to search through the smartphone's paired devices, searching for the pairing name linvor (the default for the JY-MCU device). Once found, a socket was opened to the device using the industry standard SPP UUID. Once created and connected, reading and writing to the Bluetooth device can be performed via Input/OutputStreams interfaces.

public boolean connect() {
    // All error checking code hidden for simplicity
    BluetoothAdapter adapter = BluetoothAdapter.getDefaultAdapter();
    for (BluetoothDevice dev : adapter.getBondedDevices()) {
        if (dev.getName().equals(bluetooth_pairing_name)) {
            btDevice = dev;
            Log.d("Roomba", "Connected to: " + dev.getAddress());
    // Get a Bluetooth Socket to connect with the given BluetoothDevice
    btSocket = btDevice.createRfcommSocketToServiceRecord(SPP_UUID);
    roombaInputStream = btSocket.getInputStream();
    // Start an asyncronous task to constantly read from the bluetooth input stream
    readerTask = new BluetoothReaderTask();

    return true;

To be able to read from the input stream without causing the UI of the application to become non-responsive, a background thread was implemented as an Android AsyncTask to call the blocking method. Based on a defined ASCII packet structure, the first character received indicates the format of data to follow. To signal the end of an packet the line-feed character '\n' is used. Based on the packet type, the Android application interprets the received packet and processes the data accordingly to update the UI.

// The sonar point position, relative to the roomba position and heading
double pX = Math.sin(Math.toRadians(theta + roomba.getHeading())) * distance;
double pY = Math.cos(Math.toRadians(theta + roomba.getHeading())) * distance;

floorplan.addSonarDataPoint(new SonarDataPoint(roomba.getX() + (long) pX,
                                               roomba.getY() + (long) pY));

Adding a new sonar data point

// The sonar point position, relative to the roomba position and heading
double pX = Math.sin(Math.toRadians(roomba.getHeading())) * Roomba.RADIUS;
double pY = Math.cos(Math.toRadians(roomba.getHeading())) * Roomba.RADIUS;

floorplan.addBumpDataPoint(new BumpDataPoint(roomba.getX() + (long) pX,
                                             roomba.getY() + (long) pY));

Adding a new bump data point

public class Roomba {
    void updatePosition(long deltaX, long deltaY, int heading) {
        // Update heading
        this.heading += heading;
        // Calculate new position
        this.x += Math.sin(Math.toRadians(this.heading)) * deltaY;
        this.y += Math.cos(Math.toRadians(this.heading)) * deltaY;
        this.x += Math.sin(Math.toRadians(this.heading - 90)) * deltaX;
        this.y += Math.cos(Math.toRadians(this.heading - 90)) * deltaX;

Updating a Roomba's position

The Android app was used to display the map data collected. It would plot data points sent from the base station and move circles representing the Roomba around. The following screenshot shows the app in use.

Android App with Map Data

7.0 Difficulties and Sources of Error

Each component was tested individually before system testing was conducted. While the project could create maps of some accuracy, the main issues encoutered were due to unreliable sensors. The following sections outline issues and unresolvable problems found during testing.

7.1 Radio

It is safe to say the at least half of the time spent on project 5 was related to the radio. Not only was it the most complicated chip used in terms of wiring, but the driver was also extremely complex.

Through extensive debugging, a defect was found in the radio driver given out to the class. The slave select (SS) pin was never set, causing the SPI module to not generate anything and the radio initialization to hang indefinitely. This bug was reported to the TA, and a fix was issued.

Another issue was discovered where ACK packets were not sent when packets were transmitted in a specific order. After sending the first heading request packet, the Roomba would receive the heading reply packet and move to its next position. Once there, the Roomba would try to send a position packet, but it would never receive an ACK for it, and the system would hang. The cause of this issue was never found; it was resolved by reordering the order of the radio transmit calls.

7.2 Roomba

While error was expected in the Roomba's wheel sensors, the degree of error was unexpected. The map below shows how the Roomba's heading data quickly loses accuracy over time. The map was generated from a straight hallway.

Straight Hallway Map Showing Position Error

During normal operation, the Roomba lists slightly to the left. It appears that the wheel sensors cannot or do not take this into account, however, as the Roomba thinks it is positioned directly upwards from the origin but is actually against the left wall. This inaccuracy was compounded over time, eventually producing the warped hallway picture in the map. It is estimated that after 5 sensing cycles, the error in the Roomba's heading is up to 30 degrees. Absolute distance data is a not much more accurate; after the same number of cycles, Roomba position data is about 1-2m off of the true position.

Error was expected because of the inaccurate nature of dead reckoning. It was assumed that the Roomba would be able to operate for longer without becoming prohibitively inaccurate, however. To counteract this, the Roomba's wheel sensors should be combined with a number of other sensors to corroborate their readings. An optoelectronic sensor like those in modern laser mice would be a good candidate.

7.3 Sonar

After working fairly well in projects 1 and 2, the sonar was expected to behave fairly regularly. Unfortunately, errors and blatantly inaccurate readings had to be dealt with.

The main problem with the sonar was its wide field of view. When the sonar was mounted on the front of the Roomba, the sonar would detect the ground after a distance of about 1m, so the sonar had to be raised.

When placed beside a straight wall, the sonar readings, when plotted, showed a curved wall instead. The curvature was regular and obvious enough to be noticed. The general shape of objects could still be determine on the map, however.

The sonar also gave completely erroneous readings once in a while, drastically overestimating distances. This was countered by making the sonar take measurements until it had taken three that were identical. Readings of more than 2.5m were also ignored, first because they could be errors or simply the sonar timing out, and second because resolution will be better if the Roomba waits until it is closer to obstacles.

7.4 RTOS

An RTOS was not entirely necessary to run the mapping system, and it may have been altogether easier to implement it as a giant loop as in project 1. Neither the Roomba nor the base station had timing constrains that an RTOS could help enforce, and both would have been perfectly well served by a single main loop. The base station in particular only has one task that loops continually.

8.0 Design Evaluation

The design work done in project 4 was quite helpful. The design specifications were also very thorough, with the only parts missing being details that required tuning of the actual implementation, such as the number of measurements in a sonar sweep. The thorough design made implementation into the simple transformation of specification into code, as opposed to previous projects, where design and implementation happened somewhat simultaneously. Since design considered the entire system as a whole, interfaces and expected behaviour were already well-defined and available when coding started.

The design was also almost entirely successful. The main reason for design changes was the unexpected degree of inaccuracy from the Roomba wheel sensors and sonar. No hardware changes were necessary, but some slight behavioural modifications were made. Communication protocols in the design were successfully implemented, with the addition of the bump packet mentioned earlier.

9.0 Demonstration

Opening the Android app and connecting to the base station can be seen the following video.

A demonstration of the Roomba mapping can be seen in the following video.

The map resulting from the above demonstration is shown below.

10.0 Appendix

Roomba/Base Station Source Code (project5.tar.gz)

Android Application Source Code (RoombaMappingApp.tar.gz)