New stepper movement strategy

[MichaelJFr]

This set of changes adds a new "force targeting" movement strategy which requests a target force value for the current pedal position and then attempts to move the pedal so that the measured force will match that value.

This is conceptually somewhat reversed from the existing strategies which measure the current force and calculate a target position based as a function of the force.

The new strategy is disabled by default, but can be swapped in using the new #defines in Main.ino.

[ChrGri]

@MichaelJFr for digital filtering of the noisy ADC data, a Kalman filter (KF) has been implemented, which currently observes the force and the force change rate. It can therefore quickly respond to dynamic force changes without much lag. Much better than what a simple moving averaging (MA) filter can achieve. The ADC, unfortunately, behaves like a MA itself. Reducing the ADC sample rate will increase the MA filter impact and therefore will introduce lag. I would prefer relying more on the KF prediction than a simple MA, like that of the ADC.

Furthermore, in the past, I observed significant pedal oscillations for cycle times in the millisecond regions. They were mostly mitigated at cycle times in the us region.

I will try your changes, once back home again.

[MichaelJFr]

My understanding of what's happening in the FastAccelStepper library is that the stepper trajectory only gets updated every 4ms, no matter how many updates we send to it in the intervening time. Based on that, my thinking is that we want to feed it movement data (a) at least that often; and (b) as accurately as possible; so that it doesn't drive in the wrong direction for the next 4ms based on a noisy value.

The change seems good on my test setup, but since I'm not yet using the pedal for any real driving and I guess we're likely to be seeing different amounts and types of noise - I'll be interested to hear your findings. The Kalman filter is still in the loop, but I guess operating differently since the noise profile will be different after ADC averaging. I confess I haven't understood the maths of the Kalman stuff well enough to assess the impact of that.

[ChrGri]

It's correct, that the trajectory is updated only every 4ms. During this interval, the stepper library still outputs steps depending on acceleration and velocity. If the force applied to the loadcell is stationary, the MA filter would perform perfect. If, however, during the ADC sample time, the force changes, the ADC reading will lag. These force changes are introduced, e.g. by the user but also by the pedal-to-user interaction. Imagine, you hold your foot at a certain position. If the stepper moves the pedal towards your foot, the loadcell would measure increased force. Vice versa, when the pedal is moved in the opposite direction. A typical example of this is the ABS oscillation. Even with the current KF implementation, you will see force and therefore controller output fluctuations when ABS is active. In the future, I want to update the KF to predict the force, taking into account the pedal movement and hopefully completely remove these force fluctuations when ABS is active.

In short, the problem is, that during these 4ms the applied force isn't stationary.

[MichaelJFr]

That all basically makes sense to me. I've been doing some testing with the force values turned down (to a light accelerator pedal type feeling), and in that scenario the signal-to-noise ratio from the load cell is significantly worse. I think that probably changes the tradeoff between lag and noise in the readings, and could be why I was seeing the version with more averaging as the better option.

Anyway, I'm happy to keep testing and tweaking. Would you like me to redo the pull request without the sample rate change?

[ChrGri]

Don't bother. I've ordered parts for a second pedal and be happy to test your changes. I'm a bit surprised though, as I haven't observed step fluctuations at any load and therefore found the filtering to be sufficient. Perhaps there is some hardware related SNR difference between our pedals. Once I'm back home, we need to compare the noise estimates from the pedals.

[ChrGri]

Hi @MichaelJFr, I've build myself a second pedal to be able to compare different FW versions. I've flashed your FW to one pedal and tested the noise behavior for accelerator pedal config. I did not notice significant difference between the pedals. I did manage to get a great accelerator pedal behavior by reducing the P-gain (Pr1.00 = 150) & FIR filter (Pr2.23=300) settings in the servo configuration. For the above mentioned reasons, I'd prefer to stick with the higher sampling rate.

The proper force calculation is awesome though. Could you explain the algorithm behind the new movement strategy?

BR Chris

[MichaelJFr]

The main reason I've been working towards a different movement strategy is to support more unusual/less linear force curves, thinking particularly of things like clutch pedals with bite points.

Fundamentally, the limitation of the existing approach is that it maps a force reading directly to a target move position. This means that you can't have 2 pedal positions for which the static force required to hold the pedal still (henceforth "holding force") is the same, so you can't have a constant holding force through the pedal travel (see ForceCurve_ConstantForce) or a curve where there is a peak holding force which then drops off (as you would have in a clutch pedal at the bite point).

The idea is that we should instead look at the current pedal position, and compare the holding force at that position to the actual measured force, yielding the current "force error". The force error then determines whether to move the pedal up or down, attempting to find a position where the measured force will match the holding force.

That's the theory, once I started to try to implement it I found that I had a lot of trouble determining the correct amount of movement to apply for a given force error value. In particular, steep force curves can lead to issues with the naive approach, the "overshoot" part of the code is trying to correct for scenarios where you're driving "up" a force curve and going too far could put you well underneath it, leading to oscillations.

I think there should be a way to factor in the current holding force gradient as well as the static holding force which would resolve some of those issues more cleanly, but I haven't had a lot of spare time in the last couple of weeks to look at it.

