BLDC Controller phase 5: control

In the last post in this series I had just moved the Pico-BLDC code to C but it still wasn’t a fully-fledged motor controller. It would just run the motors at full speed forwards or backwards, keeping the phase of the magnets in lockstep with the rotor.

To make the driver useful, the next step was to add a control loop. To control a BLDC motor’s speed efficiently, the control loop needs to do a few things:

  • Keep the phase of the magnetic field 90 degrees ahead of or behind the position of the motor.
  • Vary the strength of the magnetic field to vary the force (and hence the torque).
  • Decide how much torque is needed. The simplest option would be to just control power like a throttle but I wanted the Pico to govern the speed of the motor.

I already had code to control the phase, so how do I control the throttle? All that’s needed is to apply a scale factor to the PWM values. Scale them all down and the effective voltage going to the motor goes down along with it’s torque.

How do we decide what throttle to set? I started with the absolute simplest approach just to prove to my self that it’d work:

  • Measure motor speed by comparing current sensor reading to last and dividing by time taken.
  • If going too slowly, add 1 to PWM scale factor.
  • If going too fast, subtract 1.

This worked (poorly as expected) but it proved that I had all the parts needed.

The next step was to implement PID control, I started with proportional control and iterated until I found something I was happy with. Really felt like I was getting somewhere at this point!

Reading the BNO08x IMU (from Golang)

We’re very fond of having an IMU on the robot. In the past we’ve used an IMU with a rate gyro, which tells you how fast the unit is rotating at any given instant. You then have to integrate the rate to get the actual angle. The problem with rate gyros is that any error in the rate gets integrated too, which means that your compass gradually drifts. This is a pain to deal with!

When we were looking for an IMU this time, Lance spotted Adafruit’s BNO08x breakout board; the chip it contains is actual magic. It has a rate gyro inside, but it runs all the integration and error cancelling onboard.

Even better than that, the BNO08x has an “easy mode” called UART-RVC, which stands for “Remote Vacuum Cleaner” mode. If you’re building something that looks like a remote vacuum cleaner (i.e. ground dwelling robot), you can put it in RVC mode by pulling a particular pin high and then there’s no configuration or callibration required. Simply connect the board to the Pi’s UART and it will send a 3-axis heading and accelerometer reading 100 times a second.

