Firmware cleanup

firmware/platformio.ini

@@ -29,13 +29,8 @@ build_flags =
 
 [env:view]
 extends = base_config
-; platform = https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream
-; platform_packages =
-;    framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32#master
 board = esp32doit-devkit-v1
 lib_deps =
-  ; askuric/Simple FOC @ 2.2.1
-  ; bxparks/AceButton @ 1.9.1
   ${base_config.lib_deps}
   bodmer/TFT_eSPI@2.4.25
   fastled/FastLED @ 3.5.0
@@ -103,54 +98,3 @@ build_flags =
   ; GPIO >= 34 are input only
   ; (SOC_GPIO_VALID_GPIO_MASK & ~(0ULL | _FL_BIT(34) | _FL_BIT(35) | _FL_BIT(36) | _FL_BIT(37) | _FL_BIT(38) | _FL_BIT(39)))
   -DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
-
-  
-[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
-  -DSK_STRAIN=0
-  -DSK_LEDS=0
-
-  -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_MT6701=1
-  -DPIN_MT_DATA=21
-  -DPIN_MT_CLOCK=22
-  -DPIN_MT_CSN=12
-  -DPIN_LCD_BACKLIGHT=4
-
-  -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=-1
-  -DLOAD_GLCD=1
-  -DLOAD_GFXFF=1
-  -DSPI_FREQUENCY=40000000

firmware/src/main.cpp

@@ -37,7 +37,7 @@ void setup() {
 
   motor_task.addListener(knob_state_debug_queue);
 
-  // Free up the loop task
+  // Free up the Arduino loop task
   vTaskDelete(NULL);
 }
 

firmware/src/motor_task.cpp

@@ -1,11 +1,24 @@
 #include <SimpleFOC.h>
-#include <sensors/MagneticSensorI2C.h>
 
 #include "motor_task.h"
+#if SENSOR_MT6701
 #include "mt6701_sensor.h"
+#endif
+#if SENSOR_TLV
 #include "tlv_sensor.h"
+#endif
 #include "util.h"
 
+
+// #### 
+// Hardware-specific motor calibration constants.
+// Run calibration once at startup, then update these constants with the calibration results.
+static const float ZERO_ELECTRICAL_OFFSET = 2.77;
+static const Direction FOC_DIRECTION = Direction::CW;
+static const int MOTOR_POLE_PAIRS = 7;
+// ####
+
+
 static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
 static const float DEAD_ZONE_RAD = 1 * _PI / 180;
 
@@ -24,38 +37,16 @@ MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 2048, 1, task_core
 MotorTask::~MotorTask() {}
 
 
-// BLDC motor & driver instance
-BLDCMotor motor = BLDCMotor(1);
-BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
-
 #if SENSOR_TLV
     TlvSensor encoder = TlvSensor();
 #elif SENSOR_MT6701
     MT6701Sensor encoder = MT6701Sensor();
 #endif
-// MagneticSensorI2C tlv = MagneticSensorI2C(AS5600_I2C);
 
 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
-    // float zero_electric_offset = 2.93; //0.15; // 17mm test
-    // float zero_electric_offset = 0.66; // 15mm handheld
-    float zero_electric_offset = 7.34;
-    Direction foc_direction = Direction::CW;
-    motor.pole_pairs = 7;
 
     driver.voltage_power_supply = 5;
     driver.init();
@@ -66,9 +57,7 @@ void MotorTask::run() {
 
     #if SENSOR_MT6701
     encoder.init();
-    // motor.LPF_angle = LowPassFilter(0.05);
     #endif
-    // motor.LPF_current_q = {0.01};
 
     motor.linkDriver(&driver);
 
@@ -77,23 +66,21 @@ void MotorTask::run() {
     motor.velocity_limit = 10000;
     motor.linkSensor(&encoder);
 
-    // Not actually using the velocity loop; but I'm using those PID variables
-    // because SimpleFOC studio supports updating them easily over serial for tuning.
+    // Not actually using the velocity loop built into SimpleFOC; but I'm using those PID variables
+    // to run PID for torque (and SimpleFOC studio supports updating them easily over serial for tuning)
     motor.PID_velocity.P = 4;
     motor.PID_velocity.I = 0;
     motor.PID_velocity.D = 0.04;
     motor.PID_velocity.output_ramp = 10000;
     motor.PID_velocity.limit = 10;
 
-
-    // motor.useMonitoring(Serial);
-
     motor.init();
 
     encoder.update();
     delay(10);
 
-    motor.initFOC(zero_electric_offset, foc_direction);
+    motor.pole_pairs = MOTOR_POLE_PAIRS;
+    motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
 
     bool calibrate = false;
 
@@ -107,178 +94,11 @@ void MotorTask::run() {
         delay(10);
     }
     if (calibrate) {
-        motor.controller = MotionControlType::angle_openloop;
-        motor.pole_pairs = 1;
-        motor.initFOC(0, Direction::CW);
-
-
-        float a = 0;
-
-        for (uint8_t i = 0; i < 200; i++) {
-            encoder.update();
-            motor.move(a);
-            delay(1);
-        }
-        float start_sensor = encoder.getAngle();
-
-        for (; a < 3 * _2PI; a += 0.01) {
-            encoder.update();
-            motor.move(a);
-            delay(1);
-        }
-
-        for (uint8_t i = 0; i < 200; i++) {
-            encoder.update();
-            delay(1);
-        }
-        float end_sensor = encoder.getAngle();
-
-
-        motor.voltage_limit = 0;
-        motor.move(a);
-        // Serial.println("Did motor turn counterclockwise? Press Y to continue, otherwise change motor wiring and restart");
-        // while (Serial.read() != 'Y') {
-        //     delay(10);
-        // }
-
-        Serial.println();
-
-        // TODO: check for no motor movement!
-
-        Serial.print("Sensor measures positive for positive motor rotation: ");
-        if (end_sensor > start_sensor) {
-            Serial.println("YES, Direction=CW");
-            motor.initFOC(0, Direction::CW);
-        } else {
-            Serial.println("NO, Direction=CCW");
-            motor.initFOC(0, Direction::CCW);
-        }
-
-        // Rotate many electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
-        uint8_t electrical_revolutions = 20;
-        Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
-        motor.voltage_limit = 5;
-        motor.move(a);
-        Serial.println("Going to electrical zero...");
-        float destination = a + _2PI;
-        for (; a < destination; a += 0.03) {
-            encoder.update();
-            motor.move(a);
-            delay(1);
-        }
-        Serial.println("pause...");
-        for (uint16_t i = 0; i < 1000; i++) {
-            encoder.update();
-            delay(1);
-        }
-        Serial.println("Measuring...");
-
-        start_sensor = motor.sensor_direction * encoder.getAngle();
-        destination = a + electrical_revolutions * _2PI;
-        for (; a < destination; a += 0.03) {
-            encoder.update();
-            motor.move(a);
-            delay(1);
-        }
-        for (uint16_t i = 0; i < 1000; i++) {
-            encoder.update();
-            motor.move(a);
-            delay(1);
-        }
-        end_sensor = motor.sensor_direction * encoder.getAngle();
-        motor.voltage_limit = 0;
-        motor.move(a);
-
-        if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
-            Serial.println("ERROR: motor did not reach target!");
-            while(1) {}
-        }
-
-        float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
-        Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
-        Serial.println(electrical_per_mechanical);
-
-        int measured_pole_pairs = (int)round(electrical_per_mechanical);
-        Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
-
-        delay(1000);
-
-
-
-        // Measure mechanical angle at every electrical zero for several revolutions
-        motor.voltage_limit = 5;
-        motor.move(a);
-        float offset_x = 0;
-        float offset_y = 0;
-        float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
-        float destination2 = (floor(a / _2PI)) * _2PI;
-        for (; a < destination1; a += 0.4) {
-            motor.move(a);
-            delay(100);
-            for (uint8_t i = 0; i < 100; i++) {
-                encoder.update();
-                delay(1);
-            }
-            float real_electrical_angle = _normalizeAngle(a);
-            float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
-
-            float offset_angle = measured_electrical_angle - real_electrical_angle;
-            offset_x += cosf(offset_angle);
-            offset_y += sinf(offset_angle);
-
-            Serial.print(degrees(real_electrical_angle));
-            Serial.print(", ");
-            Serial.print(degrees(measured_electrical_angle));
-            Serial.print(", ");
-            Serial.println(degrees(_normalizeAngle(offset_angle)));
-        }
-        for (; a > destination2; a -= 0.4) {
-            motor.move(a);
-            delay(100);
-            for (uint8_t i = 0; i < 100; i++) {
-                encoder.update();
-                delay(1);
-            }
-            float real_electrical_angle = _normalizeAngle(a);
-            float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
-
-            float offset_angle = measured_electrical_angle - real_electrical_angle;
-            offset_x += cosf(offset_angle);
-            offset_y += sinf(offset_angle);
-
-            Serial.print(degrees(real_electrical_angle));
-            Serial.print(", ");
-            Serial.print(degrees(measured_electrical_angle));
-            Serial.print(", ");
-            Serial.println(degrees(_normalizeAngle(offset_angle)));
-        }
-        motor.voltage_limit = 0;
-        motor.move(a);
-
-        float avg_offset_angle = atan2f(offset_y, offset_x);
-
-        // Apply settings
-        motor.pole_pairs = measured_pole_pairs;
-        motor.zero_electric_angle = avg_offset_angle + _3PI_2;
-        motor.voltage_limit = 5;
-        motor.controller = MotionControlType::torque;
-
-        Serial.print("\n\nRESULTS:\n  zero electric angle: ");
-        Serial.println(motor.zero_electric_angle);
-        Serial.print("  direction: ");
-        if (motor.sensor_direction == Direction::CW) {
-            Serial.println("CW");
-        } else {
-            Serial.println("CCW");
-        }
-        Serial.printf("  pole pairs: %d\n", motor.pole_pairs);
-        delay(2000);
+        this->calibrate();
     }
 
     Serial.println(motor.zero_electric_angle);
 
-    command.add('M', &doMotor, "foo");
-    // command.add('D', &doDetents, "Detents");
     motor.monitor_downsample = 0; // disable monitor at first - optional
 
     // disableCore0WDT();
@@ -293,17 +113,17 @@ void MotorTask::run() {
 
     float idle_check_velocity_ewma = 0;
     uint32_t last_idle_start = 0;
-    uint32_t last_debug = 0;
-
     uint32_t last_publish = 0;
 
     while (1) {
         motor.loopFOC();
 
+        // Check queue for pending requests from other tasks
         Command command;
         if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
             switch (command.command_type) {
                 case CommandType::CONFIG: {
+                    // Change haptic input mode
                     config = command.data.config;
                     Serial.println("Got new config");
                     current_detent_center = motor.shaft_angle;
@@ -332,6 +152,7 @@ void MotorTask::run() {
                     break;
                 }
                 case CommandType::HAPTIC: {
+                    // Play a hardcoded haptic "click"
                     float strength = command.data.haptic.press ? 5 : 1.5;
                     motor.move(strength);
                     for (uint8_t i = 0; i < 3; i++) {
@@ -350,6 +171,7 @@ void MotorTask::run() {
             }
         }
 
+        // If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
         idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
         if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) {
             last_idle_start = 0;
@@ -358,19 +180,11 @@ void MotorTask::run() {
                 last_idle_start = millis();
             }
         }
-
-        // If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
         if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) {
             current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
-            // if (millis() - last_debug > 100) {
-            //     last_debug = millis();
-            //     Serial.print("Moving detent center. ");
-            //     Serial.print(current_detent_center);
-            //     Serial.print(" ");
-            //     Serial.println(motor.shaft_angle);
-            // }
         }
 
+        // Check where we are relative to the current nearest detent; update our position if we've moved far enough to snap to another detent
         float angle_to_detent_center = motor.shaft_angle - current_detent_center;
         #if SK_INVERT_ROTATION
             angle_to_detent_center = -motor.shaft_angle - current_detent_center;
@@ -395,7 +209,7 @@ void MotorTask::run() {
         motor.PID_velocity.P = out_of_bounds ? config.endstop_strength_unit * 4 : config.detent_strength_unit * 4;
 
 
-
+        // Apply motor torque based on our angle to the nearest detent (detent strength, etc is handled by the PID_velocity parameters)
         if (fabsf(motor.shaft_velocity) > 60) {
             // Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
             motor.move(0);
@@ -407,6 +221,7 @@ void MotorTask::run() {
             motor.move(torque);
         }
 
+        // Publish current status to other registered tasks periodically
         if (millis() - last_publish > 10) {
             publish({
                 .current_position = config.position,
@@ -417,7 +232,6 @@ void MotorTask::run() {
         }
 
         motor.monitor();
-        // command.run();
 
         delay(1);
     }
@@ -456,3 +270,179 @@ void MotorTask::publish(const KnobState& state) {
         xQueueOverwrite(listener, &state);
     }
 }
+
+void MotorTask::calibrate() {
+    // 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
+
+    Serial.println("\n\n\nStarting calibration, please do not touch to motor until complete!");
+
+    motor.controller = MotionControlType::angle_openloop;
+    motor.pole_pairs = 1;
+    motor.initFOC(0, Direction::CW);
+
+    float a = 0;
+
+    // #### Determine direction motor rotates relative to angle sensor
+    for (uint8_t i = 0; i < 200; i++) {
+        encoder.update();
+        motor.move(a);
+        delay(1);
+    }
+    float start_sensor = encoder.getAngle();
+
+    for (; a < 3 * _2PI; a += 0.01) {
+        encoder.update();
+        motor.move(a);
+        delay(1);
+    }
+
+    for (uint8_t i = 0; i < 200; i++) {
+        encoder.update();
+        delay(1);
+    }
+    float end_sensor = encoder.getAngle();
+
+
+    motor.voltage_limit = 0;
+    motor.move(a);
+
+    Serial.println();
+
+    // TODO: check for no motor movement!
+
+    Serial.print("Sensor measures positive for positive motor rotation: ");
+    if (end_sensor > start_sensor) {
+        Serial.println("YES, Direction=CW");
+        motor.initFOC(0, Direction::CW);
+    } else {
+        Serial.println("NO, Direction=CCW");
+        motor.initFOC(0, Direction::CCW);
+    }
+
+
+    // #### Determine pole-pairs
+    // Rotate 20 electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
+    uint8_t electrical_revolutions = 20;
+    Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
+    motor.voltage_limit = 5;
+    motor.move(a);
+    Serial.println("Going to electrical zero...");
+    float destination = a + _2PI;
+    for (; a < destination; a += 0.03) {
+        encoder.update();
+        motor.move(a);
+        delay(1);
+    }
+    Serial.println("pause..."); // Let momentum settle...
+    for (uint16_t i = 0; i < 1000; i++) {
+        encoder.update();
+        delay(1);
+    }
+    Serial.println("Measuring...");
+
+    start_sensor = motor.sensor_direction * encoder.getAngle();
+    destination = a + electrical_revolutions * _2PI;
+    for (; a < destination; a += 0.03) {
+        encoder.update();
+        motor.move(a);
+        delay(1);
+    }
+    for (uint16_t i = 0; i < 1000; i++) {
+        encoder.update();
+        motor.move(a);
+        delay(1);
+    }
+    end_sensor = motor.sensor_direction * encoder.getAngle();
+    motor.voltage_limit = 0;
+    motor.move(a);
+
+    if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
+        Serial.println("ERROR: motor did not reach target!");
+        while(1) {}
+    }
+
+    float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
+    Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
+    Serial.println(electrical_per_mechanical);
+
+    int measured_pole_pairs = (int)round(electrical_per_mechanical);
+    Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
+
+    delay(1000);
+
+
+    // #### Determine mechanical offset to electrical zero
+    // Measure mechanical angle at every electrical zero for several revolutions
+    motor.voltage_limit = 5;
+    motor.move(a);
+    float offset_x = 0;
+    float offset_y = 0;
+    float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
+    float destination2 = (floor(a / _2PI)) * _2PI;
+    for (; a < destination1; a += 0.4) {
+        motor.move(a);
+        delay(100);
+        for (uint8_t i = 0; i < 100; i++) {
+            encoder.update();
+            delay(1);
+        }
+        float real_electrical_angle = _normalizeAngle(a);
+        float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
+
+        float offset_angle = measured_electrical_angle - real_electrical_angle;
+        offset_x += cosf(offset_angle);
+        offset_y += sinf(offset_angle);
+
+        Serial.print(degrees(real_electrical_angle));
+        Serial.print(", ");
+        Serial.print(degrees(measured_electrical_angle));
+        Serial.print(", ");
+        Serial.println(degrees(_normalizeAngle(offset_angle)));
+    }
+    for (; a > destination2; a -= 0.4) {
+        motor.move(a);
+        delay(100);
+        for (uint8_t i = 0; i < 100; i++) {
+            encoder.update();
+            delay(1);
+        }
+        float real_electrical_angle = _normalizeAngle(a);
+        float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
+
+        float offset_angle = measured_electrical_angle - real_electrical_angle;
+        offset_x += cosf(offset_angle);
+        offset_y += sinf(offset_angle);
+
+        Serial.print(degrees(real_electrical_angle));
+        Serial.print(", ");
+        Serial.print(degrees(measured_electrical_angle));
+        Serial.print(", ");
+        Serial.println(degrees(_normalizeAngle(offset_angle)));
+    }
+    motor.voltage_limit = 0;
+    motor.move(a);
+
+    float avg_offset_angle = atan2f(offset_y, offset_x);
+
+
+    // #### Apply settings
+    // TODO: save to non-volatile storage
+    motor.pole_pairs = measured_pole_pairs;
+    motor.zero_electric_angle = avg_offset_angle + _3PI_2;
+    motor.voltage_limit = 5;
+    motor.controller = MotionControlType::torque;
+
+    Serial.print("\n\nRESULTS:\n  Update these constants at the top of " __FILE__ "\n  ZERO_ELECTRICAL_OFFSET: ");
+    Serial.println(motor.zero_electric_angle);
+    Serial.print("  FOC_DIRECTION: ");
+    if (motor.sensor_direction == Direction::CW) {
+        Serial.println("Direction::CW");
+    } else {
+        Serial.println("Direction::CCW");
+    }
+    Serial.printf("  MOTOR_POLE_PAIRS: %d\n", motor.pole_pairs);
+    delay(2000);
+}

firmware/src/motor_task.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Arduino.h>
+#include <SimpleFOC.h>
 #include <vector>
 
 #include "knob_data.h"
@@ -45,5 +46,10 @@ class MotorTask : public Task<MotorTask> {
 
         std::vector<QueueHandle_t> listeners_;
 
+        // BLDC motor & driver instance
+        BLDCMotor motor = BLDCMotor(1);
+        BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
+
         void publish(const KnobState& state);
+        void calibrate();
 };