Tag Archives: control

Velocity and acceleration limited trajectories

One of the oldest requested features for the moteus brushless controller has been a form of trajectory control beyond constant velocity trajectories. For most applications this is not an actual deal-breaker, because arbitrary trajectories can be approximated by piecewise linear constant velocity trajectories in the application layer. However, for many people, that is big hurdle to jump over to start with, and for some, it can actually limit application effectiveness because a fair amount of CAN bandwidth is required to achieve the high rate control necessary for smooth motion.

So, as of release 2022-04-07, moteus now supports velocity and acceleration limited trajectories out of the box. All devkits now come with human-eye-pleasing limits enabled by default, although bare boards leave it disabled as per board defaults. There are two parameters to control the feature:

  • servo.velocity_limit – The maximum velocity used when moving to a particular target goal
  • servo.acceleration_limit – The maximum acceleration used when moving to a particular target goal

Additionally, these limits can be overridden on a per-command basis, both using the diagnostic protocol “d pos” mechanism, and the register implementation.

Check out this video, then read below the fold for more details:

There are a few details to the implementation which help it fit better into the existing moteus control framework, while enabling some features that don’t exist elsewhere.

Special-valued limits

First, either limit may be “nan”, (the usual special value in moteus configuration-speak). In that case, no limits are applied. If both are “nan”, that results in identical behavior to what was previously used in moteus, in that a “d pos” command immediate attempts to achieve the desired position and velocity. If velocity is limited, but not acceleration, all such commands result in constant velocity trajectories to reach the target position, followed by continuing at the target velocity indefinitely. If acceleration is limited but not velocity, then the velocity will be continuous, but unbounded (except possibly by servo.max_velocity). If both are limited, then the velocity will be continuous and bounded.

Non-zero velocity targets

The target state is a position and a velocity. moteus will reach the position and be traveling at the desired velocity when it reaches it. After reaching the state, the controller will continue at that target velocity indefinitely, or until another command is received.

Idempotent commands

For many cases, it is still possible to construct commands such that they are idempotent. That means that you can send the same command over and over and it will not affect the semantics of the control. This is particularly easy if the destination state has a zero target velocity.

If the destination state has a non-zero velocity, then the command will need to change sufficiently far in advance to avoid “looping around”. If a command with a non-zero velocity is received after the machine has passed through the target state, then the only way to achieve it is to slow down, go back, and accelerate again so as to be moving at the proper speed. For applications like these, it is best to always have the target be at least several control cycles in the future so there is no risk of such looping.

Completion indication

It is now feasible, in many cases, to send moteus control commands very infrequently. A complete motion from stopped in one position to stopped in another can be initiated with a single command. So that the application code can advance in a timely manner, moteus now reports a “completion” indication that flags when the target position and velocity has been reached. This can be accessed as register 0x00b in register mode, or as servo_stats.trajectory_done in diagnostics mode.

Interaction with the “stop position”

Previously moteus had the “stop position” mechanism to emulate a constant velocity trajectory with a fixed endpoint. That mechanism is still present. While it has defined interactions with the acceleration and velocity limits, they are, shall we say, “not particularly useful”. Thus, it is recommended to not use the “stop position” feature combined with the velocity and acceleration limits unless you really know what you are doing.

Jerk limited trajectories

The implementation in moteus only supports limiting acceleration, not jerk. Thus the acceleration is allowed to be discontinuous. For an application where jerk limited trajectories are required, piecewise interpolation is still required. For accelerating phases, this can be trivially accomplished by slewing the acceleration limit override in each moteus command. For decelerating phases, or where precise control over position is required, a full fledged jerk-limited planner (like ruckig) will be required. The output from such a planner is probably easiest to feed to moteus as a piecewise series of constant velocity trajectories with no limits configured as before.

customizable PWM rate for moteus

