From an algorithmic perspective, at its core, the main task of a servomotor is to take the desired control variable (e.g., position) and command the connected DC motor in such a way that the desired position is achieved.

For Servio, we aimed for somewhat advanced forms of motion planning and control. In the first milestone, we chose basic control loops for three variables: current, position, and velocity.

We implemented three modes for Servio, in which the firmware focuses on controlling the respective variable - current mode, position mode, and velocity mode. In each mode, multiple control loops can be active:

• Current mode - uses just the current control loop.
• Position mode - uses both the current control loop and the position control loop.
• Velocity mode - uses both the current control loop and the velocity control loop.

To make the loops work, we introduced a Kalman filter into the system to estimate the current velocity/position based on the sensor used for the axis angle. This was necessary as we needed some way to estimate the velocity accurately.

All of this is illustrated in the following diagram:

Current control loop

The main control loop, the only one in direct contact with the hardware, takes a desired current as an input and controls the power sent to the motor based on the currently sensed current.

It operates as a standard PI control loop, using current as the input values to produce the desired power for the motor. The power is represented by a value ranging from -100% to 100%. (The current loop is not aware that PWM is used.) This output is then fed into timers, which generate PWM pulses to the H-bridge used by Servio.

Based on some insights from the industry, we decided to use a PI controller at 10kHz, as we were informed that the key to making the control work correctly lies in the frequency. That is, a high-frequency PID will be more tolerant to PID coefficient tuning.

To achieve this, we generate PWM at a 20kHz frequency. Each control loop iteration spans two PWM periods:

1. Odd cycle - We sample the current flowing through the H-bridge. In the diagram, the sampled values are represented by blue dots. In the real system, we can sample up to 30 values.
2. Even cycle - The average of these values is calculated, and this average is given to the control loop, and a new PWM value is calculated.

Note that the exact frequencies of the control loop and the PWM are not fixed; we have just fixed the ratio between them. In the future, we want to experiment with changing the frequency, ideally to try a higher value. In the control loops schema below, you can see which parts of the system are active in control mode.

Position control loop

We sense the position from a potentiometer at a frequency higher than 1kHz, which governs the frequency of the position control loop. This loop uses a PID controller that takes the measured position and the goal position as inputs and calculates the desired current flow through the system. Thus, the position control serves as an input to our current control loop, which handles the rest.

We don't use the measured position directly; instead, we rely on a Kalman filter to perform some filtering on the measured value.

Given the high static friction in our test system, there were issues with the robustness of the loop. To address this, we decided to implement an extra bias. If the servomotor is not moving, the current output from the position control loop is multiplied by a scale value (2.0). This scale value linearly degrades once the servo starts moving and reappears linearly once the servo stops. While not a perfect solution, it has proven to be good enough as it is quite intuitive to configure. (This bias is not visible in the schema.)

In the schema below, you can see which parts of the system are active in position mode.

Velocity control loop

Velocity control operates on the same principle as position control; we use position readings to estimate velocity. We employ a PID regulator that takes the actual velocity and the desired velocity as inputs and outputs the desired current to achieve the given velocity.

The challenging part was the estimation of velocity, which is why we introduced a Kalman filter into the system for processing the position readings. We configured the filter to take position as an input and output estimates of both position and velocity. The velocity estimate from the Kalman filter is what we use for velocity control. So far, it seems to be good enough and stable, although we have not yet conducted satisfactory tests for it.

This also proved to be a challenge a bit, as it required some time to modify the filter appropriately to work with continuous values, such as the angle of rotation.

In the schema below, you can see which parts of the system are active in velocity mode.

Limits

What consistently emerged as a challenge were the limits. I had a strict requirement that the configuration options for the servo should include limits for maximum and minimum position, velocity, and current. This means that users should be able to set restrictions on all these parameters, and the servo should adhere to them under all conditions. We considered these to be soft limitations - exceeding the range is not viewed as an error, but rather something that needs to be rectified.

Ensuring adherence to current limits while in current control mode is straightforward, and the same is true for other variables within their respective control modes. It is also simple to maintain awareness of current limits during position or velocity modes, as both use the current control loop, thus honoring the current limits.

The challenging part is: How do we ensure adherence to position and velocity limits in a the current mode that only activates the current loop? How can we maintain position limits in velocity mode? And how do we maintain velocity limits in position mode?

We devised a solution that adjusts the current limits based on the current position relative to the position limits. This concept is illustrated in the figure above. The red line symbolizes the current limit for the servo; as the position (`pos`) approaches the maximum position limit (`max`), the current limit decreases. When `pos` reaches the `max` value, the maximum current limit will be `0 A`.

The implication of this approach is that the servo will not be able to generate maximum current when nearing the position limits, but this was considered an acceptable compromise.