WIP view firmware

firmware/platformio.ini

@@ -8,9 +8,8 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:tdisplay]
-platform = espressif32
-board = esp32doit-devkit-v1
+[base_config]
+platform = espressif32@3.4
 framework = arduino
 monitor_speed = 115200
 monitor_flags = 
@@ -18,14 +17,42 @@ monitor_flags =
 	--echo
 	--filter=esp32_exception_decoder
 lib_deps =
-    bodmer/TFT_eSPI@2.4.25
     askuric/Simple FOC @ 2.2.0
-    infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
-    bxparks/AceButton @ ^1.9.1
+    infineon/TLV493D-Magnetic-Sensor @ 1.0.3
+    bxparks/AceButton @ 1.9.1
 
 build_flags =
   -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
 
+[env:proto]
+extends = base_config
+board = esp32doit-devkit-v1
+lib_deps =
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+
+  -DPOLE_PAIRS=7
+  -DPIN_UH=27
+  -DPIN_UL=26
+  -DPIN_VH=25
+  -DPIN_VL=33
+  -DPIN_WH=32
+  -DPIN_WL=13
+  -DPIN_BUTTON_NEXT=36
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=-1
+  -DPIN_SCL=-1
+  -DSENSOR_MT6701=1
+
+  -DDESCRIPTION_FONT=Roboto_Thin_24
+  -DDESCRIPTION_Y_OFFSET=20
+  -DVALUE_OFFSET=30
+  -DDRAW_ARC=0
+
   -DUSER_SETUP_LOADED=1
   -DGC9A01_DRIVER=1
   -DCGRAM_OFFSET=1
@@ -41,3 +68,126 @@ build_flags =
   -DLOAD_GLCD=1
   -DLOAD_GFXFF=1
   -DSPI_FREQUENCY=40000000
+
+[env:view]
+extends = base_config
+board = esp32doit-devkit-v1
+lib_deps =
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+  fastled/FastLED @ 3.5.0
+  bogde/HX711 @ 0.7.5
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+  -DSK_LEDS=1
+  -DNUM_LEDS=8
+  -DSENSOR_MT6701=1
+
+  -DPOLE_PAIRS=7
+  -DPIN_UH=26
+  -DPIN_UL=25
+  -DPIN_VH=27
+  -DPIN_VL=32
+  -DPIN_WH=12
+  -DPIN_WL=33
+  -DPIN_BUTTON_NEXT=39
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=15
+  -DPIN_SCL=8
+  -DPIN_MT_DATA=37
+  -DPIN_MT_CLOCK=13
+  -DPIN_MT_CSN=14
+  -DPIN_LED_DATA=7
+
+  -DDESCRIPTION_FONT=Roboto_Thin_24
+  -DDESCRIPTION_Y_OFFSET=20
+  -DVALUE_OFFSET=30
+  -DDRAW_ARC=0
+
+  -DUSER_SETUP_LOADED=1
+  -DGC9A01_DRIVER=1
+  -DCGRAM_OFFSET=1
+  -DTFT_WIDTH=240
+  -DTFT_HEIGHT=240
+  -DTFT_MISO=-1
+  -DTFT_MOSI=5
+  -DTFT_SCLK=20
+  -DTFT_CS=21
+  -DTFT_DC=22
+  -DTFT_RST=4
+  -DTFT_BL=19
+  -DLOAD_GLCD=1
+  -DLOAD_GFXFF=1
+  -DSPI_FREQUENCY=40000000
+
+  ; Modify the default unusable pin mask to allow GPIO 7 (allowed to use on ESP32-PICO-V3-02)
+  ; Unusable bits: 6, 8, 9, 10, 20
+  -DFASTLED_UNUSABLE_PIN_MASK=0x100740LL
+
+[env:handheld]
+extends = base_config
+board = tinypico
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=0
+
+  -DPOLE_PAIRS=7
+  -DPIN_UH=25
+  -DPIN_UL=26
+  -DPIN_VH=27
+  -DPIN_VL=15
+  -DPIN_WH=14
+  -DPIN_WL=4
+  -DPIN_BUTTON_NEXT=23
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=33
+  -DPIN_SCL=32
+  -DSENSOR_TLV=1
+  
+[env:handheld_tdisplay]
+extends = base_config
+board = esp32doit-devkit-v1
+lib_deps =
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+
+  -DPOLE_PAIRS=7
+  -DPIN_UH=17
+  -DPIN_UL=2
+  -DPIN_VH=13
+  -DPIN_VL=32
+  -DPIN_WH=33
+  -DPIN_WL=25
+  -DPIN_BUTTON_NEXT=35
+  -DPIN_BUTTON_PREV=0
+  -DPIN_SDA=-1
+  -DPIN_SCL=-1
+  -DSENSOR_TLV=1
+
+  -DDESCRIPTION_FONT=FreeSans9pt7b
+  -DDESCRIPTION_Y_OFFSET=80
+  -DVALUE_OFFSET=0
+  -DDRAW_ARC=1
+
+  -DUSER_SETUP_LOADED=1
+  -DST7789_DRIVER=1
+  -DCGRAM_OFFSET=1
+  -DTFT_WIDTH=135
+  -DTFT_HEIGHT=240
+  -DTFT_MISO=-1
+  -DTFT_MOSI=19
+  -DTFT_SCLK=18
+  -DTFT_CS=5
+  -DTFT_DC=16
+  -DTFT_RST=23
+  -DTFT_BL=4
+  -DLOAD_GLCD=1
+  -DLOAD_GFXFF=1
+  -DSPI_FREQUENCY=40000000

