Hello. I’m rotarymars.

The hardware side of our robot has already been covered quite a bit in K10-K10’s article about the PCB and utsukushiioto’s article about FEETECH. So this time, I’d like to give a rough overview of where the program of our 2027 robot stands right now.

Writing about everything would make this very long, so this time I’ll only cover the overview.

Also, all of the programs are still in development, so this is just their current state. If there are any big changes, I’d like to write another article about them.

Overall structure

First of all, there are two main computers on the robot.

The Raspberry Pi never touches the motors or sensors directly; it controls them by sending commands to the ESP32 over USB serial (UART).

The repositories look like this.

By the way, up until last year the program on the Raspberry Pi was written in Python, but this year we rewrote everything in C++. We wanted it to run faster, and since machine learning models can also be run with LibTorch (the C++ version of PyTorch), we wanted to unify everything into one language.

The reason the Raspberry Pi program is private is that, for ease of management, we keep things like the images used for machine learning in the repository this year, so we decided not to publish it (partly because something unintended might appear in those images).

The ESP32 program

Let’s start with the simpler one, the ESP32.

The ESP32 is in charge of the following parts.

Communication format

The Raspberry Pi and the ESP32 talk in a rather simple way: they send text commands to each other, one line at a time.

send:  12 MOTOR 3000 3000
reply: 12 ok

send:  13 TOF f
reply: 13 ok 210 205

send:  14 SWITCH
reply: 14 ok on

The number at the beginning is an ID, and the Raspberry Pi assigns a new one to every command. By checking that the ID of the reply matches the one that was sent, we keep the exchange between the two consistent.

The reason we do this is that the message the ESP32 prints right after booting (ID 0), or a late reply to a command that had already timed out, used to get mistaken for the reply to the current command. With IDs, nothing goes wrong even if the replies get out of step.

These are the commands.

About the wheels

This year we use serial bus servos called STS3032 for the wheels. Unlike normal motors, you can connect several servos to a single line and send commands to each of them by ID.

Also, the STS3032 keeps reporting its shaft angle (0 - 4095 per revolution) even while spinning as a wheel, so we read it every 10 ms and add it up to measure how far the robot has moved (this is what ODOM returns).

About the ToF sensors

Every VL53L0X has the same I2C address (0x29) when powered on. If you just connect four of them, they collide. So at boot, we first pull all the XSHUT pins LOW to put them to sleep, then wake them up one by one and change their addresses to 0x30, 0x31, and so on.

A sensor that fails to initialize is put back to sleep, and reading it returns -1.

The Raspberry Pi program

Internally, it switches between two modes: “line trace mode” and “rescue mode”.

Cameras

We use two cameras.

The cameras are controlled by calling libcamera directly from C++. There is also a mechanism similar to the pre_callback of picamera2 we used in Python last year, but for a reason I’ll explain later, we don’t use it now.

Line tracing

We gave up on machine learning

Actually, at the beginning of this year we were trying to do line tracing with machine learning.

Specifically, a human drives the robot like an RC car while recording pairs of “camera image” and “motor values at that moment”, then a small CNN is trained on them to output the motor values directly from the image (this is called behaviour cloning).

However, about 30 minutes of driving wasn’t enough for it to learn well, and I also felt that with machine learning, there would be no way to debug it if it suddenly stopped working, for example at the actual competition.

So we decided to rebuild an algorithm-based line tracer in C++, based on last year’s Python algorithm.

The control loop

Line tracing repeats the following in a 20 Hz loop.

1. Once a second, check the switch (if it’s OFF, stop and wait)
2. Take one image from the camera
3. Read the heading and distance travelled with ODOM
4. Decide the left and right motor values from the image
5. Send MOTOR
6. Pass the image to the silver tape detector (explained later)

At first, we did the image processing inside the camera callback, but then the processing ran at the camera’s frame rate, and checks like “if this happens for N frames in a row” changed their meaning depending on how fast the loop ran. So now we take the image inside a normal loop instead.

Also, heavy work like step 6 is always placed after the motor command is sent, so that nothing extra sits between a decision and the motors acting on it.

How it decides from the image

The rough flow is as follows.

1. Binarise the image to make a mask for the black line and a mask for green
2. Find the green marks and figure out which way to turn
3. Erase the lines we are not going to take from the image
4. Head towards the “far end” of the remaining line

Usually, green marks are handled as a special case for turning, but in our program, once the green marks say to turn right, the lines to the left and straight ahead are erased from the image. Then the line-following step afterwards doesn’t need to know anything about turning; there is only one line left to see, so it goes the right way. This carries on the idea from last year’s program, but we optimised the part that removes unnecessary lines and tuned it so that only the line that is really needed remains.

