Feature/motorgo support

CMakeLists.txt

@@ -12,7 +12,7 @@ set(EXTRA_COMPONENT_DIRS
 
 set(
   COMPONENTS
-  "main esptool_py cli filters i2c task monitor mt6701 bldc_motor bldc_driver bldc_haptics"
+  "main esptool_py cli filters bldc_haptics motorgo-mini tinys3_test_stand"
   CACHE STRING
   "List of components to include"
   )

README.md

@@ -5,20 +5,31 @@ designed to enable running various experiments in openloop control, closed loop
 control, and user interface / haptics using a small BLDC motor that's commonly
 used in camera gimbals.
 
+The code supports running on the custom BLDC Test Stand (`TinyS3 Test Stand`),
+as well as on the `MotorGo-Mini`. You can select which hardware you want to run
+on via `menuconfig`.
+
 ## Hardware
 
-* TinyS3
-* BLDC Motor (I used [these A and B motors](https://www.aliexpress.us/item/3256802907900422.html) from aliexpress)
-* MT6701 Magnetic Encoder Breakout (I used [this purple one (color B)](https://www.aliexpress.us/item/3256804851103272.html) from aliexpress)
-* TMC6300 BOB
-* Mini solderless breadboard
-* 3d printed test stand enclosure (.stl and source files in the [mcad](./mcad) directory)
-* Benchtop power supply (currently running at 5V 1A so many things should work)
+Either:
+* TinyS3 Test Stand:
+  * TinyS3
+  * BLDC Motor (I used [these A and B motors](https://www.aliexpress.us/item/3256802907900422.html) from aliexpress)
+  * MT6701 Magnetic Encoder Breakout (I used [this purple one (color B)](https://www.aliexpress.us/item/3256804851103272.html) from aliexpress)
+  * TMC6300 BOB
+  * Mini solderless breadboard
+  * 3d printed test stand enclosure (.stl and source files in the [mcad](./mcad) directory)
+  * Benchtop power supply (currently running at 5V 1A so many things should work)
+or:
+* MotorGo-Mini
+
+### Configure
+
+``` 
+idf.py menuconfig
+```
 
-:warning:
-> NOTE: you MUST make sure that you run this code with the
-> `zero_electrical_offset` value set to 0 (or not provided) at least once
-> otherwise the sample will not work and could potentially damage your motor.
+Select which hardware you are building and deploying to.
 
 ### Build and Flash
 
@@ -82,7 +93,11 @@ You must run this calibration any time you change your hardware configuration
 
 ## Code Breakdown
 
-This example is relatively complex, but builds complex haptic behavior using the
+This example encapsulates the board support packages into one of two components:
+* `espp::TinyS3TestStand` (in this repo)
+* `espp::MotorGoMini` (in espp)
+
+These components build complex haptic behavior using the
 following components:
 
 * `espp::Mt6701`

components/tinys3_test_stand/CMakeLists.txt

@@ -0,0 +1,4 @@
+idf_component_register(
+  INCLUDE_DIRS "include"
+  REQUIRES base_component filters math mt6701 pid bldc_driver bldc_motor i2c
+  )

components/tinys3_test_stand/include/tinys3_test_stand.hpp

@@ -0,0 +1,155 @@
+#pragma once
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "base_component.hpp"
+#include "bldc_driver.hpp"
+#include "bldc_motor.hpp"
+#include "i2c.hpp"
+#include "mt6701.hpp"
+#include "simple_lowpass_filter.hpp"
+
+namespace espp {
+/// This class acts as a board support component for the TinyS3 BLDC Test Stand.
+/// It provides a high-level interface to the system's functionality.
+///
+/// High level overview of the system
+/// - ESP32s3 module, using TinyS3
+/// - TMC6300-BOB motor driver on a breakout board
+/// - One MT6701 magnetic encoder connected via I2C
+class TinyS3TestStand : public BaseComponent {
+public:
+  using Encoder = espp::Mt6701<>;
+  using BldcMotor = espp::BldcMotor<espp::BldcDriver, Encoder>;
+
+  /// Constructor
+  /// \param verbosity The verbosity level for the logger of the MotorGo-Mini
+  ///        and its components
+  explicit TinyS3TestStand(espp::Logger::Verbosity verbosity = espp::Logger::Verbosity::WARN)
+      : BaseComponent("TinyS3 Test Stand", verbosity) {
+    init();
+  }
+
+  /// Get a reference to the encoder
+  /// \return A reference to the encoder
+  Encoder &encoder() { return encoder_; }
+
+  /// Get a reference to the motor driver
+  /// \return A reference to the motor driver
+  espp::BldcDriver &motor_driver() { return motor_driver_; }
+
+  /// Get a reference to the motor
+  /// \return A reference to the motor
+  BldcMotor &motor() { return motor_; }
+
+protected:
+  static constexpr auto I2C_PORT = I2C_NUM_0;
+  static constexpr auto I2C_SDA_PIN = GPIO_NUM_8;
+  static constexpr auto I2C_SCL_PIN = GPIO_NUM_9;
+
+  static constexpr uint64_t core_update_period_us = 1000; // 1 ms
+
+  static constexpr auto MOTOR_A_H = GPIO_NUM_1;
+  static constexpr auto MOTOR_A_L = GPIO_NUM_2;
+  static constexpr auto MOTOR_B_H = GPIO_NUM_3;
+  static constexpr auto MOTOR_B_L = GPIO_NUM_4;
+  static constexpr auto MOTOR_C_H = GPIO_NUM_5;
+  static constexpr auto MOTOR_C_L = GPIO_NUM_21;
+  static constexpr auto MOTOR_ENABLE = GPIO_NUM_34;
+  static constexpr auto MOTOR_FAULT = GPIO_NUM_36;
+
+  void init() {
+    init_encoder();
+    init_motor();
+  }
+
+  void init_encoder() {
+    bool run_task = true;
+    std::error_code ec;
+    encoder_.initialize(run_task, ec);
+    if (ec) {
+      logger_.error("Could not initialize encoder: {}", ec.message());
+    }
+  }
+
+  void init_motor() { motor_.initialize(); }
+
+  /// I2C bus for external communication
+  I2c i2c_{{
+      .port = I2C_PORT,
+      .sda_io_num = I2C_SDA_PIN,
+      .scl_io_num = I2C_SCL_PIN,
+      .sda_pullup_en = GPIO_PULLUP_ENABLE,
+      .scl_pullup_en = GPIO_PULLUP_ENABLE,
+      .clk_speed = 1 * 1000 * 1000, // MT6701 supports 1 MHz I2C
+  }};
+
+  // Encoder
+  Encoder encoder_{
+      {.write = std::bind(&espp::I2c::write, &i2c_, std::placeholders::_1, std::placeholders::_2,
+                          std::placeholders::_3),
+       .read = std::bind(&espp::I2c::read, &i2c_, std::placeholders::_1, std::placeholders::_2,
+                         std::placeholders::_3),
+       .update_period = std::chrono::duration<float>(core_update_period_us / 1e6f),
+       .auto_init = false, // we have to initialize the SPI first before we can use the encoder
+       .log_level = get_log_level()}};
+
+  // Driver
+  espp::BldcDriver motor_driver_{{.gpio_a_h = MOTOR_A_H,
+                                  .gpio_a_l = MOTOR_A_L,
+                                  .gpio_b_h = MOTOR_B_H,
+                                  .gpio_b_l = MOTOR_B_L,
+                                  .gpio_c_h = MOTOR_C_H,
+                                  .gpio_c_l = MOTOR_C_L,
+                                  .gpio_enable = MOTOR_ENABLE,
+                                  .gpio_fault = MOTOR_FAULT,
+                                  .power_supply_voltage = 5.0f,
+                                  .limit_voltage = 5.0f,
+                                  .log_level = get_log_level()}};
+
+  // Filters
+  espp::SimpleLowpassFilter motor_velocity_filter_{{.time_constant = 0.005f}};
+  espp::SimpleLowpassFilter motor_angle_filter_{{.time_constant = 0.001f}};
+
+  // Motor
+  BldcMotor motor_{{
+      .num_pole_pairs = 7,
+      .phase_resistance = 5.0f,
+      .kv_rating = 320,
+      .current_limit = 1.0f,
+      .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM,
+      // create shared_ptr from raw pointer to ensure shared_ptr doesn't delete the object
+      .driver =
+          std::shared_ptr<espp::BldcDriver>(std::shared_ptr<espp::BldcDriver>{}, &motor_driver_),
+      // create shared_ptr from raw pointer to ensure shared_ptr doesn't delete the object
+      .sensor = std::shared_ptr<Encoder>(std::shared_ptr<Encoder>{}, &encoder_),
+      .velocity_pid_config =
+          {
+              .kp = 0.010f,
+              .ki = 1.000f,
+              .kd = 0.000f,
+              .integrator_min = -1.0f, // same scale as output_min (so same scale as current)
+              .integrator_max = 1.0f,  // same scale as output_max (so same scale as current)
+              .output_min = -1.0, // velocity pid works on current (if we have phase resistance)
+              .output_max = 1.0,  // velocity pid works on current (if we have phase resistance)
+          },
+      .angle_pid_config =
+          {
+              .kp = 7.000f,
+              .ki = 0.300f,
+              .kd = 0.010f,
+              .integrator_min = -10.0f, // same scale as output_min (so same scale as velocity)
+              .integrator_max = 10.0f,  // same scale as output_max (so same scale as velocity)
+              .output_min = -20.0,      // angle pid works on velocity (rad/s)
+              .output_max = 20.0,       // angle pid works on velocity (rad/s)
+          },
+      .velocity_filter = [this](auto v) { return motor_velocity_filter_(v); },
+      .angle_filter = [this](auto v) { return motor_angle_filter_(v); },
+      .auto_init = false, // we have to initialize the SPI first before we can use the encoder
+      .log_level = get_log_level(),
+  }};
+};
+} // namespace espp

main/Kconfig.projbuild

@@ -0,0 +1,19 @@
+menu "Example Configuration"
+
+    choice EXAMPLE_HARDWARE
+        prompt "Hardware"
+        default EXAMPLE_HARDWARE_MOTORGO_MINI
+        help
+            Select the hardware to run this example on.
+
+        config EXAMPLE_HARDWARE_MOTORGO_MINI
+            depends on IDF_TARGET_ESP32S3
+            bool "MotorGo Mini"
+
+        config EXAMPLE_HARDWARE_TEST_STAND
+            depends on IDF_TARGET_ESP32S3
+            bool "BLDC Motor Test Stand (TinyS3)"
+
+    endchoice
+
+endmenu

main/main.cpp

@@ -1,15 +1,11 @@
 #include <chrono>
 #include <vector>
 
-#include "bldc_driver.hpp"
 #include "bldc_haptics.hpp"
-#include "bldc_motor.hpp"
-#include "butterworth_filter.hpp"
 #include "cli.hpp"
-#include "i2c.hpp"
-#include "lowpass_filter.hpp"
-#include "mt6701.hpp"
-#include "task.hpp"
+
+#include "motorgo-mini.hpp"
+#include "tinys3_test_stand.hpp"
 
 using namespace std::chrono_literals;
 
@@ -18,86 +14,23 @@ extern "C" void app_main(void) {
 
   logger.info("Bootup");
 
-  // make the I2C that we'll use to communicate with the mt6701 (magnetic encoder)
-  espp::I2c i2c({
-      // pins for the bldc motor test stand with the TinyS3
-      .port = I2C_NUM_1,
-      .sda_io_num = GPIO_NUM_8,
-      .scl_io_num = GPIO_NUM_9,
-      .sda_pullup_en = GPIO_PULLUP_ENABLE,
-      .scl_pullup_en = GPIO_PULLUP_ENABLE,
-  });
-
-  // make the velocity filter
-  static constexpr float core_update_period = 0.001f; // seconds
-
-  // now make the mt6701 which decodes the data
-  using Encoder = espp::Mt6701<>;
-  std::shared_ptr<Encoder> mt6701 = std::make_shared<Encoder>(
-      Encoder::Config{.write = std::bind(&espp::I2c::write, &i2c, std::placeholders::_1,
-                                         std::placeholders::_2, std::placeholders::_3),
-                      .read = std::bind(&espp::I2c::read, &i2c, std::placeholders::_1,
-                                        std::placeholders::_2, std::placeholders::_3),
-                      .update_period = std::chrono::duration<float>(core_update_period),
-                      .log_level = espp::Logger::Verbosity::WARN});
-
-  // now make the bldc driver
-  std::shared_ptr<espp::BldcDriver> driver = std::make_shared<espp::BldcDriver>(
-      espp::BldcDriver::Config{// this pinout is configured for the TinyS3 connected to the
-                               // TMC6300-BOB in the BLDC Motor Test Stand
-                               .gpio_a_h = 1,
-                               .gpio_a_l = 2,
-                               .gpio_b_h = 3,
-                               .gpio_b_l = 4,
-                               .gpio_c_h = 5,
-                               .gpio_c_l = 21,
-                               .gpio_enable = 34, // connected to the VIO/~Stdby pin of TMC6300-BOB
-                               .gpio_fault = 36,  // connected to the nFAULT pin of TMC6300-BOB
-                               .power_supply_voltage = 5.0f,
-                               .limit_voltage = 5.0f,
-                               .log_level = espp::Logger::Verbosity::WARN});
-
-  // now make the bldc motor
-  using BldcMotor = espp::BldcMotor<espp::BldcDriver, Encoder>;
-  auto motor = BldcMotor(BldcMotor::Config{
-      // measured by setting it into ANGLE_OPENLOOP and then counting how many
-      // spots you feel when rotating it.
-      .num_pole_pairs = 7,
-      .phase_resistance =
-          5.0f, // tested by running velocity_openloop and seeing if the veloicty is ~correct
-      .kv_rating =
-          320, // tested by running velocity_openloop and seeing if the velocity is ~correct
-      .current_limit = 1.0f,        // Amps
-      .zero_electric_offset = 0.0f, // set to zero to always calibrate
-      // and it will be logged.
-      .sensor_direction =
-          espp::detail::SensorDirection::UNKNOWN, // set to unknown to always calibrate
-      .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM,
-      .driver = driver,
-      .sensor = mt6701,
-      .velocity_pid_config =
-          {
-              .kp = 0.010f,
-              .ki = 1.000f,
-              .kd = 0.000f,
-              .integrator_min = -1.0f, // same scale as output_min (so same scale as current)
-              .integrator_max = 1.0f,  // same scale as output_max (so same scale as current)
-              .output_min = -1.0, // velocity pid works on current (if we have phase resistance)
-              .output_max = 1.0,  // velocity pid works on current (if we have phase resistance)
-          },
-      .angle_pid_config =
-          {
-              .kp = 7.000f,
-              .ki = 0.300f,
-              .kd = 0.010f,
-              .integrator_min = -10.0f, // same scale as output_min (so same scale as velocity)
-              .integrator_max = 10.0f,  // same scale as output_max (so same scale as velocity)
-              .output_min = -20.0,      // angle pid works on velocity (rad/s)
-              .output_max = 20.0,       // angle pid works on velocity (rad/s)
-          },
-      .log_level = espp::Logger::Verbosity::INFO});
-
-  using BldcHaptics = espp::BldcHaptics<BldcMotor>;
+#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI
+#pragma message("Using MotorGo Mini hardware configuration")
+  logger.info("Using MotorGo Mini hardware configuration");
+  // we don't want to init both motors, so we'll pass in auto_init=false
+  espp::MotorGoMini motorgo_mini({.auto_init = false});
+  motorgo_mini.init_motor_channel_1();
+  auto &motor = motorgo_mini.motor1();
+  using BldcHaptics = espp::BldcHaptics<espp::MotorGoMini::BldcMotor>;
+#elif CONFIG_EXAMPLE_HARDWARE_TEST_STAND
+#pragma message("Using TinyS3 Test Stand hardware configuration")
+  logger.info("Using TinyS3 Test Stand hardware configuration");
+  espp::TinyS3TestStand test_stand(espp::Logger::Verbosity::INFO);
+  auto &motor = test_stand.motor();
+  using BldcHaptics = espp::BldcHaptics<espp::TinyS3TestStand::BldcMotor>;
+#else
+#error "No hardware configuration selected"
+#endif
 
   auto haptic_motor = BldcHaptics({.motor = motor,
                                    .kp_factor = 2,