FOC Servo Control Firmware
I wrote a a robust servo control algorithm with various control modes based on hardware from a ST dev kit designed for high performance motor control. I primarily designed the firmware to operate based on the standard step/direction input so it can be used in coordinated motion applications such as 3D printers. My primary use for the controller is to drive the linear motors I have previously designed and built.
The firmware code is available on Github.
Board
I recently discovered a really affordable dev kit from ST Micro based on an STM32 microcontroller, the B-G431-ESC1.
The board is based on the STM32G431 microcontroller and supports up to 40A continuous phase currents (with forced air cooling) at 24V. There are exposed IO pads for incremental encoders, CANBUS connections, and a UART port. In addition, there is a built in STLINK V2-1 board with an auxiliary potentiometer and tactile button. Overall, its a very compelling kit and it only costs 20USD as of the time of writing, making it one of the most affordable servo controllers, once the firmware is written.
Project Setup
In addition to the boards, ST also publishes a variety of firmware development tools specifically catered to motor control applications. One of these is the Motor Control Workbench, which I used as a basis for my project. The tool creates a project with a provided implementation of field oriented control, which notably takes full advantage of the various peripherals on the microcontroller.
Board IO Configuration
I also used a slightly different IO configuration than is standard. I used the standard pads for the encoder and CANBUS, but I replaced the UART port with a step/direction/enable interface. In particular, it is possible to used one of the general purpose timer peripherals on the microcontroller to handle the step direction interface in hardware, which can handle step rates up to a theoretical max of 40MHz without using any CPU cylces! It is a bit painful to lose the UART port, since that could be an easy way of managing configuration, but I figured I could use the built-in STLink to configure the device, so overall it is a pretty good tradeoff.
Servo Control Algorithm
I decided to use a cascaded position-velocity controller, which in many cases performs much better than a plain PID controller for motion applications. In this type of controller the position is controlled by a closed-loop proportional controller whose output is a velocity command. The velocity is then controlled with a closed-loop PI controller, whose output is motor current, which is directly proportional to the force or torque that the motor produces. Overall, this part of the controller has three parameters: the position gain, the velocity gain, and the velocity integrator gain.
I planned on providing the controller with position commands through a step-direction interface. However, the control performance is poor if the step position is naively fed to the cascaded controller, because the controller can't keep up with high frequency motions because the controller is trying to force the velocity and acceleration to 0, which we don't actually want. The solution is to compensate for the controller's slower responses by calculating the velocity and acceleration from the position signal over time, and pass those values to cascaded controller as feed forward terms for velocity and torque. One note is that there is a constant factor between the acceleration and torque values, which is due to the inertia of the moving mass.