Down the Rabbit Hole

Wow. A year ago my family and I were spending time with my parents for Thanksgiving. The idea of Godfrey appeared somewhere in between great family times and the wonderful food. I immediately knew I wanted to commit a couple of years attempting to build him. Over this last year my goals for Godfrey have not changed, however one small event caused me to take a major detour and do some significant exploration into different architectural designs. I believe the results have been worth it, but it has keep me from having any direct robot progress to write about.

The detour began last April as I was writing my PID control algorithms. The algorithms themselves are not difficult but they do require parameterization. I knew that didn't want to tweak the values, load up a new version, try it, and iterate. I instead wanted real-time control of the parameters. I also wanted a log of all commands (stop and balance, move to the left, etc.), sensor data (accelerometers, gyroscopes and rotary encoders) and motor direction (PWM pulse widths) in one central place. Having this data would not only make the internal workings more transparent, but it would allow post-run analysis and support comparison of different algorithms and/or parameterization. Since the sensor are sampled 50 to 100 times a second, I needed an approach that could handle these speeds over many minutes. Ideally it should handle this sampling rate for extended periods of time.

My ideal vision for this logging and control system is a high speed pub/sub system supporting many individual tracks. I see it as a 2d graph where the Y axis represents each pub/sub channel and across the X axis is time marching on. Individual components of Godfrey can read and write events in any track of interest. In the image above, I show a command track that is written by some higher level function, the PID algorithm takes the command plus the data from the gyroscopes and accelerometers and it outputs the speed and direction of the left and right motors.

Having a pub/sub at the core of Godfrey has other impacts in the design. I have spent the last many months fleshing out the details to determine what kinds of hardware to use, how to develop the distributed timeline system, what languages to write my major components in, and what planner technology to use. This has lead to examination of many different hardware and software systems including: Arduinos, PandaBoards,Raspberry Pis, BeagleBone Blacks, C/C++,
Erlang, Clojure, Go, Python, Ruby, UIMA, Kafka, Redis, ROS, ChatScript, HAP and POSH.

Future posts will have to discuss the details of the full design. However, some of my conclusions are:

Arduinos are amazing devices. I love their low cost, all those lovely digital and analog I/O pins and the great built-in library support for I/O devices. But in my opinion their networking is quite not as pleasant as the rest of the system. Any low cost networking options would require me to roll-my-own hardware and software stack or I can use a Ethernet/WiFi shield with its so so network library. So instead of using Arduinos I have decided to use BeagleBone Blacks. They have similar I/O capability as Arduinos, but have a much more powerful processor and support a full Linux stack with its networking. They are also modestly priced. It is truly a joy to be able to write normal Unix programs and have it control low level I/O. It is also very cool that the same platform has the power to run complex speech analysis, image processing, and AI software on the same platform.

The use of BeagleBone Blacks (BBB) open up the languages I can program in. I know that there will be some C/C++ in Godfrey to link to libraries I will be using like PocketSpinx, Flite, and OpenCV. However, for higher level reasoning tasks and the robot planner I plan to use Go. I benched its speed against Clojure (another language I love) and Go is substantially faster on the BBB. Pragmatically, I know that I should have chosen Python because it is already supported by BBB and is commonly used by the robotics community. But for some reason I have never enjoyed programming in Python. In the end, Godfrey is my design and I just don't want to face the prospect of writing a lot of Python.

My experience using Redis, DynamoDB and SQS has convinced me of the power of simple and lightweight network APIs against simple data services. Store the data blocks in some common data exchange format such as JSON and you have a very power interconnect strategy. This strategy allows different components, running on different servers, written in different language, built in totally different epochs of time, by teams of different programming ability to all peacefully work together. As much as I wanted to like and use ROS, I find its programmatic API rigid, complex, heavy. It is in stark contrast to these competing network API services. This complexity and heaviness means that ROS only supports a limited number of languages. I have decided that I want to use a Redis-style network service supporting a pub/sub data model that blends the best of Redis and Kafka's.