Judging green marks

This year’s line trace camera is mounted quite low, so a green mark fills about 1/5 to 1/3 of the frame, and the robot can’t see the whole intersection.

So instead of looking at the mark itself, we look at the floor around the mark. The black line divides the floor into several regions, and if the region the mark is in

• touches the bottom edge of the frame on only one side of the line we came in on → it’s a mark before the intersection; turn that way
• doesn’t touch the bottom edge → it’s a mark after the intersection; ignore it
• touches the bottom edge on both sides → can’t tell

This works the same way even if the robot comes in at a slight angle.

Also, instead of deciding from a single frame, the robot only turns once the same verdict comes out for several frames in a row. The marks disappear under the robot before it finishes the turn, so once it commits, it keeps turning for 1.5 seconds.

Marks on both sides mean a U-turn. Deciding when to stop spinning is really hard for a U-turn: since the robot is spinning on an intersection, “stop when a line is in front” makes it stop after just 90 degrees. So the robot counts how far it has turned from the change in direction of the lines in the image, and stops only after turning at least 150 degrees and seeing a line ahead.

Where to head

The robot steers towards the “far end” of the line.

The “far end” here doesn’t mean the top of the frame, but the point farthest along the line. At a right-angle corner, the highest point in the frame is the corner itself, but the farthest point along the line is the arm going off to the side, so the robot can face the right way.

When there are several ends (at an intersection, for example), it picks the one closest to the previous direction of the line.

When the line disappears

At a right-angle turn, the line almost always leaves the frame once, so losing the line is completely normal.

Even when it loses the line, it doesn’t stop:

• if it’s in the middle of a turn, it keeps turning
• if the line has a break (a gap), it goes straight in the direction the line was pointing

Telling a gap from a corner is tricky here, but we distinguish them by whether the robot was turning hard just before.

Detecting the silver tape

There’s silver tape at the entrance of the rescue area, so when the robot sees it, it switches to rescue mode.

This is judged by a small CNN that takes an image shrunk to 80×60. We also wrote our own labelling and training tools for it in C++.

This detector runs on a separate thread, only once every 200 ms, so it never slows down the line trace loop. When it says “silver” twice in a row, the robot beeps and switches to rescue mode.

Rescue

Detecting balls and more

From the rescue camera’s image, we detect the following five classes with YOLO.

The YOLO model is trained in Python, exported to a format called TorchScript, and loaded with LibTorch in C++. Since it runs on a Raspberry Pi, the input image is kept small at 160×160. This also runs on a separate thread, at about 13 fps.

Managing the state

During the rescue, the robot keeps track of what it’s currently doing as a state, and changes how it moves accordingly.

In APPROACH, the motor values are set in proportion to how far the detected object is from the lower centre of the frame (proportional control).

In EXIT, the robot follows the wall while keeping a constant distance from the left ToF sensor; if there is a wall ahead, it turns left by 90 degrees, and if the left wall disappears, it turns left and moves on. Whether it has turned 90 degrees is judged by the heading from the BNO055.

Also, after 3 minutes, it is forced into the EXIT state.

CATCH and RELEASE are still empty, and the ball-grabbing part is what we’ll build next. Also, the part that actually enters the rescue process after the line trace finds the silver tape isn’t connected yet, so I want to fix that soon too.

Other tools

Besides the program on the robot itself, we’ve made several tools for development.

The viewer is especially useful. When the robot gets an intersection wrong, we can take those images and check, one frame at a time, “how did it judge the green mark?” and “which line did it erase?” It runs exactly the same line trace process as the robot, so the robot and the viewer never disagree. Most of the line trace parameter tuning is done while looking at this.

However, since it uses recorded images, changing the command doesn’t change how the robot moved. So it can tell us whether the robot reacts “sooner and harder” to those images, but whether that actually gets it back on the line can only be found out by running it on the course.

Also, building on the Raspberry Pi is very slow (a single file that uses LibTorch takes about 1 GB of memory to compile), so we made it possible to cross-compile for the Raspberry Pi on a PC. We create a Docker image from the Raspberry Pi OS image, copy the headers and libraries out of it, and build for arm64 with clang on the PC. It’s much faster than building on the Raspberry Pi and finishes in seconds.

Finally

This time, I gave a rough overview of where the program of our 2027 robot stands right now.

Since this was an overview, I hope to write about each part in more detail in another article.

I’d be happy if the team members could also read this article and get a feel for the program as a whole.

Thank you.