Being a switch mode 3 phase motor driver, the moteus controller changes the current flowing through the phases of a motor by rapidly switching the phase terminals between the positive input voltage and the input ground. The control of this switching is denoted “pulse width modulation”, or PWM for short. To date, the rate at which it has switched has been fixed in firmware at 40kHz. As of release 2022-03-12, this can now be altered anywhere between 15kHz and 60kHz to better optimize peak power capability, control bandwidth, maximum speed, and heat generation.

Read on for more details:

Peak power

Switch mode motor controllers typically have large banks of capacitors across the input immediately next to the active switching elements. This bulk capacitance has the primary goal of minimizing voltage ripple during switching events. When a switch is engaged to apply more current into the motor, that energy is pulled from the capacitors, and when a switch is engaged that pulls energy back out, that energy is stored back into the capacitors. Only more slowly is the energy pulled to and from the primary input terminal. The magnitude of the ripple is determined by the effective capacitance, the power applied to the motor, and the switching frequency. More capacitance decreases the ripple, more power increases it, and higher frequencies decrease it, all in roughly linear relationships.

For moteus r4.5 and r4.8 (and likely subsequent revisions of this design), this voltage ripple is what controls the peak power that moteus can drive. moteus is designed to have a very small form factor, and thus uses only MLCC capacitors for its bulk DC decoupling. This results in it having an abnormally small bulk capacitance compared to otherwise similar controllers (doubly so because of MLCC capacitor derating).

The rated 500W of peak power for moteus (limited to 450W by default in config), can be achieved with the default 40kHz switching rate. By increasing the switching rate, the peak power can be increased to 750W, and if the switching rate is decreased to the minimum of 15kHz, then the rated peak power is around 190W.

Heat dissipation

Switch mode controllers generate heat while operating as a result of many factors. The two biggest ones for moteus can be broken down into “dynamic” and “static” components of the gate drive and switch MOSFETs.

The easiest to understand is the “static” component of the switching losses. When the power MOSFETs are fully turned on, the power dissipated in them is determined by their on resistance and the amount of current, using the formula P=I^2*R. The on resistance for MOSFETs generally increases with temperature, so the worst case will be at the maximum rated temperature of the device.

The dynamic component encompasses a few different pieces. One, is that while the MOSFETs are in the process of turning on and off, their resistance is much higher than when fully on. Another is that during the “dead-time” window between when the high side and low side of the H-Bridge is enabled and neither is actively driven, the current is conducted through the body diode of one of the MOSFETs, which has what can be modeled as a fixed voltage drop. The third big component of this is the energy required to charge and discharge the gate capacitors of the primary switching MOSFETs. The energy used in the dynamic region for a given power output stays roughly constant for each switching event. Thus switching faster results in more energy used per unit time scaling in a roughly linear manner.

Further, the dynamic component significantly depends upon the input voltage. Higher input voltages mean that the MOSFETs take longer to switch for a given gate drive current, that the power dissipated is larger during the switching events, and that more energy is required to charge and discharge the gate capacitors.

Thus as the switching frequency increases, the power used consumed during the switching events goes up, while the total time spent in the static region goes down. That means the efficiency goes down as the switching frequency goes up.

Control bandwidth and motor speed

For PWM rates of 40kHz or less, moteus implements its entire control loop at the PWM update rate. When this control rate is decreased, the bandwidth available for control of torque, and subsequently position decreases.

Also, the maximum electrical RPM that the controller can achieve is also directly related ot the control rate. However, few applications reach the 1.5kHz electrical frequency possible at 15kHz, so this is not often a concern.

Due to the implementation details, selecting PWM rates above 40kHz results in the control cycle running every other switching event. That means that the maximum control bandwidth and electrical speed is achieved for a pwm rate of exactly 40kHz. Higher or lower and those bandwidths and speeds are decreased.

Thermal experiments

To show the effects of changing the PWM rate on thermal performance, I made up a simple experiment with a moteus r4.8 and an mj5208 attached to a devkit bracket but disconnected from the moteus so that they would be thermally decoupled.

The fixed voltage mode was used to slowly spin the motor while progressively larger amounts of current were applied to the motor. This approximates operating with full torque at a standstill, where the motor performs almost no mechanical work. For each current, I waited for the temperature in the motor windings and the controller to stabilize.