I had previously believed I would construct a HAP-style reactive planner to control Godfrey. However, in the last months I have discovered POSH. I have been amazed at the simplicity of its overall design while supporting the rich features commonly found in other reactive planners. POSH also is very well suited to systems that connect complex self-contained competencies such as the balancing system, speech recognition, etc. with the POSH planning system. POSH is also compatible with my desire to use a distributed pub/sub system. It will take a lot longer post to explain what I see in POSH and how I plan to use it.

I have been doing many experiments to test out these ideas and how well they work together. To speed up end-to-end development I have created a experimental testbed on top of an iRobot Create. It uses the Create for movement, collision detection, and power. Added to it are some cheap switching power supplies to convert the unregulated 18 volts to 5V, a BeagleBone Black, serial port level converters, and a Wi-Fi Ethernet Hub. All the hardware is up and running under software control over my home WiFi network. I am now working on the software architecture discussed in this post. Eventually I will adding another BBB, a USB camera, mic, and speaker to support speech and image processing.

I will be outlining my progress in future posts.

New Current Measurements

My previous post has a graph showing erratic motor current measurements. To stablize these measurements I added code that calculates the running average over the last 50 samples (one full second of measurements). I reran the previous experiment and here are the results:

I have no idea why the right motor (green) is taking so much less current than the left (red). I am happy that it is at least consistent with the right motor running at a lower speed than the left. I also don't understand the asymmetry between forward and reverse in both motors. I will ponder these issues more. I will make sure that Godfrey's software manages them.

My next step is to attach wheels to the base and a wooden pole on the top to bring Godfrey up to three or four feet tall. Once completed I will start writing and testing the code to balance Godfrey. Things are starting to get fun.

Testing Results

I have been able to verify most of Godfrey's hardware components are working correctly using the test software described in the last post. The one exception is the motor fault indicator which I didn't test because I was uncomfortable shorting the pins to see if it worked.

The IMU was already working and tested previously. This data is passed straight through to the status report.

To test the odometer I manually spun the rear shaft and saw the odometer counts increased and decreased appropriately. This count was seen in the status output. I tested each motor independently and they each worked.

Next I tested the motors themselves using the 'm' command. I was able to move the motors forward and backwards on command. Each motor worked independently and changed speed and direction depending on the value specified. However it was immediately obvious that the motors were not running at the same speed for the same input value. To determine the exact behavior I ran a series of tests in the forward and backwards direction and recorded the results. Below is a graph of the actual speed as measured by the odometer based on the input value. The right motor is in green and the left motor is in red and the odometer was sampled over 1 second periods.

It is obvious from the graph that the right motor is slower than the left motor. For example, at the maximum forward speed of 400 the left motor's odometer is 10392 over one second and the right one is 9563 (around an 8% difference in speed). To determine whether the speed difference is the motors or the driver chips I flipped which driver drove which motor and reran the tests. To two motors showed almost exactly the same odometer readings for each input value. So it seems that the issue is with the motors and not the drivers.

This graph also shows that the motor response is not linear to the input values requested. As we approach the maximum value the velocity tapers off. The right/left differences along with the non-linearity will have to be considered when creating the balancing algorithm. It is very important to know the maximum speed the motors can go so Godfrey doesn't attempt to exceed this speed and ultimately fall over.

I monitored the current used by each motor and plotted it in a graph similar to the odometer graph. Here it is:

I am not exactly sure what to make of this graph. I noticed that the current for both motors was not very steady from one reading to the other. But it is pretty interesting that the right motor, which is the slower one, had a much poorer behavior. Perhaps the right motor just had a bad choice of samples. Maybe if I averaged the values over several readings the graph will make more sense. That is my next step and I will report the results in another post.

Testing Software