We’re reading the packets from the UART from Golang using the `”” library. After enabling the Pi’s GPIO UART using the device overlay, we open the serial port and wrap it with a buffered reader for efficiency and to allow peaking ahead:

mode := &serial.Mode{
	BaudRate: 115200,
s, err := serial.Open("/dev/ttyAMA0", mode)
if err != nil {
br := bufio.NewReader(s)

The packets from the chip always start with two bytes 0xAA 0xAA, so we wait for those; peaking ahead 2 bytes and then discarding 1 byte if we don’t see the correct bytes:

for {
	buf, err := br.Peek(2)
	if err != nil {
	if bytes.Equal(buf, []byte{0xAA, 0xAA}) {
	_, _ = br.Discard(1)
fmt.Printf("\nGot packet start.")

Then we loop, reading the 19-byte packets. We check each packet to make sure it starts with the header bytes and ends with the correct checksum…

const packetLen = 19
buf := make([]byte, packetLen)
var i int
for {
	_, err := io.ReadAtLeast(br, buf, packetLen)
	if err != nil {
	if !bytes.Equal(buf[:2], []byte{0xaa, 0xaa}) {
		fmt.Println("Lost sync.")
		goto resync
	//fmt.Printf("Packet: %x\n", buf)
	var checksum uint8
	for _, b := range buf[2 : packetLen-1] {
		checksum += b
	if buf[18] != checksum {
		fmt.Printf("  BAD CHECKSUM %x != %x\n", buf[18], checksum)
		goto resync
	var report IMUReport
	report.Index = buf[2]
	report.Yaw = int16(binary.LittleEndian.Uint16(buf[3:5]))
	report.Pitch = int16(binary.LittleEndian.Uint16(buf[5:7]))
	report.Roll = int16(binary.LittleEndian.Uint16(buf[7:9]))
	report.XAccel = int16(binary.LittleEndian.Uint16(buf[9:11]))
	report.YAccel = int16(binary.LittleEndian.Uint16(buf[11:13]))
	report.ZAccel = int16(binary.LittleEndian.Uint16(buf[13:15]))
	if i%10 == 0 {
		fmt.Printf("Report: %s\n", report.String())

It took me days to bring up the last Gyro we used, this one took about 30 minutes and its accuracy/resistance to drift looks great. With it sat on my desk, it detects a 0.2 degree rotation of the desk surface when I lean forward to type; magic!

$ go run ./cmd/imutests/
Got packet start.
Report: 1f Y: -32.18 P:  -0.94 R:  18.85 X:  -3.24 Y:  -0.16 Z:   9.48
Report: 29 Y: -32.18 P:  -0.94 R:  18.82 X:  -3.24 Y:  -0.16 Z:   9.51
Report: 33 Y: -32.18 P:  -0.93 R:  18.82 X:  -3.24 Y:  -0.16 Z:   9.51
Report: 3d Y: -32.18 P:  -0.93 R:  18.82 X:  -3.24 Y:  -0.16 Z:   9.44
Report: 47 Y: -32.18 P:  -0.93 R:  18.82 X:  -3.20 Y:  -0.16 Z:   9.55

The complete code for the test program is on github.

A better PIO program for PWM decode

One of the things that the BLDC controller needs to do is to decode a PWM signal from an AS5048A hall effect sensor. I had been using two RP2040 PIO programs to measure both the high time and the interval, but I was finding it a little awkward to interleave reads from two FIFOs (and it was using all the PIO resources with 4 motors).

After a bit of head scratching, I came up with this program that reads both the high time of the PWM and the interval and sends them in one 32-bit “struct” on the FIFO. The main reason that I wanted to use two programs before was to avoid the possibility of getting out of sync if I alternated high/interval on the same FIFO.

What if the CPU was reading the interval but thought it was the high time or vice versa? Packing them into one 32-bit value neatly solves that problem. The trick is to use the “IN” instruction, which is normally used to read from pins, but it supports reading from another register instead. By using IN to fetch 16 bits from the counter register (X), we can shift 16 bits of the counter into the ISR register, ready to be sent. By using the “autopush” feature on 32 bits the PIO automatically pushes the packed value after the second “IN” instruction. The only gotcha with this approach is that the counters are limited to 16 bits, which is enough to handle roughly 1.5ms intervals. The AS5048A uses a 1ms interval so that’s just about right for my application.

  mov x, !NULL             ; x = 0xffffffff

loop_high:                 ; do {
  jmp x-- cont_loop_high   ;   x--
cont_loop_high:            ;
  nop                      ;   nop to match number of cycles below
  jmp pin loop_high        ; } while pin is high

  in x, 16                 ; Copy 16 bits of counter into ISR

loop_low:                  ; do {
  jmp x-- cont_loop_low   ;   x--
cont_loop_low:             ;
  jmp pin exit_loop_low    ;   if pin is high: break
  jmp loop_low             ; } while 1

  in x, 16                 ; Copy 16 bits of counter into ISR

The interval and count can then be decoded as follows:

uint32_t high = 0xffff - (raw_pio_output>>16);
uint32_t invl = 0xffff - (raw_pio_output & 0xffff);

The PIO assembler file is here on Github.

BLDC controller phase 4: moving to C

While I had got a motor moving in MicroPython, I found I hit a wall with the garbage collector causing multi-millisecond pauses.

Green signal high showing a multi-millisecond pause. Multiple PWM pulses go by on the yellow signal while Python is snoozing!

A BLDC controller needs to be updating its model at at least 1KHz (and preferably more) so a multi-ms pause at random times is out of the question. I tried a few approaches to avoid GC but, even using the “compile-to-machine code” options in MicroPython, I couldn’t seem to control it. Beyond that, you’re only one small mistake from reintroducing GC by allocating something on the heap accidentally…. If I’m going to be one small mistake away from blowing my foot off, I’d rather work in C, where that’s perfectly normal 🙂

Porting the code to C went very smoothly:

  • The Pico C SDK is wonderful, with excellent documentation. Probably the smoothest C SDK bring-up that I’ve ever experienced. Along with copious examples, there’s even a GUI tool to generate a skeleton project that has a hello-world for each peripheral that you intend on using.
  • I translated the PIO programs from Python syntax to native PIO assembler and the C SDK made it easy to build that into the project.
  • Translating the driver code itself, I did a fairly straight port for the initial version, but I opted to roll my own fixed point arithmetic instead of defaulting to floating point. For example, rather than converting a raw PWM value (which ranges from 0-212-1) I stuck with 212 as my base for angles. That means that clamping to 0-360 degrees can be handled by bitwise AND with 212-1 rather than an expensive modulo operation.

And what was the end result? The C code was significantly faster. So fast that my trick of toggling a pin while the code was actively running didn’t seem to be working at first. I had to zoom in on the oscilloscope to catch it!

Equivalent C code, blink and you’ll miss it!

Once I started work on the C code, I created a Github repo, the initial commit of the C code is here. The initial commit was still a prototype, it

  • Measured the angle of the motor.
  • Multiplied up to get the angle relative to the magnetic poles in the motor.
  • Added a offset, controlled by a pair of pushbuttons.
  • Drove the motor PWMs with a sine wave at the offset phase.

Still, it was enough to prove that C was fast and that the motor could be driven efficiently by correctly controlling that phase angle.

BLDC motor phase 3: finding the limits of MicroPython

With motor position sensor and motor drivers in hand, I set to work actually implementing the “real” control algorithm. I was expecting MicroPython to be “too slow” for this kind of work but, initially it seemed to be working well so I thought I’d push it as far as I could…

The key control loop of the field oriented control (FOC) algorithm that I had in mind is as follows:

  • Measure the angle of the wheel.
  • Convert that to an angle relative to the magnetic poles of the motor.
  • To rotate in one direction, add ~90 degrees to that pole angle.
  • To rotate in the other direction, subtract ~90 degrees to that pole angle.
  • Drive the coils to create a magnetic field at the adjusted angle.
  • Vary the strength of the drive to vary the torque (and hence the speed).

What does those steps actually look like?

Measuring the angle of the wheel starts with the PIO programs in phase 2. Once the PIO programs are configured:

sm0 = rp2.StateMachine(0, measure_high_time, freq=125_000_000,  in_base=pin18, jmp_pin=pin18)
sm1 = rp2.StateMachine(4, measure_interval, freq=125_000_000, in_base=pin18, jmp_pin=pin18)

Reading the PIO values looks like this in MicroPython:

    # Read the interval from the measure_interval PIO
    # sm1.get() blocks until the result is ready.
    # PIO program sends 0xffffffff minus the count so 
    # we need to undo that...
    ivl = 0xffffffff - sm1.get()

    while True:
        # Get the "on" time from the measure_high_time PIO.
        high = 0xffffffff - sm0.get()

        # Extract out the angle measurement; which should be 
        # between 0 and 4095.  The 4119 and 15 come from the
        # datasheet of the sensor.
        duty = high*4119//ivl - 15

        # angle in degrees would be (duty * 360) / 4096

Once we have the wheel angle, we need to convert it to an angle relative to the coils/magnets in the motor. This accounts for the fact that a BLDC motor has three wires wound through it (let’s call them A, B, C) but they are not just wound into 3 separate electromagnets at 120 degrees. Instead, there are many small elecromagnets around the circumference of the motor: first an A coil, then a B coil, then a C and so on ABCABC… The rotor has a similar but different number of permanent magnets attached. The overall effect is that, to spin the motor through 360 degrees, we need to cycle power to the 3 coils, N times instead of just once. It’s like the motor is “geared down”.

With that waffly explanation out of the way(!) in code it’s very simple, we just multiply the angle by half the number of “poles” in the motor, 11 in my case. I chose to keep representing angles by 0-4095 because powers of 2 are generally easier to work with. We also need to add a calibration offset to account for the alignment of the sensor on the motor:

pole_angle = (duty * 11 + calibration_offset) % 4096

OK, now for an easy step; depending on which direction we want to rotate, add or subtract 90 degrees. In my prototype I just hard coded it. We add 1024 because the “degrees” I’m using run from 0-4095:

pole_angle = (pole_angle + 1024) % 4096

Why 90 degrees? It has been worked out that, when two magnets (in our case the permanent magnets in the rotor and the electromagnets we’re about to drive) are at 90 degrees to each other, that gives maximum torque. And, with energy being conserved, maximum torque means doing maximum work to drive the motor round and minimal work to make heat in the coils. We need to constantly keep moving the magnetic field to always be 90 degrees ahead of the permanent magnets in order to maximise torque and minimise heat build up.

OK, we’ve got an angle that we want to drive the magnetic field to, how do we do that? The literature makes it sound very difficult and I suspect that there are much more complex algorithms that take more factors into account, but the basic idea is to drive the three inputs of the motor with 3 sine wave voltages, each 120 degrees out of phase with each other. That energises the three coils in a way that the overall voltage sums to zero but, by varying the phase of the three inputs together, we can rotate the magnetic field.

In microcontroller land, we can’t really alter the voltage, but we can use PWM, which is good enough. At the top of the program we set up 3 PWM pins. It’s important that the PWMs run in phase with each other and unfortunately, MicroPython doesn’t expose that feature so we need to poke directly at the PWM registers…

# Define our PWM pins
pwms = [PWM(Pin(n)) for n in [14, 15, 16]]

# Set the PWM frequency.
f = 40000
for pwm in pwms:
    pwm.freq(f*2)  # *2 for phase accurate PWM

# Define constants for the PWM registers that we need to poke.
# These come from the RP2040 datasheet.
PWM_BASE = 0x40050000
PWM_EN = PWM_BASE + 0xa0

# Note: CH0 and CH7 are the right banks for pins 14-16.
CH0_CSR = PWM_BASE + 0x00
CH0_CTR = PWM_BASE + 0x08
CH7_CSR = PWM_BASE + 0x8c
CH7_CTR = PWM_BASE + 0x94

# Disable all PWMs.
machine.mem32[PWM_EN] = 0
# Set phase correct mode on the two PWM modules that
# relate to pins 14-16.
machine.mem32[CH0_CSR] = machine.mem32[CH0_CSR] | 0x2
machine.mem32[CH7_CSR] = machine.mem32[CH7_CSR] | 0x2
# Reset the counters so that the PWMs start in-phase.
machine.mem32[CH0_CTR] = 0
machine.mem32[CH7_CTR] = 0
# Enable the PWMs together so they stay in sync.
machine.mem32[PWM_EN] = (1<<0) | (1 <<7)

Then, since the RP2040 has plenty of RAM but floating point is slow, I created a lookup table holding the values of sine(angle) for various fractions of the circle where the output value is scaled correctly to be a PWM value (MicroPython always uses 0-65535 for PWM values).

lut_len = 4096
# Add a slight offset to compensate for the on delay of the 
# motor driver.
offset = 0.016
lut = [
                (math.sin(i * 2 * math.pi / lut_len) + 1)/2 +
            ) / 
            (1+offset) * 
    ) for i in range(lut_len)]

With the look up table pre-calculated, setting the PWM in the main loop is as simple as:


With all that glued together and after much debugging (missing the divide by 2 in the look-up table and having all the values wrap being my favourite!) It actually worked… but the motor juddered every few revs. (Sorry, didn’t get a video of this stage.)

After trying a few things to debug I tried adding a debug pin and toggled it high at the start of the loop and then low at the end. That found the problem:

Most of the time, the debug pin (green) toggled on for a few microseconds each time the position sensor sent a pulse (yellow) but sometimes it was multiple milliseconds!

While debugging code by oscilloscope was new to me, it immediately made me think of garbage collection. Turns out that MicroPython’s GC takes a few ms to run, which is fine for a lot of tasks but not for BLDC control. I did try a few approaches to tame the GC (using the machine code decorators in MicroPython, for example) but I think the PWM library was doing allocations under the hood. To avoid that, I’d have needed to directly poke memory and wouldn’t be able to use any libraries without fear of introducing GC again. Not much fun! It was time to switch to the C SDK…

BLDC controller phase 2: adding feedback through the magic of PIOs

After my hello world of BLDC control I waited eagerly for some breakout boards to arrive. To get past the hello world stage I needed:

  • Some sort of position sensor. I chose the AS5048A Hall effect sensor because it was known to work well with SimpleFOC and the motor that I had bought came with the right sort of magnet to work perfectly with it.
  • A decent motor driver. Up to now I’d been using two L298Ns (to get 3 half bridges), which I had on hand, but that’s all there is to recommend it! At time of ordering, TI seemed to have the best available stocks of motor drivers so I picked up a DRV8313 breakout board. The DRV8313 is a dedicated three-phase motor driver capable of 2.5A and 60V, more than enough for my needs.

The position sensor arrived first and I set about connecting it to my Pico. There are two options for that: SPI, or PWM. I had planned to use SPI because it’s faster and the Pico has an SPI port. However, the pads on the breakout board for SPI were super-small and I didn’t have any wire on hand that was small enough(!) Maybe I should try the PWM option for now? But how to interface PWM to the Pico…?

The PWM output from the sensor sends an (approx) 1KHz signal to the Pico and it varies the “on” time of the signal depending on the angle of the wheel. To decode that precisely, you need to measure the “on” time and the “off” time precisely.

Yellow trace is the PWM signal.

Now, the “straightforward” way to do that would be to set an interrupt on the pin and to use the microsecond clock to measure the time between changes in the signal in the interrupt handler. But the RP2040’s datasheet basically says you’re a clown if you don’t use the PIO peripheral for all your bit-banging needs. I don’t want Eben to think I’m a clown! So, I dove into PIO assembler…

Each PIO peripheral has several state machine cores, which are like mini CPUs that execute their very specialised instructions with very precise timing. The specialised instructions can do things like read the state of a pin, push some data to the main processor over a FIFO, or pull data from the main processor and push it out on one or more pins. In fact, one instruction can often do several of those things, all in one clock cycle. The trade off for this precision and specialisation is that each PIO block has only 32 words of instruction memory, shared between 4 cores!

So, what did I come up with? First I wrote a PIO program to measure the “on” time. How hard can it be, just write a program that:

  • Waits for the pin to go high.
  • Increments a counter while high.
  • When pin goes low, push the counter to the CPU.

Waiting for a pin to change is very easy in the PIO, but incrementing a counter turned out to be a problem! The PIO has no “add” or “increment” instruction! 🤔

You can write the PIO code in MicroPython using a special syntax:

def measure_high_time():

    # Set x register to 0
    mov(x, null)

    # Wait for a HIGH.
    wait(1, pin, 0)

    # ???? what goes here ????

    jmp(pin, "high_loop")

    mov(isr, x)

    # Not a real instruction, tells the PIO that, after
    # the push(), it should continue after the wrap_target().
    # Saving a jump is cool when you've only got 32 
    # instructions!

Scouring the PIO manual in the RP2040 datasheet, it took me a while to see the answer, but in the end it was the only option. The only “arithmetic” operation that the PIO has is “decrement x and jump if x is non-zero”. If we only have “decrement”, can we do that instead?

  • Set X to something large
  • Wait for pin to be high
  • While pin is high, decrement X (and somehow turn the unwanted jump into a no-op)
  • When pin goes low, send x to CPU

It took me a while to puzzle out how to set X to something large but I came up with this:

# x = 0
mov(x, null)
# Decrement x, which wraps around to 0xffffffff
# x was 0 so the jump falls through.
jmp(x_dec, "wait")

# Wait for a HIGH.
wait(1, pin, 0)

# Decrement x; jump always fires because x is non-0.
jmp(x_dec, "cont_high_loop")
# Jump lands on next instruction anyway, staying inside
# Loop.
jmp(pin, "high_loop")

# Send x to the CPU.
mov(isr, x)


And it worked! The value sent to the CPU is 0xffffffff minus the count but that’s easily corrected.

I was able to adapt the approach to make a second PIO program that measures the full cycle time of the PWM (i.e. “on” time + “off” time). That was a little trickier because there’s no equivalent to “jmp(pin)” that loops while a pin is low. The code is here in case it’s useful.

Of course, as soon as I showed Lance my code, he Googled the problem and found someone else had an even neater solution. Turns out you can save a whole instruction(!) by using

mov(x, invert(null))

to set x to 0xffffffff directly. You live and learn!

Enabling a second i2c bus on the Pi 5

One of the problems that we had with our previous PiWars entry was unreliable I2C communication from the Pi to all our peripherals. One reason for that was that we had a lot of devices on the bus.

We’re using a Pi 5 this year and one of its advantages is that there GPIO header has been massively upgraded. It defaults to the same functions as the Pi 4 but it can be reconfigured to trade GPIO pins for extra I2C busses, PWMs and several other functions. So, this evening I had a go at remaking a second I2C bus and, after a bit of digging, it turned out to be very easy.

After reading the device tree documentation in /boot/overlay/README, right on the Pi, it turns out that there’s a pre-made configuration “overlay” for each peripheral that you might want to enable.

The overlays are all listed in that file along with their configuration flags. All I needed to do to enable the I2C2 bus was to add this line to /boot/config.txt and then reboot:


Then, to check it was working, I put my oscilloscope on those pins and ran

sudo i2cdetect 2

That gave the output down at the top of the article. The rise time looks slow, which I think is because it’s using the internal pull up. I probably need to add an external pull up radiator if the right value.

Along the way I find the pinctrl command helpful. It can be used to show the function and current state of the pins.

pinctrl funcs 0-27 # show the alternate functions of GPIO pins
pinctrl get 12-13 # get current state of pins 12 and 13

Driving a BLDC motor from zero to slow-and-janky in Python

Motor and Pico, ready for battle…

One of my personal goals for PiWars this year was to try to build a BLDC motor controller. Mainly because I find that kind of thing a fun challenge…

Why is controlling a BrushLess DC motor a challenge? It has no brushes(!) the brushes in a brushed motor physically implement the control algorithm for the coils, energising each coil in the motor in correct sequence in lockstep with the rotation of the motor. Controlling a brushless motor requires the controller to correctly energise the coils in lockstep with the rotation of the motor. This gives great flexibility (but also allows for easily burning out a motor!).

Of course, we could buy a controller off the shelf, but the Pico is out and I wanted to use one in anger. It has been done before by the SimpleFOC project and my plan B was to “just use SimpleFOC” (but there’s less fun in using a library)!

Hello world

At the most basic level, I had the idea that field-oriented control (FOC) was about driving sine wave signals into the three terminals of the motor at 120-degrees out of phase with each other and then rotating those sine waves through 360 degrees in lock-step. In turn that rotates the magnetic field smoothly through 360 degrees and motor’s rotor turns with it. There’s a lot more to it than that, but I didn’t have have a rotary encoder when my motor arrived so I thought “let’s just give that a try in MicroPython”:

  • Assign 3 GPIO pins to the 3 phases of the motor.
  • Make them all PWM outputs at say, 20KHz and connect to motor through a (half bridge) motor driver per pin.
  • Loop, incrementing x each PWM cycle…
    • Set PWM 1 to sin(x)
    • Set PWM 2 to sin(x+120 degrees)
    • Set PWM 3 to sin(x+240 degrees)

And… after some fiddling (and lots of debug prints to the console) something happened!

Writing this now that I’m a bit further on in the project I now know that controlling a motor like this is really bad(TM) because the rotor in the motor “catches up” to the magnetic field, which means that the field stops doing as much work on the rotor and it dumps all that electrical energy we’re feeding it into heat instead :-/ Still, as a hello world, it felt great!

If you’re going to try anything like this and you want to keep the magic smoke inside the motor, you’ll want a benchtop power supply with current limiter and an oscilloscope!

Soldering iron: hot; kettle: on; OpenCV… should finish compiling by PiWars 2024

After a few years where life, a non-silicon baby, and, global pandemic got in the way, the Tigerbot team has dusted off our soldering irons and put the kettle on while we compile OpenCV for PiWars 2024.

We all have our own reasons for entering PiWars:

  • Lance likes to have a pretext to build barely-legal nerf cannons.
  • Nell likes to be the only one who’s event code actually works.
  • I find myself being drawn to the low-level stuff: building boards and programming microcontrollers… Didn’t the Pi foundation release one of those of their very own? Well that discounts reusing any of our old hardware doesn’t it. We’ll have to rebuild all the fun bits around a Pico (or two) 😉

How are we getting on:

  • We’ve made some choices on theme and overall shape of the bot; we want to go back to 4 meccanum-style wheels this year since they were a lot of fun to drive on the Orange Tigerbot.
  • Lance has done a first pass at a CAD model for the bot and he’s started 3D printing. He’s planning to pull together a basic version of the bot with basic motors ASAP so Nell has something to work with.
  • Nell’s started on coding; repurposing our Golang controller code from last time and sketching some code for one of the events.
  • I’ve fallen down the motor control rabbit hole; I really like the idea of using brushless motors this year because it’s a challenge to try writing a motor controller for the Pico…