Using this data, I was able to roughly estimate a few parameters at these different operating points. First, in ambient air with no heat sinking or forced cooling the mj5208’s thermal conductivity is around 4 C/W. moteus’s thermal conductivity in the same environment is around 40 C/W. The thermal efficiency and effective idle thermal generation of the controller at various operating points can be seen below along with the available peak power:

PWM FrequencyPeak Power24V36V
15kHz190W94% / 0.20W92% / 0.25W
40kHz500W88% / 0.25W85% / 0.30W
50kHz625W84% / 0.30W84% / 0.35W
Thermal efficiency for moteus r4.8 driving a nearly-stationary motor

These efficiency tables can be used to roughly determine the thermal design for moteus in a setting with air cooling, no heat sink, and any motor heating dealt with independently. If you are running the default 40kHz PWM rate at 24V and are applying 5W of power to your motor, that means the total power consumed will be 5W/0.88% +0.25W=5.93W. The 0.93W of waste power dissipated in moteus will result in its temperature being approximately 0.93W* 40C/W=37C higher than ambient.

That value can be improved either by lowering the PWM frequency or input voltage to improve efficiency, or by using heatsinking and/or forced air or liquid cooling to decrease the effective thermal conductivity of the controller.

How to configure moteus

Once you know the PWM frequency you want to use, selecting it is as simple as setting the servo.pwm_rate_hz configuration value to something between 15000 and 60000.

It is important to note that the maximum power configurable value, servo.max_power_W is now defined as the power when configured at 40kHz. Internally it is scaled accordingly if the PWM rate is changed. This means you can change the PWM rate without worrying about accidentally having too large of a power limit configured.

Fixed voltage mode for moteus

The most recent moteus firmware release, 2021-12-03, added not one, but two new control modes for less common applications. Previously, mentioned was the “voltage_control_mode” for using gimbal style high resistance motors without changing the sense resistors. In this post, I’ll describe a similarly named, but very different mode “fixed voltage mode” for operating brushless motors as if they were a stepper motor.

For some applications, you don’t care about torque control, or about power consumption at all. Traditionally you would use a stepper motor in those applications, with a correspondingly less expensive stepper motor driver. However, in some cases you may still want to have high rate trajectory control, CAN based telemetry, or have already standardized on moteus controllers for other moving parts of your solution. There are two new options that can be used in such situations:

  • servo.fixed_voltage_mode: When this is non-zero, the primary encoder is not used for control at all. Instead, positioning is performed entirely in the electrical phase space, with a fixed voltage applied always to the phase windings.
  • servo.fixed_voltage_control_V: The fixed voltage to apply to the phase windings.

When this new mode is active, the controller will continually burn power in the windings of the motor and uses no feedback from the motor at all. As mentioned above, this makes it operate like a micro-stepping stepper motor, or an inexpensive gimbal motor controller. If the externally applied torque is greater than that produced by the fixed voltage, the motor will “skip” and lose track of its position.

Filtering encoder values in moteus

TLDR: moteus can now filter the encoder, resulting in less audible noise. Use firmware version 2021-04-20 and ‘pip3 install moteus’ version 0.3.19, then re-calibrate to get the benefits.

Background

The moteus controller uses an absolute magnetic encoder to measure the position of the rotor. It uses this knowledge to accurately control the current through the three phases of a brushless motor so that the desired torque is produced, i.e. “field oriented control”. This works well, but has some downsides. One, is that magnetic encoders work by sensing the magnetic field produced by a “sensing magnet” that is somehow affixed to the rotor. This sensing process always introduces some noise, so that the sensed rotor position is never perfect.

Because of this, even if the rotor is perfectly stationary, moteus will constantly be tweaking the phase of the motor current to track the noise. This results in slightly increased power consumption, and more important to most, audible noise as the slight variations in control current can induce vibrations in the phase windings of the motor.

Fortunately, in most applications, the rotor is not actually capable of accelerating with the full bandwidth that the magnetic sensor is capable of sensing. Thus, we can filter it to remove high frequency noise without any loss of performance.