firmware/src/display_task.cpp

@@ -1,18 +1,17 @@
+#if SK_DISPLAY
 #include "display_task.h"
 #include "semaphore_guard.h"
 
 #include "font/roboto_light_60.h"
 
 DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} {
-    semaphore_ = xSemaphoreCreateMutex();
-    assert(semaphore_ != NULL);
-    xSemaphoreGive(semaphore_);
+  knob_state_queue_ = xQueueCreate(1, sizeof(KnobState));
+  assert(knob_state_queue_ != NULL);
+
 }
 
 DisplayTask::~DisplayTask() {
-    if (semaphore_ != NULL) {
-        vSemaphoreDelete(semaphore_);
-    }
+  vQueueDelete(knob_state_queue_);
 }
 
 static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
@@ -77,12 +76,15 @@ void DisplayTask::run() {
     tft_.begin();
     tft_.invertDisplay(1);
     tft_.setRotation(0);
-    tft_.fillScreen(TFT_PURPLE);
+    tft_.fillScreen(TFT_DARKGREEN);
 
     spr_.setColorDepth(16);
     if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) {
       Serial.println("ERROR: sprite allocation failed!");
       tft_.fillScreen(TFT_RED);
+    } else {
+      Serial.println("Sprite created!");
+      tft_.fillScreen(TFT_PURPLE);
     }
     spr_.setTextColor(0xFFFF, TFT_BLACK);
     