[ChrGri]

The main reason I've been working towards a different movement strategy is to support more unusual/less linear force curves, thinking particularly of things like clutch pedals with bite points.

Fundamentally, the limitation of the existing approach is that it maps a force reading directly to a target move position. This means that you can't have 2 pedal positions for which the static force required to hold the pedal still (henceforth "holding force") is the same, so you can't have a constant holding force through the pedal travel (see ForceCurve_ConstantForce) or a curve where there is a peak holding force which then drops off (as you would have in a clutch pedal at the bite point).

The idea is that we should instead look at the current pedal position, and compare the holding force at that position to the actual measured force, yielding the current "force error". The force error then determines whether to move the pedal up or down, attempting to find a position where the measured force will match the holding force.

That's the theory, once I started to try to implement it I found that I had a lot of trouble determining the correct amount of movement to apply for a given force error value. In particular, steep force curves can lead to issues with the naive approach, the "overshoot" part of the code is trying to correct for scenarios where you're driving "up" a force curve and going too far could put you well underneath it, leading to oscillations.

I think there should be a way to factor in the current holding force gradient as well as the static holding force which would resolve some of those issues more cleanly, but I haven't had a lot of spare time in the last couple of weeks to look at it.

For me this sounds exactly like the what is already implemented with "INTERP_SPRING_STIFFNESS". At first, a curve is fitted to the parameterized force points, then a curve is interpolated, lastly, the force gradient is calculated along these curve. In the pedalUpdate task, the force gradient at the current pedal positions is used to calculate the new target position. As long as the cycleTime is relatively short, the prediction gets better, as the position between the cycles doesn't change a lot. With the short cycle time, even at high speed, the position cannot change only a few <10 steps per cycle. I tried multiple interpolation methods and currently like linear interpolation the most.

[MichaelJFr]

Consider the case where we want the pedal resistance (holding force) to be constant throughout the range of pedal motion. Specifying the force values as { 0:10, 20:10, 40:10, 60:10, 80:10, 100:10 }, then I believe springStiffness will be zero, and springStiffnessInv will be infinite.

Granted, you probably wouldn't want a fully constant resistance in a real pedal, though many throttle pedals aren't very far off. A clutch pedal though has a peak in the force curve at the bite point, perhaps something like { 0:10, 20:30, 40:50, 60:60, 80:30, 100:10 }. This means that at the peak of the curve it will have this zero gradient problem, and then beyond that a negative gradient, which comes with a new set of issues.

[ChrGri]

I understand what you are referring to --> When setting a local maxima in the middle travel range, e.g. as shown for a clutch pedal by VRS, it's hard to balance on the maxima. As soon as the maxima is reached, the pedal drops forward. Judging from these static force curve, I would say, that it's a plausible behavior and I don't understand the technical reason yet, why it's easier to balance a real clutch at the maxima. Once we get a deeper understanding of the root cause, we can try to mimic that behavior.

[MichaelJFr]

OK. I'm going to keep working on developing this approach, we can revisit whether to merge it at a later date if it proves fruitful.

I'll close this request for now and open a new one with only the more minor changes applied, just to avoid our code trees drifting too far apart.

[ChrGri]

@MichaelJFr I've played around with a PID to controll the force by adjusting the stepper position, similar to your implementation. The behaviour around local maxima is more realistic, but the pedal sometimes oscillates. As a first step to tackle this problem, I've started adding a cubic spline interpolation to the SimHub plugin to get a smoother interpolant, compared to the linear interpolation. Next, the spline parameters need to be send to the esp32 and corresponding interpolation code has be implemented. Afterwards, I might be playing around with some model predictive control.

[MichaelJFr]

Oh nice, that sounds like it could be a really neat solution.

[ChrGri]

@MichaelJFr I've updated the SimHub plugin and the ESP code. The PID strategy is now activated by default and the PID parameters can be configured via SimHub. After pressing the "Send config to pedal" in SimHub, one has to power cycle the esp, as there seems to be a bug left with the parameter update. Going to debug that next. Reasonable starting PID values are P=0.3, I=50, D=0. You would do me a favour, testing the changes and letting me know, whether you think that the pedal feel has improved.

And don't even try changing the D-gain...

---

Edit: Target code has been fixed & slightly refactored. Little D-gain can now be used.

[MichaelJFr]

I only just got some time to load this up and try this out, seems like great progress. I'm still getting some oscillation when I mess around with the force curve values too much, but I haven't even touched the available tuning parameters yet so that's no surprise. Nice work!

I have a couple of changes that I was working on last week that improve task scheduling behaviour when running on single-core chips (ie ESP32 S2) - I'll get them merged with your latest and hopefully can offer a pull request in the next few days.

[ChrGri]

Thanks a lot. It's good to hear, that development is going in the right direction.

To help finding the right PID parameters, a routine for system identification was added. One has to press on the pedal and then trigger the routine. The pedal will make some moves and the pedal position and loadcell reading is printed. The SimHub plugin creates an export, which can be used via e.g. https://pidtuner.com to identify the system model and find proper PID values. Still under development.