Approach

The filter method that moteus uses is an “all-digital phase locked loop“. Formulated in the traditional PLL terminology, this consists of three pieces: a numerically controlled oscillator, a phase detector, and a PI controller.

The “numerically controlled oscillator” is just a counter that tries to “guess” what the encoder is doing by propagating a value forward at an estimated velocity. For an ADPLL in this setup (where our encoder gives us an absolute phase measurement), the “phase detector” just subtracts the measured phase from the estimated phase. The PI controller determines how the estimated oscillator tracks disturbances in the actual encoder frequency.

In my post on automatic torque bandwidth selection for moteus, I derived the necessary equations for determining the torque bandwidth. Here, I’m just going to reference the excellent article by Neil Robertson, “Digital PLL’s” (part 1, and part 2), over at dsprelated.com. There, he derived the necessary proportional and derivative gains for a given bandwidth and damping ratio:

K_p = 2 \zeta \omega_{3db}

K_i = {\omega_{3db}}^ 2

Where \zeta is the “damping ratio“. For our purposes, we’ll use 1.0, which is known as “critically damped” and is often a good balance between stability and response time.

Experiments

To see this in action, I ran some experiments on a moteus devkit. I added back in a conditional ability to emit a few bytes of debugging information at the full control rate to the firmware, and used that to emit the encoder value as used by FOC at each control cycle. I made up a script, that can plot the power spectral density in the encoder measurements. This is basically the noise present at any given frequency. When run on an unmodified development kit with no encoder filtering, the result looks like:

Power spectral density of moteus AS5047P encoder in a devkit

This shows that there are peaks in the noise at around 700Hz and 2kHz and after that the noise drops off rather rapidly. Then if we enable filtering at a few different bandwidths, you can see how the plot changes:

Power spectral density of AS5047P with various filter bandwidths configured

The filter bandwidths here varied from 5000 rps (~800Hz) down to 100rps (~15Hz). The filtered noise profiles follow what one would expect for each of the filters.

Audible noise

To measure the combined effect of this and torque bandwidth on audible noise, I switched to a controller attached to an 8108 motor, as I’ve seen those tend to demonstrate more audible noise. The motor had a constant back and forth motion at 0.5Hz for 60s and the result was captured with a studio mic set about 1cm away from the motor. In each test, I ran moteus_tool --calibrate and selected only a torque bandwidth, letting it pick an appropriate encoder bandwidth. For the 100Hz torque bandwidth and above tests, the same position PID values were used, although they needed to be tweaked for stability at the lower bandwidths. I similarly plotted the power spectral density for several filtering values and for silence. The total signal strength measured in negative dB is noted in the label:

It is not shown here, but the unfiltered noise is about the same as the 400Hz one. Thus, the maximum improvement is around 6dB of audible noise. Filtering at 100Hz gives most of that benefit for this motor, with slight improvements beyond that. Most of the audible energy is in the spectrum below 1kHz and there are several frequency bands where the encoder does not appear to be the dominant source of audible noise, as all options are equivalent in those.

For comparison purposes, the ODrive firmware defaults to filtering both the torque and encoder at around 160Hz (1000rps).

moteus_tool integration

As of version 0.3.19, moteus_tool has support for configuring the encoder bandwidth during calibration. By default, it will select a value appropriate for the selected torque bandwidth, or you can specify it manually with --encoder-bw-hz.

Auto-tuning current control loops

Since the moteus controller was first released, it has implemented a two-stage controller. The outer loop is a combined position/velocity/torque PID controller, which takes as an input a position trajectory, and outputs a desired torque. The inner loops accepts this torque, and uses a PI controller to generate the Q phase voltage necessary to achieve that torque.

Until now, the constants for that PI controller were left as an exercise for the reader. i.e. there were some semi-sensible defaults, but the end-user ultimately had to manually select those constants to achieve a given torque bandwidth. That isn’t too much of a problem for a sophisticated user, but for the rest of us, it is hard to know how to go from a desired torque bandwidth to reasonable PI gains.

