Overview

VanGo is a differential drive robot for drawing SVG images. Put simply, VanGo is a wheeled mobile robot capable of trajectory tracking, with the added feature that the trajectory is generated from an SVG image and the robot holds a marker which is brought into contact with the page while the robot is on the desired path, thus drawing the SVG image.

Usage

A basic explanation of how VanGo is used and what goes on under-the-hood during general use is explained below. More in-depth explaination can be found is later sections of this site.

1. The user produces a CSV file containing (x,y) points from a SVG image using Coordinator
2. The user starts the vango-client program, providing the CSV file
3. A path of waypoints is generated from the CSV file and clustered into closed curves based on some threshold distance
4. The robot will navigate to the first point in the next closed curve
5. Lower the marker, then navigate to each additional point in the same closed curve
6. After passing through all the points in this closed curve, raise the marker and go back to step 4.

Control

VanGo's control architecture is comprised of a lower level speed control loop and a higher level trajectory tracking controller. From a user perspective, the lower level control loop can mostly be ignored, or at least treated as a black box and not modified, while the higher level controller may leave more room for customization.

Low-Level Speed Controller

Running on the ESP32 microcontroller, a timer based interrupt is resposible for reading the quadrature wheel encoders to compute the speed of the wheels. Experimentally it was determined that a simply proportional controller was sufficient for closed-loop speed control, although Proportional-Derivative (PD) control was also tested. Although PD control results is a smoother trajectory with less overshoot, the slower response was not worth the minimization of overshoot. Either way, both P and PD controllers work fairly well for closed-loop speed control, and if one is preferred by the user, the gains may be adjusted in the firmware and reflashed to the board. For more information on editing and flashing firmware, see Development.

The two images below show the speed controller response for both the Proportional and Proportional-Derivative controllers.

High-Level Trajectory Controller

The vango-client implements a trajectory tracking algorithm which is used to follow a trajectory defined by a set of waypoints.

There are many different trajectory tracking algorithms in existance, however at this time only one,and perhaps the most basic one,the PID controller, has been implemented for the VanGo robot. There are various ways a PID controller can be applied to the trajectory tracking problem, however the method I've chosen to implement is to use a PID controller for angular velocity control. To move the robot from its current (x, y) to some goal position which is presumably along the target path, the algorithm works as follows:

1. Read the current pose (computed from wheel odometry) via BLE
2. Compute the target angle as \(atan2(y_{goal} - y, x_{goal} - x)\)
3. Let the error be the minimum angle between the robots current heading and the target heading
4. Compute the control signal from the PID controller for the given error. This control signal is chosen to represent the angular velocity of the robot.
5. The desired motion of the robot is then represented as a 2D twist (body velocity), using the computed target angular velocity, a constant x velocity of your choice, and a y velocity of 0.0 (which assumes no wheel slipping)
6. Compute the left and right wheel speeds needed to achieve the target twist

During development, there was a choice to be made regarding whether the trajectory controller will operate at the client application level or the firmware level. Both options have positives and negatives and will be briefly discussed below. In the end, it was chosen to operate the trajectory controller at the client application level.

Client-Side Trajectory Controller

A client side trajectory controller works by implementing the trajectory tracking algorithm in the vango-client application. The controller will obtain the robots current pose using bluetooth low-energy (BLE) and compute the necessary wheel speeds for tracking the desired trajectory, and send those target wheel speeds back to the robot over BLE.

Pros

• Faster development time since you will not need to reflash the microcontroller
• Easier to test different trajectory tracking algorithms or enable several algorithms to be available as options to the user
• Can take advantage of a client machine with higher computing power than the microcontroller

Cons

• Control loop speed is limited by BLE read/write latency. In it's current state, loop speeds top out around 30Hz.

Firmware-Side Trajectory Controller

An alternative way to implement the trajectory tracking controller would be to implement the controller in the firmware on the microcontroller, obtaining the target path to follow from the client over BLE. Pros and Cons are listed below.

Pros

• The MCU is capable of more precise and consistant timing through the use of hardware timers and interrupts
• BLE communication latency is no longer a concern since waypoints could be sent and processed beforehand

Cons

• Resources are somewhat limited on the MCU and may impose practical limits of which algorithms can be employed
• Slower development time since for the most part, modifications to the trajectory tacking algorithm require reflashing the board

Although performance/timing of the control loop could potentially be better if implemented in the MCU, it was determined that easy customization of algorithms as well as development time are key aspects of the project, therefore a client-side trajectory tracking algorithm was employed.

Firmware

The firmware running on the ESP32 microcontroller is contained in the vango-firmware directory. The firmware is responsible for establishing Bluetooth Low-Energy (BLE) communications, reading encoders, computing wheel angles and speeds,and computing the robot's pose through wheel odometry.

Communication

The VanGo robot is controlled via a BLE server. Using the esp32-nimble crate with esp-idf-hal, several BLE characteristics are established. For those unfamiliar with BLE, a characteristic is simply a part of the server that a client can read/write data to. Each characteristic is given a unique UUID to identify it by.

Below are the BLE characteristics provided by the robot and some information about what they are for.

Left/Right Speed Characteristics (Read | Write):

• Read: When read from, returns the current wheel speed
• Write: When written to, sets the target wheel speed to the written value

Pose X, Y, and \(\theta\) Characteristics (Read | Write):

• Read: When read from, returns the current x,y, or \(\theta\) position of the robot
• Write: When '0' is written to these characteristics, the value is zeroed

Pen Characteristic (WRITE):

• Write: When '1' is written, raises the pen, when '0' is written, lowers the pen

Embedded Development

To set up a development environment to modify and flash firmware to the device, you will need to install several things such as the Espressif compiler toolchain, espflash cargo utility, and of course rust and cargo if you don't have that already.

The best way to get all this setup and make sure it works is to follow the directions on setting up an esp-idf-template project: esp-idf-template. If you are able to successfully build the template project, you should in theory be able to build and flash the vango-firmware without issue. In the vango-firmware/.cargo/config.toml file, there is an alias for "flash", which allows you to flash the board with espflash by simply typing cargo flash.

Performance

In its current stage, the VanGo drawing robot does not work well enough to draw consistent and accurace images, however with additional tuning, a more sophisticated trajectory tracking algorithm, and possibly additional sensors, the performance will improve. Manual mode, which enables a user to manually drive the robot and move the marker up and down works as desired. Below the results of several tests are shown.

Test 1: Manual Mode "Hi"

In the video linked below, I manually controlled the robot using the vango-client application to write "Hi". This worked fairly well, but as you can see, driving the robot smoothly with the current controls is a bit challenging.

Test 1 Video

Test 2: Automatic Mode "Triangle"

The video linked below performs a test of the robot in automatically mode attempting to draw a triangle. There is also a plot of the estimated x and y position of the robot from wheel odometry. As you can see the actual position of the robot is not very good, however the position estimate from odometry appears to be a bit better. This could potentially be due to wheel slipping.

Test 2 Video