Strain calibration

firmware/src/configuration.cpp

@@ -40,6 +40,7 @@ bool Configuration::loadFromDisk() {
         char buf[200];
         snprintf(buf, sizeof(buf), "Decoding failed: %s", PB_GET_ERROR(&stream));
         log(buf);
+        pb_buffer_ = {};
         return false;
     }
 
@@ -47,6 +48,7 @@ bool Configuration::loadFromDisk() {
         char buf[200];
         snprintf(buf, sizeof(buf), "Invalid config version. Expected %u, received %u", PERSISTENT_CONFIGURATION_VERSION, pb_buffer_.version);
         log(buf);
+        pb_buffer_ = {};
         return false;
     }
     loaded_ = true;
@@ -118,6 +120,15 @@ bool Configuration::setMotorCalibrationAndSave(PB_MotorCalibration& motor_calibr
     return saveToDisk();
 }
 
+bool Configuration::setStrainCalibrationAndSave(PB_StrainCalibration& strain_calibration) {
+    {
+        SemaphoreGuard lock(mutex_);
+        pb_buffer_.strain = strain_calibration;
+        pb_buffer_.has_strain = true;
+    }
+    return saveToDisk();
+}
+
 void Configuration::setLogger(Logger* logger) {
     logger_ = logger;
 }

firmware/src/configuration.h

