Improve pots/encoder safety test

[adeguet1]

The current implementation of the pots/encoder safety check is simplistic, i.e. at each iteration, make sure the difference between the two is under a certain threshold.

This implementation is not working well at high speed (e.g. when engaging sterile adapter or tool). It seems that the potentiometers are fairly slow and/or noisy at high speed so we had to increase the threshold to failry high values (as high as 180 degrees). This renders the safety check close to useless (it will detect catastrophic failures but not slow drifts).

There are a few methods we could use just based on time:

• Trigger the error in the difference is consistantly above threshod for a given amount of time (e.g. 100 ms)
• Maintain a confidence value:
  • If within threshold, multiply by 1.02 (maybe factor in timestamp here so we would be frequency independent)
  • If outside, multiply by 0.98 (maybe factor in timestamp ...)
  • If confidence greater than 0.8, report ok
  • If confidence lesser than 0.2, report error

The issue with these methods is that they will delay the error trigger. We could increase the tolerance based on velocity but this might be an egg and chicken problem, i.e. if the encoder fails we will end up computing a large velocity, increase the tolerance and therefore not detect the failure.

[pkazanzides]

The pots are filtered by a second order (analog) Butterworth filter with a cutoff frequency of 60 Hz. This will add some delay -- I think on the order of 100 msec. Rather than comparing the current pot reading to the current encoder reading, we could compare to a previous encoder reading.