SmoothingSensor: Experimental sensor angle extrapoltion

developement
6

Create the wrapped sensor like normal, pass it to the SmoothingSensor, and link the SmoothingSensor to the motor. Also set sensor_downsample to something other than 0 or it will sample the sensor every update like usual.

MagneticSensorPWM pwmSensor(your parameters here);
SmoothingSensor(&pwmSensor, &motor);

void setup() {
...
  motor.linkSensor(&smoothingSensor);
  smoothingSensor.sensor_downsample = something;

Now that I think about it, I wonder if it would be better to sample the sensor on a set time interval rather than a set fraction of loop cycles. Something like

class SmoothingSensor : public Sensor
{
public:
  SmoothingSensor(Sensor *s, const FOCMotor *m):wrappedSensor(s),motor(m){}

  float getVelocity() { return wrappedSensor->getVelocity(); }
  int needsSearch() { return wrappedSensor->needsSearch(); }
  float getMechanicalAngle() { return _normalizeAngle(wrappedSensor->getMechanicalAngle() + projectedAngle()); }
  float getAngle() { wrappedSensor->getAngle() + projectedAngle(); }
  double getPreciseAngle() { wrappedSensor->getPreciseAngle() + projectedAngle(); }
  int32_t getFullRotations() { float angle = getAngle(); return lroundf((angle - _normalizeAngle(angle)) / _2PI); }
  
  void update() {
    unsigned long now_us = _micros();
    if (now_us - sample_timestamp >= sample_interval || 
      now_us < sample_timestamp) { // _micros() overflow
    sample_timestamp = now_us;
    wrappedSensor->update();
  }

  unsigned long sample_interval = 0; // how often to sample the sensor, in microseconds

protected:
  float projectedAngle() { return motor->shaft_velocity * (_micros() - wrappedSensor->angle_prev_ts) * 1e-6f; }
  float getSensorAngle() { return wrappedSensor->getSensorAngle(); }
  void init() { wrappedSensor->init(); }
  
  Sensor *wrappedSensor;
  const FOCMotor *motor;
  unsigned long sample_timestamp; // Value of _micros() when sensor was last read
};