Now, I’ve finally decided to tackle this problem! There are two phases, the first being how to measure the inductance of a given motor, which is last major motor parameter to be automatically measured. The second is how to generate a controller using the resistance and inductance which has a desired bandwidth.

Measuring inductance

An inductor is an energy storage device where the rate of change of current is directly proportional to the voltage across its terminals. Most (probably all?) electrical motors act as inductors in various forms. The hobby outrunners typically driven by moteus controllers, surface permanent synchronous magnet motors, can be treated as two equal valued inductors across the imaginary D and Q axes.

Given an ideal inductor, an easy way to measure that inductance is to apply a constant voltage and see how fast the current rises. That’s roughly the approach I took with moteus, but instead of a fixed voltage, it applies a square wave voltage centered about 0 across the D axis. That way the motor doesn’t move, the net current is 0 over time, and the average rate of change can be integrated over many cycles for much higher accuracy.

This “square wave” mode is implemented as a new control mode in moteus, primarily intended to be invoked by moteus_tool during the calibration process. You can specify the half-period of the square wave in control cycles, and the voltage to apply across the D axis. Here’s a plot of the current, as reported by a moteus controller over its debug DAC connection during a test on an mj5208:

This shows that yes, the current is roughly triangular for the input square wave.

Designing a current controller

Given this newly measured inductance, and the phase resistance that the calibration process already measured, we have sufficient information to design a PI controller which achieves a desired bandwidth response.

For a PI controller, a simple second order approximation of the closed loop transfer function can be written as:

I(s)=\frac{K_p s + K_i}{L s^2 + (R+K_p) s + K_i}

Where L is the D/Q phase inductance, R is the phase resistance, and Kp and Ki are the PI gains. Taking that, if we constrain:

K_i = K_p \frac{R}{L}

We can then substitute this into the original transform, which results in a first order system:

I(s)=\frac{K_p}{L} \frac{1}{s + \frac{K_p}{L}}

This system has a 3db frequency at the sole pole:

w_{3db} = \frac{K_p}{L}

Which then allows us to fully determine all the remaining parameters:

K_p = w_{3db} L
K_i = w_{3db} R

Finally, when we double check our results, the “rise time” for a first order system (roughly the time it takes to go from 10% to 90%) can be described as:

t_{rise} = \frac{0.35}{BW_{hz}}

A motor like the mj5208 can be approximated as a phase resistance of 0.04 ohm, with a D/Q phase inductance of 25e-6 H. For a 3dB bandwidth of 1000 radians per second (~160Hz), the gains measured in radians per second would be:

K_p=0.025
K_i = 40.0

To set these on the controller, we would use the following parameters:

servo.pid_dq.kp=0.025
servo.pid_dq.ki=40.0

Empirical testing

To make sure these make sense, I used this process for a few different bandwidths on a few different motors. For each, I used an oscilloscope to measure the rise time of the current waveform in response to a step input of a relatively small 4A command. The shape of that curve lets us be pretty confident that the damping ratio is correct, and the total rise time of the curve gives a good measure of the overall bandwidth that was achieved.

Across the motors I tested, the resistance varied from 35-65 milliohm, with inductances varying from 9uH to 33uH. The grid shows that the bandwidth worked out pretty close to the target in all those cases.

moteus_tool integration

Now that the math is out of the way, you don’t need to worry about it at all, because as of version 0.3.17 of moteus_tool (with version 2021-04-09 of the firmware), this whole process is automated. When you run the calibration step, you can just specify the desired 3db bandwidth on the command line of moteus_tool and it will calculate and set the parameters for you. By default it will select 100Hz bandwidth.

python3 -m moteus.moteus_tool -t 1 --calibrate --cal-bw-hz 100

Video

If you want to see this content explained more verbosely, here’s a video that covers most of it!

moteus position anti-windup

The moteus controller uses a somewhat unique integrated position / velocity / torque controller with per-command configurable proportional and derivative gains. Through various combinations of these settings, it can emulate many different types of controllers, but one that it has struggled with until now was a pure velocity controller.

