Created SmoothingSensor

The example sketch is untested due to lack of hardware, but minimal changes from the original hall sensor velocity example so it should work.

Author: dekutree64

examples/encoders/smoothing/smoothing.ino

@@ -0,0 +1,141 @@
+/**
+ *
+ * Hall sensor velocity motion control example, modified to demonstrate usage of SmoothingSensor
+ * Steps:
+ * 1) Configure the motor and sensor
+ * 2) Run the code
+ * 3) Set the target velocity (in radians per second) from serial terminal
+ * 4) Try with and without smoothing to see the difference (send E1 and E0 commands from serial terminal)
+ *
+ *
+ *
+ * NOTE :
+ * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor
+ * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager.
+ *
+ * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins
+ * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger
+ *
+ */
+#include <SimpleFOC.h>
+// software interrupt library
+#include <PciManager.h>
+#include <PciListenerImp.h>
+#include <SimpleFOCDrivers.h>
+#include <encoders/smoothing/SmoothingSensor.h>
+
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
+// Stepper motor & driver instance
+//StepperMotor motor = StepperMotor(50);
+//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);
+
+// hall sensor instance
+HallSensor sensor = HallSensor(2, 3, 4, 11);
+// wrapper instance
+SmoothingSensor smooth(sensor, motor);
+
+// Interrupt routine intialisation
+// channel A and B callbacks
+void doA(){sensor.handleA();}
+void doB(){sensor.handleB();}
+void doC(){sensor.handleC();}
+// If no available hadware interrupt pins use the software interrupt
+PciListenerImp listenerIndex(sensor.pinC, doC);
+
+// velocity set point variable
+float target_velocity = 0;
+// instantiate the commander
+Commander command = Commander(Serial);
+void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); }
+
+void enableSmoothing(char* cmd) {
+  float enable;
+  command.scalar(&enable, cmd);
+  motor.linkSensor(enable == 0 ? (Sensor*)&sensor : (Sensor*)&smooth);
+}
+
+void setup() {
+
+  // initialize sensor sensor hardware
+  sensor.init();
+  sensor.enableInterrupts(doA, doB); //, doC);
+  // software interrupts
+  PciManager.registerListener(&listenerIndex);
+  // set SmoothingSensor phase correction for hall sensors
+  smooth.phase_correction = -_PI_6;
+  // link the SmoothingSensor to the motor
+  motor.linkSensor(&smooth);
+
+  // driver config
+  // power supply voltage [V]
+  driver.voltage_power_supply = 12;
+  driver.init();
+  // link the motor and the driver
+  motor.linkDriver(&driver);
+
+  // aligning voltage [V]
+  motor.voltage_sensor_align = 3;
+
+  // set motion control loop to be used
+  motor.controller = MotionControlType::velocity;
+
+  // contoller configuration
+  // default parameters in defaults.h
+
+  // velocity PI controller parameters
+  motor.PID_velocity.P = 0.2f;
+  motor.PID_velocity.I = 2;
+  motor.PID_velocity.D = 0;
+  // default voltage_power_supply
+  motor.voltage_limit = 6;
+  // jerk control using voltage voltage ramp
+  // default value is 300 volts per sec  ~ 0.3V per millisecond
+  motor.PID_velocity.output_ramp = 1000;
+
+  // velocity low pass filtering time constant
+  motor.LPF_velocity.Tf = 0.01f;
+
+  // use monitoring with serial
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+
+  // initialize motor
+  motor.init();
+  // align sensor and start FOC
+  motor.initFOC();
+
+  // add target command T
+  command.add('T', doTarget, "target voltage");
+
+  // add smoothing enable/disable command E (send E0 to use hall sensor alone, or E1 to use smoothing)
+  command.add('E', enableSmoothing, "enable smoothing");
+
+  Serial.println(F("Motor ready."));
+  Serial.println(F("Set the target velocity using serial terminal:"));
+  _delay(1000);
+}
+
+
+void loop() {
+  // main FOC algorithm function
+  // the faster you run this function the better
+  // Arduino UNO loop  ~1kHz
+  // Bluepill loop ~10kHz
+  motor.loopFOC();
+
+  // Motion control function
+  // velocity, position or voltage (defined in motor.controller)
+  // this function can be run at much lower frequency than loopFOC() function
+  // You can also use motor.move() and set the motor.target in the code
+  motor.move(target_velocity);
+
+  // function intended to be used with serial plotter to monitor motor variables
+  // significantly slowing the execution down!!!!
+  // motor.monitor();
+
+  // user communication
+  command.run();
+}

src/encoders/smoothing/README.md

