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
PWM_INTR = PWM_BASE + 0xa4

# 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 = [
    min(
        int(
            (
                (math.sin(i * 2 * math.pi / lut_len) + 1)/2 +
                 offset
            ) / 
            (1+offset) * 
            65535
        ),
        65535
    ) for i in range(lut_len)]

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

pwms[0].duty_u16(lut[angle%lut_len])
pwms[1].duty_u16(lut[(angle+(4096/3))%lut_len])
pwms[2].duty_u16(lut[(angle+(4096*2/3))%lut_len])

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…

Leave a Reply

Your email address will not be published. Required fields are marked *