It has been minimally possible to use moteus as a purely velocity controlled since wraparound support was implemented, but that came with a caveat. Either the proportional term needed to be set to 0, in which case velocity tracking performance was poor, or if the proportional term was non-zero, an external torque would cause the position to drift arbitrarily far from the target position. Then if the external torque were released, the controller would “catch up” for all the lost ground, moving very rapidly.

Now, however, as of release 2021-03-05, an optional configurable parameter has been added to the moteus firmware which enables the “servo.max_position_slip” option. When this is finite, it acts as an anti-windup term on the position tracker, keeping it from getting far out of line. Tuning this lets you control how hard the controller tries to track velocity in the face of external disturbance, and how much catching up it will do when that external disturbance is relaxed.

This wasn’t conceptually hard to implement, but needed careful construction to interact properly with the existing stop position and position bounds that the firmware implements.

Here’s a video demonstrating the problem, and how the new configurable lets you resolve it:

Stable gait sequencing

In the last post, I described the newer gait engine which takes a desired command and produces a set of gait parameters. At that point, the gait engine needs to implement those gait parameters in a way that is stable with respect to disturbances and keeps the two legs properly out of phase with one another.

The gait variables that the gait selection procedure emits are as follows, each “leg” is actually a pair of legs.

  • swing time: This is the amount of time between when a leg lifts off the ground, and when it lands again.
  • one leg time: This is the amount of time spent with only a single leg on the ground.
  • two leg time: The amount of time with both legs on the ground.
  • flight time: The amount of time with no legs on the ground.

I’ve chosen to implement this for now with two controllable variables, when to pick up the next leg to begin its swing phase and how far out to place the foot when landing. When in flight, the swing time is fixed to be exactly what the gait engine selected. When on the ground, the leg position and velocity are fixed in the world frame to match the current command. This results in the other three parameters, one leg, two leg, and flight time, being approximated as best as possible.

When to lift

When there is no flight time, then the heuristic for when to lift the leg is identical to that described in “Balancing gait in 2D“. The trailing leg is lifted when the opposing leg is half of a swing time away from crossing the balance point.

When there is a flight time present, we have to add a corrective term to that heuristic, otherwise the two legs can lose their phase synchronization. If the correct out of phase relationship is being held, then the leg will be lifted when the alternate leg is in flight and has the “flight time” remaining in its swing cycle. The correction for that leg is calculated as a difference in time around that point such that the correction is positive before that point and negative after scaling in a linear manner. This has the desired property that the two legs are kept out of phase, without enforcing a strict leg cycle time.

Where to place

Once again, when there is no flight time, the placement location is the same as before. When there is a flight time, then the placement location is even simpler. Now it is just a position forward of the balance point by one half of the “one leg time”. This results in the machine spending roughly half of its time on each side of the balance point.

Further work

I’ve got one further enhancement to this technique to describe, at which point I think I’ll have basically exhausted its promise. Further advances will likely have to come from using an optimization based controller rather than a heuristic driven approach.

Higher speed gait formulation

As hinted in my earlier video I’ve been working towards some higher speed gaits with the quad A1. To accomplish that, I had to restructure the gait sequencing logic to permit changing cycle times and allow flight phases.

For now, I’ve tentatively broken down the trot gait into 5 regimes, based on how fast the machine is moving:

  1. At the slowest speeds, the flight legs swing through a step in the configured maximum flight time. The interval between flight times is fixed at a configured maximum. Here the speed is determined by how far the flight legs move.
  2. Once the flight legs are moving through their maximum allowed distance, then the amount of time spent with both legs on the ground is reduced in order to increase speed.
  3. At the point when both legs are not on the ground at the same time, then there begins to be a flight phase. Increasing the length of the flight phase increases the speed.
  4. When the flight phase reaches a configured maximum, then the swing time is decreased until it reaches a configured minimum.
  5. When the swing time is at a configured minimum, the flight time is at a configured maximum, and the legs are moving through their maximum range, then the machine is moving at its maximum speed.