It is now time to build the software necessary to drive and test the hardware. Each piece of hardware in the robot base must be initialized and then either read or written in a specific manner. Ideally, the software should be simple to use and understand.

The complete test program can be found here on GitHub.

Test Interface

The test software is designed to read commands from Ardunio's serial port and write status back to this same port. The port is set up at 115200 baud.

Unless otherwise specified, the software periodically reports the status from the IMU as well as the motor current and odometer counts. The format of this status message is:

 46 -151 -291 43 -31 17 418 304 14348 12947

The numbers are the the accelerators x, y, z, the gyros x, y, z, the left and right motor currents, and the left and right odometer readings from the rotary encoders.

This port accepts several input commands:

m left-motor right-motor    Set left and right motor speed. The values are decimal numbers between -400 and 400. Positive values move the motor in the forward direction, negative in reverse, and zero stops the motor. Missing arguments default as zero. An 'm' without any arguments stops both motors.

f report-divider    Set the frequency of the status reports as a multiple of 50Hz. It defaults to a report every five seconds. Setting this value to 50 will report once a second.

r    Toggles the status reports on and off.

How the Hardware is Programmed

The program in the IMU hardware was discussed in a previous post. The data outputted by the IMU is passed straight through this system into the status message.

The motor controller outputs a voltage proportional to the current being consumed by each motor. Godfrey uses two analog ports to read these voltages and then multiplies them by a constant to convert them into values that represent the milliamps used.

The motor controller outputs a signal when the H-bridge is overheating either due to driving the system too hard or because of a direct short. This error condition is detected by digital pins and reported as an error.

The odometer measures how far each wheel rotates over a specific period of time. The actual value is calculated by decoding the 2-bit gray code signal supplied by rotary encoders attached to the shafts of the motors. Each motor's odometer count is updated by adding a delta value generated through a lookup table indexed by the rotary encoder current 2 bits along with the last 2 bits.

Finally, there is the actual motor control itself. The Arduino can generate the PWM signals necessary to drive the motors. So the test program initializes the necessary timers and uses the Arduino to create a 20KHz PWM signals sent to the driver chips. The Arduino also specifies the motor direction. The speed value can ranges between -400 to 400.

The next post will outline the results of testing.

Two Wheel Base

With the IMU ready to use it is time to make the rest of Godfrey's wheel base.  Godfrey's Phase I motor base is a two-wheel balancing design. It contains all the elements necessary to detect the robot's position and balance and move the robot in its environment.

Here is a block diagram of the system:

At its heart is an Arduino Leonardo board. Connected to this board's serial port is the Sparkfun 9 Degrees of Freedom IMU programmed to continuously report the accelerator and gyroscope data. Also connected to the Arduino through PWM lines is the Pololu two channel motor driver which itself is connected to the drive motors. The motor current reporting component of the motor drivers are feed back to two Arduino analog ports. The motors also have built in rotary encoders and their signals are feed to the Arduino to measure rotation. A full schematic can be found here on Upverter.

It is constructed on a wood base using wood screws and hot glue. Here it what is looks like:

With all of this hardware, the base can:

1. It can move with left and right wheel independently in both the forward and backwards direction.
2. It accurately measures wheel rotation. This is used to accurately determine the ground path. It also helps balance the natural variations in motor speed.
3. The base uses it gyroscopes to determine how fast it is pitching in any direction. This is use to balance the robot.
4. It measures acceleration in all directions. Its primary use is to determine when Godfrey is upright. The gyroscopes are the primary actors in balancing, but they drift. The accelerometer measure the downward direction in a stable manner.
5. It is able to communicate with other computer elements through its serial port.
6. It measures the current usage of each motor. I am not sure I will use this.

A details parts list can be found here.

I will discuss the testing program and what I have learned in the next post.

IMU software

The code for Godfrey's IMU can be found here on GitHub.

