How the controller is set up

In Yahboom’s original code, the controller interacts with the hardware, however, I decided to go for a different option. I decided that the controller should never actually touch the hardware. It doesn’t know about the registers, pins, or timers. It just asks for sensor data, monotonic time (never goes backwards), and then writes motor instructions through three small interfaces. Here is what it looks like:

Basic setup

// hal/interface/i_sensor_hal.hpp
class ISensorHal
{
public:
    virtual ~ISensorHal() = default;

    virtual float getAngle() = 0;
    virtual float getGyroBalance() = 0;
    virtual float getGyroTurn() = 0;
    virtual float getAccelZ() = 0;
    virtual float getBattery() = 0;
    virtual int getEncoderLeft() = 0;
    virtual int getEncoderRight() = 0;
    virtual bool poll() = 0;
};

// hal/interface/i_motor_hal.hpp
class IMotorHal
{
public:
    virtual ~IMotorHal() = default;

    virtual void setMotorPWM(int pwm_left, int pwm_right) = 0;
};

// hal/interface/i_monotonic_clock.hpp
class IMonotonicClock
{
public:
    virtual ~IMonotonicClock() = default;
    virtual std::uint32_t nowMs() const = 0;
};

Those are three separate headers, shown together here with the #pragma once and includes trimmed.

If you are wondering what the poll() bool is for, it reports whether a complete packet was accepted. If it’s false the controller skips the update entirely, and if false keeps coming back past a timeout it cuts the motors. This stops the car driving on a stale angle if the IMU ever goes quiet on it.

So why did I set it up this way? Essentially, I did this so I could swap in and out the implementation for the real robot hardware, and then have a basic simulation implementation with the physics required to test the controller. The controller doesn’t know which one it’s talking to, so I can test the controller setup without causing any hardware issues. Here is a rough layout of the overall setup below:

src/pid/, src/app/       pure C++ control logic, no hardware knowledge at all
        |
hal/interface/           sensor, motor and monotonic-clock contracts
        |
   +----+----+
hal/stm32/   hal/sim/    real hardware   |   physics model
        |
bsp/                     Motor, Encoder, Imu, Battery, Usart as C++ classes
        |
FWLib/, CMSIS/, DMP/     ST's and InvenSense's C drivers, left as C

One thing I made sure to do was reuse the vendored C drivers, as rewriting those in C++ would have been a bit of a nightmare and I saw no real benefit. Keeping them byte-identical means I can re-download ST’s or InvenSense’s code at any time without losing anything. However, this means that hardware-specific fixes have to go in bsp/ or hal/stm32/ rather than becoming private edits buried inside one of the vendor files.

Was the simulation useful?

Overall yes. Not only did it allow me to check how the controller stabilised in simulation without causing my real hardware to go flying, it did help with a fault that occurred. At one point on the real hardware only one wheel was working. The simulation told me the controller itself was behaving, and the interface boundary did the rest: IMotorHal takes two numbers and has no way to name a specific timer channel, so neither it nor the control logic is capable of causing a one-wheel fault. From there I could focus on the bottom two layers of the setup, and it was in the last file that touched the timer (it was just a missing initialisation).

It is worth adding that there are really three implementations rather than two. The tests have a MockMotorHal that just records whatever it was told, so a test can assert on it.

How the SBC board fits in

At the time of this I am still planning on using the Radxa Q6A. This board will handle the higher-level movement, but it won’t replace the controller. The STM32 still covers the fast loop and keeps the robot upright. The way I have this setup means that if the Q6A side crashes or goes haywire, the worst it can do is send a bad setpoint, as it has no route to the PWM register at all.