Depending upon the current commanded rotation rate and translation velocity, the distance available for the legs to travel through may change. This uses the same mechanism from the step selection technique to determine the maximum distance at each update cycle, then selects which of the above regimes is active based on the commanded speed.

For a given maximum distance the legs can move through, that results in key gait parameters changing with speed as in the plot below:

Next up I’ll cover the heuristics used to implement any given set of gait parameters in a stable manner.

Unlimited rotations for moteus

The moteus controller has always supported multiple turns when counting positions. It has a one-revolution magnetic encoder built in, but after turn on, it keeps track of how many turns have occurred. However, if you’ve followed previous moteus tutorials, you have probably noticed a persistent caveat that for accurate control, the position of the output shaft needs to stay within a hundred revolutions of 0.0 or so. Now, I’ll describe why that was, and what I’ve done to remove the limitation, allowing unlimited rotations!

Background

The moteus controller uses a somewhat unique integrated position / velocity / torque control loop. This formulation gives a couple of advantages: First, there is no bandwidth loss due to having a cascaded position and velocity controller. Second, when driven by a higher level controller, it can seamlessly switch between position, velocity, and torque control, or any combination of them without having to manage mode transitions.

The command consists of the following values, all as 32 bit floating point values (optionally upscaled from integer values using the register protocol).

  • Desired Position
  • Desired Velocity
  • Feedforward Torque
  • kp scale
  • kd scale
  • Maximum Torque

The control loop measures two quantities as input, the “current position” and the “current velocity”. The position is measured as a 32 bit signed integer, where one revolution of the magnetic encoder equals 65536 counts. The velocity is numerically differentiated across the most recent 6.4ms of movement.

There are two internal state variables as well: One is the “target position”. This captures the most recent position command, and is advanced by the velocity command at the full control rate. The other is the integrative term of the PID controller. Both of these are stored as 32 bit floating point values.

The problem

This structure poses a few inherent limitations. One, being that as the control position is sent as a floating point value, the resolution available for positioning decreases as you get further from 0. That probably isn’t a big limitation, as there aren’t many applications where you want to have both absolute positions and also unlimited revolutions.

The bigger limitation is in the “target position” internal state variable. It needs to be updated to take into account the current velocity command at every control cycle, or 1/40000 of a second. For a commanded speed of 0.01 revolutions per second, this incremental update is only 2.5e-7 of a revolution. Given that 32 bit floating point values only have roughly 7 decimal digits of mantissa available to them, you don’t have to get far beyond 0 before an update that small doesn’t even change the value at all.

The command format also has an option, such that if the command position is set to a floating point NaN value, it will “capture” the current position. This can be used to command velocity-only control with an implicit integrative term, or when combined with a stop position to move to a target at a fixed velocity. However, since “capturing” stores the value as a floating point value, significant precision can be lost. This was only a problem at larger position values, but at the maximum position before wraparound, the available capture resolution was measured in multiple degrees.

The resolution

The resolution was relatively straightforward. Instead of storing the “target position” as a floating point value, it is now stored as a 64 bit integer measured in 1/(2**32) of a magnetic encoder revolution. This gives sufficient precision to represent velocities as small as 0.0001Hz (0.036 dps) uniformly at all positions, while still having more absolute range than the measured current position value. The final PID controller is then expressed relative to the target position. This lets it still operate in floating point coordinates, but with no worry about large artifacts due to a position offset.

The only other implementation hurdle was making it run fast enough. Largely that revolved around ensuring there was never a need to convert between 64 bit integers and floating point values, which is relatively slow on the STM32G4.

The result

With this fix in place, it is possible to operate the controller safely at high velocities for arbitrary periods of time. Even when the “current position” value wraps around from positive to negative! Also, low speed control works just as well at any position offset. When operating in those “continuous rotation” applications, the user should just be careful about if the “desired position” field of the command should be set. Largely, it should be left as NaN for when used in continuous rotation applications.

Here’s a video showing high speed wraparound and low speed at arbitrary offsets.