This code is intentionally simple. It initializes the accelerator and gyroscope chips and then queries their values at 50 times a second and outputs the results. The values are output in the order of accelerator X, Y, Z followed by gyroscope X, Y, Z. All values are signed 16 bit two's complement quantities.

The output looks like:

  252 108 -63 44 -30 17 

This code currently outputs data at 50Hz and 57600 baud. Both these may be too slow for the operational system.

To burn this code into the device I used the FTDI Basic Breakout connecting the IMU to a serial port. Set the Arduino IDE device (>Tools>Board) to Arduino Pro or Pro Mini (3.3V, 8 MHz) w/ ATmega 328.

I had a problem programming my device with the FTDI Basic Breakout. I would hit upload in the Arduino IDE and it would hang or I would receive an error message. I discovered (through web searching and some experimentation) that I could program the device if I started the upload and then hit the reset button on the IMU.

Use FTDI Basic Breakout and connect the device to a serial port and watch the values as you move the device. Set your terminal program to 57600 baud to see the results.

The Need For Balance

Umashankar Nagarajan's thesis explains why robots in human environments should be dynamically balanced. Simply put, it is because dynamically balanced robots can be of human-like heights and still move nimbly in human spaces whereas statically balanced robots require larger bases to support any given height. To be able to balance, Godfrey must have an Inertial Measurement Unit (IMU) to measure the linear accelerations and angular changes as he moves. I choose to purchase a Sparkfun 9 Degrees of Freedom - Razor IMU.

In addition to a 3-axis accelerometer, 3-axis gyroscope and 3-axis magnetometer (to find north) the IMU has a built-in 8MHz ATmega328. This allows embedded software that initializes the sensors and then communicates their values through a serial port. Sparkfun has preloaded it with some firmware goodies that make it immediately usable. They also supply links to other projects with additional software you can load into the IMU. I connecting the IMU to my Mac using a FTDI Basic Breakout.

After playing with the Sparkfun firmware, I read and loaded the software from AHRS/Head-tracker using 9DOF Razor IMU. Even though I am not interested in a head tracker, its software is extremely helpful for getting a feel of the IMU and where the limitations are in these devices. I highly recommend their graphical test app. It lets you move the device around and see the results of the gyros on your screen. One important thing to notice is that the gyroscopes drift over time and will not come back to the exact same starting location after many motions.

The webpage also explains how to calibrate your IMU. The reason to calibrate becomes clear as you do the process and read their source code. These devices do not give the same output value for the same physical input. For example, if you point one accelerometer axis straight down you can measure the 1G pull the earth is making on the accelerometer. If you invert the axis 180 degrees, you measure its inverse. Ideally, these numbers would have the same magnitude of input (1G in this case). Sadly my device gave 258 and -270 for the X axis, 282 and -248 for y and 221 and 294 for the z. On some of these axes there is a discrepancy of over 20%. Looking at the head tracker code you can see them use some simple algebra to compensate for the differences using calibration values.

Eventually I will carefully go over each of the spec sheets for these devices to better understand their behavior. For now I have a basic sense of what they do and their limitations.

The next post will talk about the software I wrote for Godfrey's IMU.

Lessons Learned

• Gyroscopes drifts over time and therefore cannot be the sole input used to balance Godfrey. If it was used as the sole input, Godfrey would start out vertically and slowly drift and fall over as the errors in the gyro added up. I will need to use an accelerometer to measure the direction of the center of the earth and use that to compensate for the gyro drift.
• Any algorithms I use will either have to handle the innate errors in the sensors or I will have to calibrate to the specific instances of the chips I am using. I would prefer handling the innate errors because that will be easier to build and deploy.
• Having the built-in ATmega328 is great for learning and testing, but in the final product it is unnecessary. Having the CPU adds an unnecessary piece of code to to written and programmed into the device. It also consumes a serial port on the motor base CPU. This CPU should be able to talk to these chips directly and still have more then enough computer cycles to handle its other tasks.