@@ -100,9 +102,8 @@ void DisplayTask::run() {
     spr_.setTextDatum(CC_DATUM);
     spr_.setTextColor(TFT_WHITE);
     while(1) {
-        {
-            SemaphoreGuard lock(semaphore_);
-            state = state_;
+        if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
+          continue;
         }
 
         spr_.fillSprite(TFT_BLACK);
@@ -112,9 +113,9 @@ void DisplayTask::run() {
         }
 
         spr_.setFreeFont(&Roboto_Light_60);
-        spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - 30, 1);
-        spr_.setFreeFont(&Roboto_Thin_24);
-        int32_t line_y = TFT_HEIGHT / 2 + 20;
+        spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - VALUE_OFFSET, 1);
+        spr_.setFreeFont(&DESCRIPTION_FONT);
+        int32_t line_y = TFT_HEIGHT / 2 + DESCRIPTION_Y_OFFSET;
         char* start = state.config.descriptor;
         char* end = start + strlen(state.config.descriptor);
         while (start < end) {
@@ -139,6 +140,9 @@ void DisplayTask::run() {
           spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(left_bound), TFT_HEIGHT/2 - RADIUS * sinf(left_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(left_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(left_bound), TFT_WHITE);
           spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(right_bound), TFT_HEIGHT/2 - RADIUS * sinf(right_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(right_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(right_bound), TFT_WHITE);
         }
+        if (DRAW_ARC) {
+          spr_.drawCircle(TFT_WIDTH/2, TFT_HEIGHT/2, RADIUS, TFT_DARKGREY);
+        }
 
         float adjusted_sub_position = state.sub_position_unit * state.config.position_width_radians;
         if (state.config.num_positions > 0) {
@@ -175,7 +179,8 @@ void DisplayTask::run() {
     }
 }
 
-void DisplayTask::setData(KnobState state) {
-    SemaphoreGuard lock(semaphore_);
-    state_ = state;
+QueueHandle_t DisplayTask::getKnobStateQueue() {
+  return knob_state_queue_;
 }
+
+#endif

firmware/src/display_task.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#if SK_DISPLAY
+
 #include <Arduino.h>
 #include <TFT_eSPI.h>
 
@@ -13,7 +15,7 @@ class DisplayTask : public Task<DisplayTask> {
         DisplayTask(const uint8_t task_core);
         ~DisplayTask();
 
-        void setData(KnobState state);
+        QueueHandle_t getKnobStateQueue();
 
     protected:
         void run();
@@ -24,7 +26,8 @@ class DisplayTask : public Task<DisplayTask> {
         /** Full-size sprite used as a framebuffer */
         TFT_eSprite spr_ = TFT_eSprite(&tft_);
 
-        SemaphoreHandle_t semaphore_;
+        QueueHandle_t knob_state_queue_;
 
         KnobState state_;
 };
+#endif

firmware/src/interface_task.cpp

@@ -103,17 +103,35 @@ InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : T
 InterfaceTask::~InterfaceTask() {}
 
 void InterfaceTask::run() {
-    AceButton button(36);
-    pinMode(36, INPUT);
-    button.getButtonConfig()->setIEventHandler(this);
+    #if PIN_BUTTON_NEXT >= 34
+        pinMode(PIN_BUTTON_NEXT, INPUT);
+    #else
+        pinMode(PIN_BUTTON_NEXT, INPUT_PULLUP);
+    #endif
+    AceButton button_next((uint8_t) PIN_BUTTON_NEXT);
+    button_next.getButtonConfig()->setIEventHandler(this);
+
+    #if PIN_BUTTON_PREV > -1
+        #if PIN_BUTTON_PREV >= 34
+            pinMode(PIN_BUTTON_PREV, INPUT);
+        #else
+            pinMode(PIN_BUTTON_PREV, INPUT_PULLUP);
+        #endif
+        AceButton button_prev((uint8_t) PIN_BUTTON_PREV);
+        button_prev.getButtonConfig()->setIEventHandler(this);
+    #endif
+    
 
     motor_task_.setConfig(configs[0]);
     while (1) {
-        button.check();
+        button_next.check();
+        #if PIN_BUTTON_PREV > -1
+            button_prev.check();
+        #endif
         if (Serial.available()) {
             int v = Serial.read();
             if (v == ' ') {
-                nextConfig();
+                changeConfig(true);
             }
         }
         delay(10);
@@ -123,15 +141,31 @@ void InterfaceTask::run() {
 void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t button_state) {
     switch (event_type) {
         case AceButton::kEventPressed:
-            nextConfig();
+            if (button->getPin() == PIN_BUTTON_NEXT) {
+                changeConfig(true);
+            }
+            #if PIN_BUTTON_PREV > -1
+                if (button->getPin() == PIN_BUTTON_PREV) {
+                    changeConfig(false);
+                }
+            #endif
             break;
         case AceButton::kEventReleased:
             break;
     }
 }
 
-void InterfaceTask::nextConfig() {
-    current_config_ = (current_config_ + 1) % COUNT_OF(configs);
+void InterfaceTask::changeConfig(bool next) {
+    if (next) {
+        current_config_ = (current_config_ + 1) % COUNT_OF(configs);
+    } else {
+        if (current_config_ == 0) {
+            current_config_ = COUNT_OF(configs) - 1;
+        } else {
+            current_config_ --;
+        }
+    }
+    
     Serial.printf("Changing config to %d:\n%s\n", current_config_, configs[current_config_].descriptor);
     motor_task_.setConfig(configs[current_config_]);
 }

firmware/src/interface_task.h

@@ -22,5 +22,5 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
         MotorTask& motor_task_;
         int current_config_ = 0;
 
-        void nextConfig();
+        void changeConfig(bool next);
 };

firmware/src/main.cpp

@@ -1,31 +1,95 @@
 #include <Arduino.h>
-// #include <FastLED.h>
 #include <SimpleFOC.h>
-#include <TFT_eSPI.h>
+#include <HX711.h>
 
+#if SK_LEDS
+#include <FastLED.h>
+#endif
+
+#if SK_DISPLAY
 #include "display_task.h"
+#endif
+
 #include "interface_task.h"
 #include "motor_task.h"
 #include "tlv_sensor.h"
+#include "util.h"
 
+#if SK_DISPLAY
 DisplayTask display_task = DisplayTask(1);
-MotorTask motor_task = MotorTask(0, display_task);
+#endif
+MotorTask motor_task = MotorTask(0);
+
 InterfaceTask interface_task = InterfaceTask(1, motor_task);
 
-// CRGB leds[1];
+#if SK_LEDS
+CRGB leds[NUM_LEDS];
+#endif
+
+
+HX711 scale;
 
+static QueueHandle_t knob_state_debug_queue;
 
 void setup() {
   Serial.begin(115200);
 
   motor_task.begin();
   interface_task.begin();
+
+  #if SK_DISPLAY
   display_task.begin();
 
-  vTaskDelete(nullptr);
+  // Connect display to motor_task's knob state feed
+  motor_task.addListener(display_task.getKnobStateQueue());
+  #endif
+
+
+  // Create a queue and register it with motor_task to print knob state to serial (see loop() below)
+  knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
+  assert(knob_state_debug_queue != NULL);
+
+  motor_task.addListener(knob_state_debug_queue);
+
+  #if SK_LEDS
+  FastLED.addLeds<SK6812, PIN_LED_DATA, GRB>(leds, NUM_LEDS);
+  #endif
+
+  scale.begin(38, 2);
 }
 
 
+static KnobState state = {};
+uint32_t last_debug;
+
 void loop() {
-  assert(false);
+  // Print any new state, at most 5 times per second
+  if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
+    Serial.println(state.current_position);
+    last_debug = millis();
+  }
+
+  if (scale.wait_ready_timeout(100)) {
+    long reading = scale.read();
+    Serial.print("HX711 reading: ");
+    Serial.println(reading);
+    long lower = 950000;
+    long upper = 2500000;
+    long value = CLAMP(reading, lower, upper);
+    float unit = 1. * (value - lower) / (upper - lower);
+
+    for (uint8_t i = 0; i < NUM_LEDS; i++) {
+      leds[i].setHSV(255 * unit, 255, 128);
+    }
+    FastLED.show();
+
+  } else {
+    Serial.println("HX711 not found.");
+
+    for (uint8_t i = 0; i < NUM_LEDS; i++) {
+      leds[i] = CRGB::Red;
+    }
+    FastLED.show();
+
+  }
 }

firmware/src/motor_task.cpp

@@ -3,12 +3,7 @@
 #include "motor_task.h"
 #include "tlv_sensor.h"
 #include "mt6701_sensor.h"
-
-
-template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
-{
-  return value < low ? low : (value > high ? high : value); 
-}
+#include "util.h"
 
 static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
 static const float DEAD_ZONE_RAD = 1 * _PI / 180;
@@ -20,7 +15,7 @@ static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
 static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
 
 
-MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
+MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 8192, 1, task_core) {
     queue_ = xQueueCreate(1, sizeof(KnobConfig));
     assert(queue_ != NULL);
 }
@@ -29,11 +24,14 @@ MotorTask::~MotorTask() {}
 
 
 // BLDC motor & driver instance
-BLDCMotor motor = BLDCMotor(7);
-BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
+BLDCMotor motor = BLDCMotor(POLE_PAIRS);
+BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
 
-// TlvSensor tlv = TlvSensor();
-MT6701Sensor encoder = MT6701Sensor();
+#if SENSOR_TLV
+    TlvSensor encoder = TlvSensor();
+#elif SENSOR_MT6701
+    MT6701Sensor encoder = MT6701Sensor();
+#endif
 
 Commander command = Commander(Serial);
 
@@ -41,12 +39,32 @@ Commander command = Commander(Serial);
 void doMotor(char* cmd) { command.motor(&motor, cmd); }
 
 void MotorTask::run() {
+    // Hardware-specific configuration:
+    // TODO: make this easier to configure
+    // Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
+    // SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
+    // it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
+    // So this value is based on experimentation.
+    // TODO: dig into SimpleFOC calibration and find/fix the issue
+    // float zero_electric_offset = -0.6; // original proto
+    //float zero_electric_offset = 0.4; // handheld 1
+    float zero_electric_offset = -0.8; // handheld 2
+    bool tlv_invert = true;
+    Direction foc_direction = Direction::CCW;
+
+
     driver.voltage_power_supply = 5;
     driver.init();
 
-    // Wire.begin();
-    // Wire.setClock(400000);
+    #if SENSOR_TLV
+    Wire.begin(PIN_SDA, PIN_SCL);
+    Wire.setClock(400000);
+    encoder.init(Wire, tlv_invert);
+    #endif
+
+    #if SENSOR_MT6701
     encoder.init();
+    #endif
 
     motor.linkDriver(&driver);
 
@@ -70,13 +88,7 @@ void MotorTask::run() {
     encoder.update();
     delay(10);
 
-    // Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
-    // SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
-    // it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
-    // So this value is based on experimentation.
-    // TODO: dig into SimpleFOC calibration and find/fix the issue
-    float zero_electric_offset = -0.2;
-    motor.initFOC(zero_electric_offset, Direction::CCW);
+    motor.initFOC(zero_electric_offset, foc_direction);
     Serial.println(motor.zero_electric_angle);
 
     command.add('M', &doMotor, "foo");
@@ -97,7 +109,7 @@ void MotorTask::run() {
     uint32_t last_idle_start = 0;
     uint32_t last_debug = 0;
 
-    uint32_t last_display_update = 0;
+    uint32_t last_publish = 0;
 
     while (1) {
         motor.loopFOC();
@@ -176,13 +188,13 @@ void MotorTask::run() {
             motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
         }
 
-        if (millis() - last_display_update > 10) {
-            display_task_.setData({
+        if (millis() - last_publish > 10) {
+            publish({
                 .current_position = config.position,
                 .sub_position_unit = -angle_to_detent_center / config.position_width_radians,
                 .config = config,
             });
-            last_display_update = millis();
+            last_publish = millis();
         }
 
         motor.monitor();
@@ -195,3 +207,13 @@ void MotorTask::run() {
 void MotorTask::setConfig(const KnobConfig& config) {
     xQueueOverwrite(queue_, &config);
 }
+
+void MotorTask::addListener(QueueHandle_t queue) {
+    listeners_.push_back(queue);
+}
+
+void MotorTask::publish(const KnobState& state) {
+    for (auto listener : listeners_) {
+        xQueueOverwrite(listener, &state);
+    }
+}

firmware/src/motor_task.h

@@ -4,21 +4,25 @@
 
 #include "knob_data.h"
 #include "task.h"
-#include "display_task.h"
 
 class MotorTask : public Task<MotorTask> {
     friend class Task<MotorTask>; // Allow base Task to invoke protected run()
 
     public:
-        MotorTask(const uint8_t task_core, DisplayTask& display_task);
+        MotorTask(const uint8_t task_core);
         ~MotorTask();
 
         void setConfig(const KnobConfig& config);
 
+        void addListener(QueueHandle_t queue);
+
     protected:
         void run();
 
     private:
-        DisplayTask& display_task_;
         QueueHandle_t queue_;
+
+        std::vector<QueueHandle_t> listeners_;
+
+        void publish(const KnobState& state);
 };

firmware/src/mt6701_sensor.cpp

@@ -1,10 +1,6 @@
 #include "mt6701_sensor.h"
 #include "driver/spi_master.h"
 
-#define PIN_CSN 12
-#define PIN_SDA 21
-#define PIN_SCL 22
-
 static const float ALPHA = 0.4;
 
 
@@ -44,13 +40,13 @@ MT6701Sensor::MT6701Sensor() {}
 
 void MT6701Sensor::init() {
 
-    pinMode(PIN_CSN, OUTPUT);
-    digitalWrite(PIN_CSN, HIGH);
+    pinMode(PIN_MT_CSN, OUTPUT);
+    digitalWrite(PIN_MT_CSN, HIGH);
 
   spi_bus_config_t tx_bus_config = {
       .mosi_io_num = -1,
-      .miso_io_num = PIN_SDA,
-      .sclk_io_num = PIN_SCL,
+      .miso_io_num = PIN_MT_DATA,
+      .sclk_io_num = PIN_MT_CLOCK,
       .quadwp_io_num = -1,
       .quadhd_io_num = -1,
       .max_transfer_sz = 1000,
@@ -68,7 +64,7 @@ void MT6701Sensor::init() {
       .cs_ena_posttrans=0,
       .clock_speed_hz=4000000,
       .input_delay_ns=0,
-      .spics_io_num=PIN_CSN,
+      .spics_io_num=PIN_MT_CSN,
       .flags = 0,
       .queue_size=1,
       .pre_cb=NULL,

firmware/src/tlv_sensor.cpp

@@ -4,8 +4,9 @@ static const float ALPHA = 0.04;
 
 TlvSensor::TlvSensor() {}
 
-void TlvSensor::init() {
-  tlv_.begin();
+void TlvSensor::init(TwoWire& wire, bool invert) {
+  invert_ = invert;
+  tlv_.begin(wire);
   tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
   tlv_.disableInterrupt();
   tlv_.disableTemp();
@@ -19,7 +20,7 @@ float TlvSensor::getSensorAngle() {
       y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
       last_update_ = now;
     }
-    float rad = atan2f(y_, x_);
+    float rad = (invert_ ? -1 : 1) * atan2f(y_, x_);
     if (rad < 0) {
         rad += 2*PI;
     }

firmware/src/tlv_sensor.h

@@ -8,7 +8,7 @@ class TlvSensor : public Sensor {
         TlvSensor();
 
         // initialize the sensor hardware
-        void init();
+        void init(TwoWire& wire, bool invert);
 
         // Get current shaft angle from the sensor hardware, and 
         // return it as a float in radians, in the range 0 to 2PI.
@@ -21,4 +21,5 @@ class TlvSensor : public Sensor {
         float x_;
         float y_;
         uint32_t last_update_;
+        bool invert_;
 };

firmware/src/util.h

@@ -0,0 +1,7 @@
+
+#pragma once
+
+template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
+{
+  return value < low ? low : (value > high ? high : value); 
+}