@@ -0,0 +1,69 @@
+# Smoothing Sensor
+
+by [@dekutree64](https://github.com/dekutree64)
+
+A SimpleFOC Sensor wrapper implementation which adds angle extrapolation
+
+Please also see our [forum thread](https://community.simplefoc.com/t/smoothingsensor-experimental-sensor-angle-extrapoltion/3105) on this topic.
+
+
+SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better quality angle readings from sensors that are low resolution or are slow to communicate. It provides no improvement for sensors that are high resolution and quick to update. It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity from the motor to predict what the angle should be at the current time.
+
+
+## Hardware setup
+
+Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without smoothing first. Once things are working and tuned without smoothing, you can add the SmoothingSensor to see if you get an improvement.
+
+
+## Softwate setup
+
+The SmoothingSensor acts as a wrapper to the actual sensor class. When creating the SmoothingSensor object, provide the real sensor to the constructor of SmoothingSensor. Initialize the real sensor instance as normal. Then link the SmoothingSensor to the motor and call motor.initFOC().
+
+
+```c++
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9);
+// real sensor instance
+MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
+// instantiate the smoothing sensor, providing the real sensor as a constructor argument
+SmoothingSensor smooth = SmoothingSensor(sensor, motor);
+
+void doPWM(){sensor.handlePWM();}
+
+void setup() {
+  sensor.init();
+  sensor.enableInterrupt(doPWM);
+  // Link motor to sensor
+  motor.linkSensor(&smooth);
+  // power supply voltage
+  driver.voltage_power_supply = 20;
+  driver.init();
+  motor.linkDriver(&driver);
+  // aligning voltage 
+  motor.voltage_sensor_align = 8;
+  motor.voltage_limit = 20;
+  // set motion control loop to be used
+  motor.controller = MotionControlType::torque;
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+  motor.monitor_variables =  _MON_VEL; 
+  motor.monitor_downsample = 10; // default 10
+
+  // initialize motor
+  motor.init();
+  motor.initFOC();
+}
+```
+
+
+## Roadmap
+
+Possible future improvements we've thought about:
+
+- Add extrapolation of acceleration as well
+- Switch to open loop control at very low speed with hall sensors, which otherwise move in steps even with smoothing.
+

src/encoders/smoothing/SmoothingSensor.cpp

@@ -0,0 +1,57 @@
+#include "SmoothingSensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+
+SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m)
+{
+}
+
+
+void SmoothingSensor::update() {
+  // Update sensor, with optional downsampling of update rate
+  if(sensor_cnt++ >= sensor_downsample) {
+    sensor_cnt = 0;
+    _wrapped.update();
+  }
+
+  // Copy state variables from the sensor
+  angle_prev = _wrapped.angle_prev;
+  angle_prev_ts = _wrapped.angle_prev_ts;
+  full_rotations = _wrapped.full_rotations;
+
+  // Perform angle prediction, using low-pass filtered velocity. But don't advance more than
+  // pi/3 (equivalent to one step of block commutation) from the last true angle reading.
+  float dt = (_micros() - angle_prev_ts) * 1e-6f;
+  angle_prev += _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs);
+
+  // Apply phase correction if needed
+  if (phase_correction != 0) {
+    if (_motor.shaft_velocity < -0) angle_prev -= phase_correction / _motor.pole_pairs;
+    else if (_motor.shaft_velocity > 0) angle_prev += phase_correction / _motor.pole_pairs;
+  }
+
+  // Handle wraparound of the projected angle
+  if (angle_prev < 0) full_rotations -= 1, angle_prev += _2PI;
+  else if (angle_prev >= _2PI) full_rotations += 1, angle_prev -= _2PI;
+}
+
+
+float SmoothingSensor::getVelocity() {
+  return _wrapped.getVelocity();
+}
+
+
+int SmoothingSensor::needsSearch() {
+  return _wrapped.needsSearch();
+}
+
+
+float SmoothingSensor::getSensorAngle() {
+  return _wrapped.getSensorAngle();
+}
+
+
+void SmoothingSensor::init() {
+  _wrapped.init();
+}

src/encoders/smoothing/SmoothingSensor.h

@@ -0,0 +1,46 @@
+#ifndef SMOOTHING_SENSOR_H
+#define SMOOTHING_SENSOR_H
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+
+/**
+  SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better
+  quality angle readings from sensors that are low resolution or are slow to communicate. It provides
+  no improvement for sensors that are high resolution and quick to update.
+  It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity
+  from the motor to predict what the angle should be at the current time.
+*/
+
+class SmoothingSensor : public Sensor
+{
+  public:
+    /**
+    SmoothingSensor class constructor
+    @param s  Wrapped sensor
+    @param m  Motor that the SmoothingSensor will be linked to
+    */
+    SmoothingSensor(Sensor& s, const FOCMotor& m);
+
+    void update() override;
+    float getVelocity() override;
+    int needsSearch() override;
+
+    // For sensors with slow communication, use these to poll less often
+    unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update
+    unsigned int sensor_cnt = 0; // counting variable for downsampling
+
+    // For hall sensors, the predicted angle is always 0 to 60 electrical degrees ahead of where it would be without
+    // smoothing, so offset it backward by 30 degrees (set this to -PI_6) to get the average in phase with the rotor
+    float phase_correction = 0;
+
+  protected:
+    float getSensorAngle() override;
+    void init() override;
+
+    Sensor& _wrapped;
+    const FOCMotor& _motor;
+};
+
+#endif