@@ -19,6 +19,7 @@ class Configuration {
         bool saveToDisk();
         PB_PersistentConfiguration get();
         bool setMotorCalibrationAndSave(PB_MotorCalibration& motor_calibration);
+        bool setStrainCalibrationAndSave(PB_StrainCalibration& strain_calibration);
 
     private:
         SemaphoreHandle_t mutex_;

firmware/src/interface_task.cpp

@@ -11,6 +11,7 @@
 #endif
 
 #include "interface_task.h"
+#include "semaphore_guard.h"
 #include "util.h"
 
 #if SK_LEDS
@@ -208,7 +209,7 @@ static PB_SmartKnobConfig configs[] = {
 };
 
 InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : 
-        Task("Interface", 3000, 1, task_core),
+        Task("Interface", 3400, 1, task_core),
         stream_(),
         motor_task_(motor_task),
         display_task_(display_task),
@@ -223,6 +224,13 @@ InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, Dis
 
     knob_state_queue_ = xQueueCreate(1, sizeof(PB_SmartKnobState));
     assert(knob_state_queue_ != NULL);
+
+    mutex_ = xSemaphoreCreateMutex();
+    assert(mutex_ != NULL);
+}
+
+InterfaceTask::~InterfaceTask() {
+    vSemaphoreDelete(mutex_);
 }
 
 void InterfaceTask::run() {
@@ -252,11 +260,37 @@ void InterfaceTask::run() {
     motor_task_.setConfig(configs[0]);
     motor_task_.addListener(knob_state_queue_);
 
-
-    // Start in legacy protocol mode
     plaintext_protocol_.init([this] () {
         changeConfig(true);
+    }, [this] () {
+        if (!configuration_loaded_) {
+            return;
+        }
+        if (strain_calibration_step_ == 0) {
+            log("Strain calibration step 1: Don't touch the knob, then press 'S' again");
+            strain_calibration_step_ = 1;
+        } else if (strain_calibration_step_ == 1) {
+            configuration_value_.strain.idle_value = strain_reading_;
+            snprintf(buf_, sizeof(buf_), "  idle_value=%d", configuration_value_.strain.idle_value);
+            log(buf_);
+            log("Strain calibration step 2: Push and hold down the knob with medium pressure, and press 'S' again");
+            strain_calibration_step_ = 2;
+        } else if (strain_calibration_step_ == 2) {
+            configuration_value_.strain.press_delta = strain_reading_ - configuration_value_.strain.idle_value;
+            configuration_value_.has_strain = true;
+            snprintf(buf_, sizeof(buf_), "  press_delta=%d", configuration_value_.strain.press_delta);
+            log(buf_);
+            log("Strain calibration complete! Saving...");
+            strain_calibration_step_ = 0;
+            if (configuration_->setStrainCalibrationAndSave(configuration_value_.strain)) {
+                log("  Saved!");
+            } else {
+                log("  FAILED to save config!!!");
+            }
+        }
     });
+
+    // Start in legacy protocol mode
     SerialProtocol* current_protocol = &plaintext_protocol_;
 
     ProtocolChangeCallback protocol_change_callback = [this, &current_protocol] (uint8_t protocol) {
@@ -293,6 +327,14 @@ void InterfaceTask::run() {
 
         updateHardware();
 
+        if (!configuration_loaded_) {
+            SemaphoreGuard lock(mutex_);
+            if (configuration_ != nullptr) {
+                configuration_value_ = configuration_->get();
+                configuration_loaded_ = true;
+            }
+        }
+
         delay(1);
     }
 }
@@ -316,7 +358,6 @@ void InterfaceTask::changeConfig(bool next) {
         }
     }
     
-    char buf_[256];
     snprintf(buf_, sizeof(buf_), "Changing config to %d -- %s", current_config_, configs[current_config_].text);
     log(buf_);
     motor_task_.setConfig(configs[current_config_]);
@@ -332,7 +373,7 @@ void InterfaceTask::updateHardware() {
         float lux = veml.readLux();
         lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
         static uint32_t last_als;
-        if (millis() - last_als > 1000) {
+        if (millis() - last_als > 1000 && strain_calibration_step_ == 0) {
             snprintf(buf_, sizeof(buf_), "millilux: %.2f", lux*1000);
             log(buf_);
             last_als = millis();
@@ -341,40 +382,38 @@ void InterfaceTask::updateHardware() {
 
     #if SK_STRAIN
         if (scale.wait_ready_timeout(100)) {
-            int32_t reading = scale.read();
+            strain_reading_ = scale.read();
 
             static uint32_t last_reading_display;
-            if (millis() - last_reading_display > 1000) {
-                snprintf(buf_, sizeof(buf_), "HX711 reading: %d", reading);
+            if (millis() - last_reading_display > 1000 && strain_calibration_step_ == 0) {
+                snprintf(buf_, sizeof(buf_), "HX711 reading: %d", strain_reading_);
                 log(buf_);
                 last_reading_display = millis();
             }
-
-            // TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
-            const int32_t lower = 950000;
-            const int32_t upper = 1800000;
-            // Ignore readings that are way out of expected bounds
-            if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
-                long value = CLAMP(reading, lower, upper);
-                press_value_unit = 1. * (value - lower) / (upper - lower);
-
-                static bool pressed;
-                static uint8_t press_count;
-                if (!pressed && press_value_unit > 0.75) {
-                    press_count++;
-                    if (press_count > 2) {
-                        motor_task_.playHaptic(true);
-                        pressed = true;
-                        changeConfig(true);
-                    }
-                } else if (pressed && press_value_unit < 0.25) {
-                    press_count++;
-                    if (press_count > 2) {
-                        motor_task_.playHaptic(false);
-                        pressed = false;
+            if (configuration_loaded_ && configuration_value_.has_strain && strain_calibration_step_ == 0) {
+                // TODO: calibrate and track (long term moving average) idle point (lower)
+                press_value_unit = lerp(strain_reading_, configuration_value_.strain.idle_value, configuration_value_.strain.idle_value + configuration_value_.strain.press_delta, 0, 1);
+
+                // Ignore readings that are way out of expected bounds
+                if (-1 < press_value_unit && press_value_unit < 2) {
+                    static bool pressed;
+                    static uint8_t press_count;
+                    if (!pressed && press_value_unit > 0.75) {
+                        press_count++;
+                        if (press_count > 2) {
+                            motor_task_.playHaptic(true);
+                            pressed = true;
+                            changeConfig(true);
+                        }
+                    } else if (pressed && press_value_unit < 0.25) {
+                        press_count++;
+                        if (press_count > 2) {
+                            motor_task_.playHaptic(false);
+                            pressed = false;
+                        }
+                    } else {
+                        press_count = 0;
                     }
-                } else {
-                    press_count = 0;
                 }
             }
         } else {
@@ -401,7 +440,7 @@ void InterfaceTask::updateHardware() {
 
     #if SK_LEDS
         for (uint8_t i = 0; i < NUM_LEDS; i++) {
-            leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
+            leds[i].setHSV(200 * CLAMP(press_value_unit, (float)0, (float)1), 255, brightness >> 8);
 
             // Gamma adjustment
             leds[i].r = dim8_video(leds[i].r);
@@ -411,3 +450,8 @@ void InterfaceTask::updateHardware() {
         FastLED.show();
     #endif
 }
+
+void InterfaceTask::setConfiguration(Configuration* configuration) {
+    SemaphoreGuard lock(mutex_);
+    configuration_ = configuration;
+}

firmware/src/interface_task.h

@@ -3,6 +3,7 @@
 #include <AceButton.h>
 #include <Arduino.h>
 
+#include "configuration.h"
 #include "display_task.h"
 #include "logger.h"
 #include "motor_task.h"
@@ -16,9 +17,10 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger {
 
     public:
         InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task);
-        virtual ~InterfaceTask() {};
+        virtual ~InterfaceTask();
 
         void log(const char* msg) override;
+        void setConfiguration(Configuration* configuration);
 
     protected:
         void run();
@@ -31,7 +33,16 @@ class InterfaceTask : public Task<InterfaceTask>, public Logger {
     #endif
         MotorTask& motor_task_;
         DisplayTask* display_task_;
-        char buf_[64];
+        char buf_[128];
+
+        SemaphoreHandle_t mutex_;
+        Configuration* configuration_ = nullptr; // protected by mutex_
+
+        PB_PersistentConfiguration configuration_value_;
+        bool configuration_loaded_ = false;
+
+        uint8_t strain_calibration_step_ = 0;
+        int32_t strain_reading_ = 0;
 
         int current_config_ = 0;
 

firmware/src/main.cpp

@@ -32,6 +32,8 @@ void setup() {
   config.setLogger(&interface_task);
   config.loadFromDisk();
 
+  interface_task.setConfiguration(&config);
+
   motor_task.setLogger(&interface_task);
   motor_task.begin();
 

firmware/src/proto_gen/smartknob.pb.c

@@ -33,4 +33,7 @@ PB_BIND(PB_PersistentConfiguration, PB_PersistentConfiguration, AUTO)
 PB_BIND(PB_MotorCalibration, PB_MotorCalibration, AUTO)
 
 
+PB_BIND(PB_StrainCalibration, PB_StrainCalibration, AUTO)
+
+
 

firmware/src/proto_gen/smartknob.pb.h

@@ -80,10 +80,17 @@ typedef struct _PB_MotorCalibration {
     uint32_t pole_pairs;
 } PB_MotorCalibration;
 
+typedef struct _PB_StrainCalibration {
+    int32_t idle_value;
+    int32_t press_delta;
+} PB_StrainCalibration;
+
 typedef struct _PB_PersistentConfiguration {
     uint32_t version;
     bool has_motor;
     PB_MotorCalibration motor;
+    bool has_strain;
+    PB_StrainCalibration strain;
 } PB_PersistentConfiguration;
 
 
@@ -99,8 +106,9 @@ extern "C" {
 #define PB_SmartKnobState_init_default           {0, 0, false, PB_SmartKnobConfig_init_default}
 #define PB_SmartKnobConfig_init_default          {0, 0, 0, 0, 0, 0, 0, 0, 0, "", 0, {0, 0, 0, 0, 0}, 0}
 #define PB_RequestState_init_default             {0}
-#define PB_PersistentConfiguration_init_default  {0, false, PB_MotorCalibration_init_default}
+#define PB_PersistentConfiguration_init_default  {0, false, PB_MotorCalibration_init_default, false, PB_StrainCalibration_init_default}
 #define PB_MotorCalibration_init_default         {0, 0, 0, 0}
+#define PB_StrainCalibration_init_default        {0, 0}
 #define PB_FromSmartKnob_init_zero               {0, 0, {PB_Ack_init_zero}}
 #define PB_ToSmartknob_init_zero                 {0, 0, 0, {PB_RequestState_init_zero}}
 #define PB_Ack_init_zero                         {0}
@@ -108,8 +116,9 @@ extern "C" {
 #define PB_SmartKnobState_init_zero              {0, 0, false, PB_SmartKnobConfig_init_zero}
 #define PB_SmartKnobConfig_init_zero             {0, 0, 0, 0, 0, 0, 0, 0, 0, "", 0, {0, 0, 0, 0, 0}, 0}
 #define PB_RequestState_init_zero                {0}
-#define PB_PersistentConfiguration_init_zero     {0, false, PB_MotorCalibration_init_zero}
+#define PB_PersistentConfiguration_init_zero     {0, false, PB_MotorCalibration_init_zero, false, PB_StrainCalibration_init_zero}
 #define PB_MotorCalibration_init_zero            {0, 0, 0, 0}
+#define PB_StrainCalibration_init_zero           {0, 0}
 
 /* Field tags (for use in manual encoding/decoding) */
 #define PB_Ack_nonce_tag                         1
@@ -141,8 +150,11 @@ extern "C" {
 #define PB_MotorCalibration_zero_electrical_offset_tag 2
 #define PB_MotorCalibration_direction_cw_tag     3
 #define PB_MotorCalibration_pole_pairs_tag       4
+#define PB_StrainCalibration_idle_value_tag      1
+#define PB_StrainCalibration_press_delta_tag     2
 #define PB_PersistentConfiguration_version_tag   1
 #define PB_PersistentConfiguration_motor_tag     2
+#define PB_PersistentConfiguration_strain_tag    3
 
 /* Struct field encoding specification for nanopb */
 #define PB_FromSmartKnob_FIELDLIST(X, a) \
@@ -207,10 +219,12 @@ X(a, STATIC,   SINGULAR, FLOAT,    snap_point_bias,  12)
 
 #define PB_PersistentConfiguration_FIELDLIST(X, a) \
 X(a, STATIC,   SINGULAR, UINT32,   version,           1) \
-X(a, STATIC,   OPTIONAL, MESSAGE,  motor,             2)
+X(a, STATIC,   OPTIONAL, MESSAGE,  motor,             2) \
+X(a, STATIC,   OPTIONAL, MESSAGE,  strain,            3)
 #define PB_PersistentConfiguration_CALLBACK NULL
 #define PB_PersistentConfiguration_DEFAULT NULL
 #define PB_PersistentConfiguration_motor_MSGTYPE PB_MotorCalibration
+#define PB_PersistentConfiguration_strain_MSGTYPE PB_StrainCalibration
 
 #define PB_MotorCalibration_FIELDLIST(X, a) \
 X(a, STATIC,   SINGULAR, BOOL,     calibrated,        1) \
@@ -220,6 +234,12 @@ X(a, STATIC,   SINGULAR, UINT32,   pole_pairs,        4)
 #define PB_MotorCalibration_CALLBACK NULL
 #define PB_MotorCalibration_DEFAULT NULL
 
+#define PB_StrainCalibration_FIELDLIST(X, a) \
+X(a, STATIC,   SINGULAR, INT32,    idle_value,        1) \
+X(a, STATIC,   SINGULAR, INT32,    press_delta,       2)
+#define PB_StrainCalibration_CALLBACK NULL
+#define PB_StrainCalibration_DEFAULT NULL
+
 extern const pb_msgdesc_t PB_FromSmartKnob_msg;
 extern const pb_msgdesc_t PB_ToSmartknob_msg;
 extern const pb_msgdesc_t PB_Ack_msg;
@@ -229,6 +249,7 @@ extern const pb_msgdesc_t PB_SmartKnobConfig_msg;
 extern const pb_msgdesc_t PB_RequestState_msg;
 extern const pb_msgdesc_t PB_PersistentConfiguration_msg;
 extern const pb_msgdesc_t PB_MotorCalibration_msg;
+extern const pb_msgdesc_t PB_StrainCalibration_msg;
 
 /* Defines for backwards compatibility with code written before nanopb-0.4.0 */
 #define PB_FromSmartKnob_fields &PB_FromSmartKnob_msg
@@ -240,16 +261,18 @@ extern const pb_msgdesc_t PB_MotorCalibration_msg;
 #define PB_RequestState_fields &PB_RequestState_msg
 #define PB_PersistentConfiguration_fields &PB_PersistentConfiguration_msg
 #define PB_MotorCalibration_fields &PB_MotorCalibration_msg
+#define PB_StrainCalibration_fields &PB_StrainCalibration_msg
 
 /* Maximum encoded size of messages (where known) */
 #define PB_Ack_size                              6
 #define PB_FromSmartKnob_size                    264
 #define PB_Log_size                              258
 #define PB_MotorCalibration_size                 15
-#define PB_PersistentConfiguration_size          23
+#define PB_PersistentConfiguration_size          47
 #define PB_RequestState_size                     0
 #define PB_SmartKnobConfig_size                  173
 #define PB_SmartKnobState_size                   192
+#define PB_StrainCalibration_size                22
 #define PB_ToSmartknob_size                      185
 
 #ifdef __cplusplus

firmware/src/serial/serial_protocol_plaintext.cpp

@@ -41,11 +41,16 @@ void SerialProtocolPlaintext::loop() {
             }
         } else if (b == 'C') {
             motor_task_.runCalibration();
+        } else if (b == 'S') {
+            if (strain_calibration_callback_) {
+                strain_calibration_callback_();
+            }
         }
     }
 }
 
-void SerialProtocolPlaintext::init(DemoConfigChangeCallback cb) {
-    demo_config_change_callback_ = cb;
-    stream_.println("SmartKnob starting!\n\nSerial mode: plaintext\nPress 'C' at any time to calibrate.\nPress <Space> to change haptic modes.");
+void SerialProtocolPlaintext::init(DemoConfigChangeCallback demo_config_change_callback, StrainCalibrationCallback strain_calibration_callback) {
+    demo_config_change_callback_ = demo_config_change_callback;
+    strain_calibration_callback_ = strain_calibration_callback;
+    stream_.println("SmartKnob starting!\n\nSerial mode: plaintext\nPress 'C' at any time to calibrate motor/sensor.\nPress 'S' at any time to calibrate strain sensors.\nPress <Space> to change haptic modes.");
 }

firmware/src/serial/serial_protocol_plaintext.h

@@ -7,6 +7,7 @@
 #include "uart_stream.h"
 
 typedef std::function<void(void)> DemoConfigChangeCallback;
+typedef std::function<void(void)> StrainCalibrationCallback;
 
 class SerialProtocolPlaintext : public SerialProtocol {
     public:
@@ -16,11 +17,12 @@ class SerialProtocolPlaintext : public SerialProtocol {
         void loop() override;
         void handleState(const PB_SmartKnobState& state) override;
 
-        void init(DemoConfigChangeCallback cb);
+        void init(DemoConfigChangeCallback demo_config_change_callback, StrainCalibrationCallback strain_calibration_callback);
     
     private:
         Stream& stream_;
         MotorTask& motor_task_;
         PB_SmartKnobState latest_state_ = {};
         DemoConfigChangeCallback demo_config_change_callback_;
+        StrainCalibrationCallback strain_calibration_callback_;
 };

firmware/src/util.cpp

@@ -0,0 +1,6 @@
+#include "util.h"
+
+float lerp(const float value, const float inMin, const float inMax, const float min, const float max) {
+    // Map the input value from the input range to the output range
+    return ((value - inMin) / (inMax - inMin)) * (max - min) + min;
+}

firmware/src/util.h

@@ -7,3 +7,5 @@ template <typename T> T CLAMP(const T& value, const T& low, const T& high)
 }
 
 #define COUNT_OF(A) (sizeof(A) / sizeof(A[0]))
+
+float lerp(const float value, const float inMin, const float inMax, const float min, const float max);

proto/smartknob.proto

@@ -73,6 +73,7 @@ message RequestState {}
 message PersistentConfiguration {
     uint32 version = 1;
     MotorCalibration motor = 2;
+    StrainCalibration strain = 3;
 }
 
 message MotorCalibration {
@@ -81,3 +82,8 @@ message MotorCalibration {
     bool direction_cw = 3;
     uint32 pole_pairs = 4;
 }
+
+message StrainCalibration {
+    int32 idle_value = 1;
+    int32 press_delta = 2; 
+}