Build a Tabletbot – Robohub
There is a shortage of entries in tablebot competition just before the RoboGames 2023 registration window closes. To make sure the contest will be held, I entered the robot. Then I have to build it.
What are tablebots?
A tablebot lives on a table. There are three “phases” to the competition:
- Phase I: Build a robot that walks from one end of the table to the other and back.
- Phase II: Have the robot push blocks off the table ledge.
- Phase III: Have the robot push the blocks into a shoe box attached to the end of the table.
There’s also the unofficial Phase IV – which is falling off the table and surviving. I didn’t try this phase.
Most tablebots are fairly simple – a few sonar or IR sensors and they roam around the table hoping to complete the different phases. My tablebot is definitely different – and it paid off when it won a gold medal at RoboGames 2023.
The entire robot is made from 3D printed parts and random stuff that I have.
I’ve had one of those $99 LD-06 lidars for a while, and decided it was a great project to take on. I use a Dynamixel AX-12 servo to angle the laser so I can find a table, cube or goal.
All code runs on STM32, on my custom Etherbotix board designed for me Maxwell Robots a few years ago. The robot uses differential drive with a 30:1 12V multiple gear motor, which was purchased from Lynxmotion in 2008 and used in various firefighting robots over the years.
A small digital Sharp IR sensor set is used as the cliff sensor. It can be moved up or down to calibrate different tabletops using a pair of adjustment screws. While the sensors are very accurate and stop the robots, they don’t see far enough ahead when traveling at full speed, so I also use a laser to detect when the edge of a table is approaching.
Stage 1 Software
Phase 1 is pretty straight forward – and mostly based on dead reckoning odometry:
- The laser tilts down looking at the table. It does this by projecting the scan onto a 3D point, and filtering out anything not in front of the robot at table height. When the table disappears (point count drops too low), we reduce our maximum speed to something safe for the cliff sensors to detect.
- While the laser sensor searches for the edge of the table, the robot moves forward, and a simple feedback loop keeps the robot centered on the table using odometry.
- When the cliff sensor finally triggers, the robot stops, steps back 15 centimeters, then rotates 180 degrees – all using dead reckoning odometry.
- Maximum speed was then reset and we headed off to the end of the table in the same manner.
Phase 2 Software
Movement Phase 2 is basically the same as Phase 1 – we move forward, staying centered with odometry. The speed is slightly lower than Stage 1 because the laser is also looking for blocks:
- Laser scans are projected into 3D, and we filter each point that is part of the table by height. These remaining points were then grouped and the groups analyzed for size.
- If a cluster is a good candidate for the block, the robot turns towards that block (using, you guessed it, dead reckoning from odometry).
- The robot then advances towards the block using a simple control loop to maintain heading.
- As soon as the block arrives, the robot drives straight until the cliff sensor is tripped.
- At that point, the robot stops the wheel on the side of the cliff sensor that it tripped over and moves the other wheel very slowly forward so that we align the front of the robot with the edge of the table – ensuring the block has been pushed off the table. .
Phase 3 Software
The last phase is the most complex, but not much. Like the previous phase, the robot moves under the table to find a block:
- Unlike in Phase 2, the robot approaches the pose right behind the block.
- Once the pose is achieved, the robot tilts the laser back to level and finds its destination.
- The robot then turns towards the goal in the same way it first turned towards the block.
- The robot then approaches the goal using the same simple control loop, and in the process ends up pushing a block into the goal.
All software for my Tablebot is available GitHub.
Jim Dinunzio, a member of the Homebrew Robotics Club, took a video during the actual competition at Robogames so you can actually see the winning streak:
To make development easier, I also wrote a Python GUI that renders tables, robot odometry traces, laser data, and detected targets and cubes.
Fun with math
Along the way I actually came across a bug in the ARM CMSIS DSP library. I use
arm_sin_cos_f32() function to calculate my odometry:
arm_sin_cos_f32(system_state.pose_th * 57.2958f, &sin_th, &cos_th);
system_state.pose_x += cos_th * d;
system_state.pose_y += sin_th * d;
system_state.pose_th = angle_wrap(system_state.pose_th + dth);
This function takes the angle (in degrees!) and returns the sine and cosine of the angle using a lookup table and some interesting interpolation. With the visualization of the robot’s path, I noticed the robot’s odometry would occasionally jump sideways and backwards – which made no sense.
Further investigations show that for very small negative angles,
arm_sin_cos_f32 returns a large value. I dug deeper into the code and found that there are several different versions out there:
- The version from my old STM32 library, has this particular problem with very small negative numbers. The same bug still exists in the official CMSIS-DSP on the arm account.
- The current version in the STM32 library has a fix for this place – but that fix then breaks the functionality for the entire quadrant!
The problem turned out to be quite simple:
- The code uses a 512 element lookup table.
- For a given angle, it has to interpolate between the previous and next entry in the table.
- If your corner is between the 511th entry and the next entry (which will be the 0th entry because it’s wrapped), then you use the random value in the next memory slot to interpolate between them (and to calculate the interpolation). At one point, this resulted in sin(-1/512) returning an outrageous value like 30.
With that bug fixed, the odometry worked flawlessly afterward. Turns out, I had the same function/bug present in some brushless motor control code at work.
The awesome RoboGames is back! This little robot won’t appear again, but I’m starting work on a RoboMagellan robot for next year.