add HysteresisSensor

Author: Richard Unger

src/encoders/hysteresis/HysteresisSensor.cpp

@@ -0,0 +1,46 @@
+
+#include "./HysteresisSensor.h"
+
+
+HysteresisSensor::HysteresisSensor(Sensor& wrapped, float amount) : _wrapped(wrapped), _amount(amount) {
+    // empty
+};
+
+
+void HysteresisSensor::init() {
+    _wrapped.update();
+    _window = _wrapped.getMechanicalAngle();
+    this->Sensor::init();
+};
+
+float HysteresisSensor::getSensorAngle() {
+    _wrapped.update();
+    float raw = _wrapped.getMechanicalAngle();
+
+    float d_angle = raw - _window;
+    if(abs(d_angle) > (0.8f*_2PI) ) {
+        if (d_angle > 0) {
+            if (raw < (_2PI - _amount + _window)) {
+                _window = _normalizeAngle(raw + _amount);
+                return raw;
+            }
+        } else {
+            if (raw > (_amount - (_2PI - _window))) {
+                _window = _normalizeAngle(raw - _amount);
+                return raw;
+            }
+        }
+    }
+    else  {
+        if (raw > (_window + _amount)) {
+            _window = _normalizeAngle(raw - _amount);
+            return raw;
+        }
+        if (raw < (_window - _amount)) {
+            _window = _normalizeAngle(raw + _amount);
+            return raw;
+        }
+    }
+    return angle_prev; // no change
+};
+

src/encoders/hysteresis/HysteresisSensor.h

@@ -0,0 +1,23 @@
+
+#pragma once
+
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+
+class HysteresisSensor : public Sensor {
+public:
+    HysteresisSensor(Sensor& wrapped, float amount = 0.0125f);
+
+    float getSensorAngle() override;
+
+    void init() override;
+
+    float _amount;
+protected:
+    Sensor& _wrapped;
+    float _window;
+
+};
+

src/encoders/hysteresis/README.md

@@ -0,0 +1,36 @@
+
+# HysteresisSensor
+
+A simple wrapper sensor which adds a configurable amount of hysteresis to any other sensor.
+
+Set the amount of hysteresis, in rads. The hysteresis window will have a width of twice the set
+amount.
+
+## Usage
+
+```c++
+// for example, using a MA730 as the real sensor
+MagneticSensorMA730 sensor = MagneticSensorMA730(PIN_nCS);
+// wrapping sensor with hysteresis
+HysteresisSensor hysteresisSensor = HysteresisSensor(sensor, 0.01f);
+
+void setup() {
+    ...
+
+    // init real sensor
+    sensor.init();
+    hysteresisSensor.init();
+    motor.linkSensor(&hysteresisSensor);
+
+    ...
+
+    // it may be better to run the calibration without hysteresis...
+    hysteresisSensor._amount = 0.0f;
+    motor.init();
+    motor.initFOC();
+    // switch it back on afterwards
+    hysteresisSensor._amount = 0.01f;
+
+    ...
+}
+```