I was able to repro the issue using the code above. It also repros with mtp, since mrp is just a wrapper around mtp. I think the problem is a race condition in the "motor done" logic - after the "done" flag gets cleared, it sometimes gets immediately set again, resulting in the motor not moving. More details below.

If I understand correctly, the "motor done" flag gets cleared when the motor mode register changes (see wallaby_dma.c#L226-L234):

// handle motors modes clearing done bits
// TODO: cleanup
if (address == REG_RW_MOT_MODES)
{
    if (value & 0b00000011) aTxBuffer[REG_RW_MOT_DONE] &= ~1;
    if (value & 0b00001100) aTxBuffer[REG_RW_MOT_DONE] &= ~2;
    if (value & 0b00110000) aTxBuffer[REG_RW_MOT_DONE] &= ~4;
    if (value & 0b11000000) aTxBuffer[REG_RW_MOT_DONE] &= ~8;
}

The current mtp logic is:

Private::set_motor_mode(motor, Private::Motor::SpeedPosition); // (A)
Private::set_motor_goal_position(motor, goal_pos); // (B)
Private::set_motor_goal_velocity(motor, velocity); // (C)

Say you just called:

mtp(0, 1000, 1000);
bmd(0);

After this finishes, the goal position is 1000, the current position is around 1000, and the "done" flag is true.

Now you call mtp again. Line (A) sets the motor mode, which clears the "done" flag. Before line (B) runs, the STM32 code may run and see that the goal position (the old one) has been reached, so it sets the "done" flag back to true. Now line (B) runs and sets the new goal position, but it's too late. The "done" flag has been set, so the motor won't move towards the new goal position.

I think the easiest solution is reordering the register writes to B, C, A. There can still be different race conditions, but thinking through the possibilities, they shouldn't result in bad behavior.

More complex but possibly better solutions could be:

• Combine the register writes so they're written together, if possible
• Move the responsibility of clearing the "done" flag to libwallaby, so mtp can ensure the "done" flag isn't cleared until it's time