From fcea8e4cc758ff926cbf51b3978cd97a8ca7b0aa Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 29 Jun 2026 10:11:26 -0500 Subject: [PATCH 1/3] feat(bldc): Refactor bldc some to create new `bldc_types` component; add `bldc_current_sense` (untested); Improve motorgo APIs; Update BLDC examples to support running on Motorgo boards --- .github/workflows/build.yml | 2 + .github/workflows/upload_components.yml | 2 + components/bldc_current_sense/CMakeLists.txt | 4 + components/bldc_current_sense/README.md | 41 ++++ ...ent_sense_example.cpp.C08DCA03C693871A.idx | Bin 0 -> 3156 bytes .../bldc_current_sense/example/CMakeLists.txt | 22 ++ .../bldc_current_sense/example/README.md | 34 +++ .../example/main/CMakeLists.txt | 2 + .../main/bldc_current_sense_example.cpp | 90 ++++++++ .../example/sdkconfig.defaults | 14 ++ .../bldc_current_sense/idf_component.yml | 25 +++ .../include/current_sense.hpp | 211 ++++++++++++++++++ .../bldc_driver/include/bldc_driver.hpp | 69 ++++++ components/bldc_haptics/CMakeLists.txt | 2 +- .../bldc_haptics/example/CMakeLists.txt | 2 +- .../example/main/Kconfig.projbuild | 32 ++- .../example/main/bldc_haptics_example.cpp | 111 ++++++--- components/bldc_haptics/idf_component.yml | 1 + .../bldc_haptics/include/bldc_haptics.hpp | 16 +- components/bldc_motor/CMakeLists.txt | 2 +- components/bldc_motor/example/CMakeLists.txt | 2 +- .../bldc_motor/example/main/Kconfig.projbuild | 32 ++- .../example/main/bldc_motor_example.cpp | 129 +++++++---- components/bldc_motor/idf_component.yml | 1 + components/bldc_motor/include/bldc_motor.hpp | 55 +---- components/bldc_types/CMakeLists.txt | 4 + components/bldc_types/idf_component.yml | 19 ++ .../bldc_types/include/bldc_concepts.hpp | 73 ++++++ .../include/bldc_types.hpp | 18 +- .../include/foc_utils.hpp | 0 .../include/sensor_direction.hpp | 0 components/motorgo-axis/CMakeLists.txt | 2 +- components/motorgo-axis/README.md | 41 ++++ components/motorgo-axis/idf_component.yml | 2 + .../motorgo-axis/include/motorgo-axis.hpp | 33 +++ components/motorgo-axis/src/motorgo-axis.cpp | 78 +++++++ components/motorgo-mini/README.md | 37 +++ .../example/main/motorgo_mini_example.cpp | 8 +- .../motorgo-mini/include/motorgo-mini.hpp | 116 +++++++++- components/motorgo-mini/src/motorgo-mini.cpp | 187 ++++++++++++++++ doc/Doxyfile | 8 +- doc/en/bldc/bldc_current_sense.rst | 43 ++++ doc/en/bldc/bldc_current_sense_example.md | 2 + doc/en/bldc/bldc_motor.rst | 14 +- doc/en/bldc/bldc_types.rst | 28 +++ doc/en/bldc/index.rst | 2 + 46 files changed, 1462 insertions(+), 154 deletions(-) create mode 100644 components/bldc_current_sense/CMakeLists.txt create mode 100644 components/bldc_current_sense/README.md create mode 100644 components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx create mode 100644 components/bldc_current_sense/example/CMakeLists.txt create mode 100644 components/bldc_current_sense/example/README.md create mode 100644 components/bldc_current_sense/example/main/CMakeLists.txt create mode 100644 components/bldc_current_sense/example/main/bldc_current_sense_example.cpp create mode 100644 components/bldc_current_sense/example/sdkconfig.defaults create mode 100644 components/bldc_current_sense/idf_component.yml create mode 100644 components/bldc_current_sense/include/current_sense.hpp create mode 100644 components/bldc_types/CMakeLists.txt create mode 100644 components/bldc_types/idf_component.yml create mode 100644 components/bldc_types/include/bldc_concepts.hpp rename components/{bldc_motor => bldc_types}/include/bldc_types.hpp (79%) rename components/{bldc_motor => bldc_types}/include/foc_utils.hpp (100%) rename components/{bldc_motor => bldc_types}/include/sensor_direction.hpp (100%) create mode 100644 doc/en/bldc/bldc_current_sense.rst create mode 100644 doc/en/bldc/bldc_current_sense_example.md create mode 100644 doc/en/bldc/bldc_types.rst diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 764e4ad5b..1630cdc53 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -39,6 +39,8 @@ jobs: target: esp32s3 - path: 'components/binary-log/example' target: esp32s3 + - path: 'components/bldc_current_sense/example' + target: esp32s3 - path: 'components/bldc_haptics/example' target: esp32s3 - path: 'components/bldc_motor/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index c6e179a2a..c9e9c7a0b 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -39,9 +39,11 @@ jobs: components/base_peripheral components/bdc_driver components/binary-log + components/bldc_current_sense components/bldc_driver components/bldc_haptics components/bldc_motor + components/bldc_types components/ble_gatt_server components/bm8563 components/bmi270 diff --git a/components/bldc_current_sense/CMakeLists.txt b/components/bldc_current_sense/CMakeLists.txt new file mode 100644 index 000000000..e1504e517 --- /dev/null +++ b/components/bldc_current_sense/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES base_component math bldc_driver bldc_types + ) diff --git a/components/bldc_current_sense/README.md b/components/bldc_current_sense/README.md new file mode 100644 index 000000000..b47c3c0e4 --- /dev/null +++ b/components/bldc_current_sense/README.md @@ -0,0 +1,41 @@ +# BLDC Current Sense + +[![Badge](https://components.espressif.com/components/espp/bldc_current_sense/badge.svg)](https://components.espressif.com/components/espp/bldc_current_sense) + +The `bldc_current_sense` component provides `espp::CurrentSense`, a field-oriented +control (FOC) current sensor for BLDC motors. It implements the +`CurrentSensorConcept` used by `espp::BldcMotor`, so it can be supplied as a +motor's current sense to enable current-feedback torque control +(`TorqueControlType::DC_CURRENT` and `TorqueControlType::FOC_CURRENT`). + +`espp::CurrentSense` is intentionally decoupled from any specific ADC. You +provide a `read_phase_currents` callback that returns the most recently sampled +phase currents (in amps). The class then: + +- subtracts the per-phase zero-current offset captured during `driver_align()`, +- applies any per-phase gain-sign correction, +- reconstructs an unmeasured phase (return `NAN` for it) assuming `Ia + Ib + Ic = 0`, +- runs the Clarke + Park transforms to produce the d/q currents + (`get_foc_currents()`) and the signed DC current magnitude (`get_dc_current()`). + +## PWM-synchronized sampling + +Accurate low-side current sensing requires sampling while the low-side FETs are +conducting (the PWM center). Use +`espp::BldcDriver::register_pwm_sample_callback()` to trigger your ADC read at +the timer "empty" (TEZ) event, and have your `read_phase_currents` callback +return the latest sampled values. + +## Status + +> **Experimental.** Zero-current offset calibration is implemented, but automatic +> phase-ordering / gain-sign discovery is not — set `phase_gain_signs` in the +> `Config` if a sense channel is inverted. Current-mode torque control depends on +> accurate, PWM-synchronized sampling and per-board tuning and must be validated +> on hardware. + +## Example + +The example builds a software model of the analog front-end (no motor hardware +required) to exercise the calibration + Clarke/Park pipeline, and statically +verifies that `espp::CurrentSense` satisfies the `CurrentSensorConcept`. diff --git a/components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx b/components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx new file mode 100644 index 0000000000000000000000000000000000000000..545220055623517308e1959e1be8ec630644dd35 GIT binary patch literal 3156 zcmZ8i2{@GN7oT^C8Osy4|9U6y{`MET8CYU zo76HPO_2_@RHNO`Q&x=(UxaooKVb`F!<46Qx^Mg=MnXHEteoN&DAKA9WIHX4q||0! zEg4R!w3+2}%iznn-ZK6LuZS1Th>NON36c83Z;W`+T9r79zM$*+enB=VHs)_QMg~(E z${}$uC*a!IuQtP6+Jp9W$oo!J^@+1fAqAQtK?47IW$~x->4qb0U6N}0NW*0t>Q;!6 zfLn!>4u8-4h0@F%O?4QeGBPG={Zd)pCTqTUOH~r){TjaNocn=d4&TyNX#_mKCC{Vt z^5p*N*HI#Gu+6V?Hc}q;uz1U5__2dlA1>fHgXMNWk7guXFsF zL#ph6uLgc0A)Mm-SNA*0YX#+@^CO%5H2tK3s*vh1q;5L--_{3tVU?FDMWf<_KFH78rXhZc#O8@C9kjYmQEb;53ul5 zWY@;!g`HwbKgOwR`41av{z3ftZ;R|FHTNxUkuLbJD zW0vRK5QWqBp*4hyKYU-wr+iYKH`I5uyZPO+Got1h%O&}kjOeZQJD23#U5U0O%kFMH zfvKpxoQEz04TDA#2lMYo);7NU*w&ceshN{7XBt1I%j8ZWm-__=j637HZ9hW+-&=0?1tfn=5b(<|kVxkvG^T01I{pwR$N{?R+8)_UR;|jSeWn#GHJd)K)+vccvgYWgU(TKvpxqE&Z zqA7-Z&79Ae@NGS8)bo2pGW++Zov+GcC+x(b+R)hOxyU5zlKT6V*oMZ^w!8M{#qV-w z*QU7^303XcQ)e;!*7T8sqK_o$(OxENLa9rPxM&kRxOO;xp1C8soj=(HXXQmLMV}Gp zC^Kfurj(75H(sP(3V6lWkW@6EZP`1@Tvo3f*BqR|6HjT6?wWzqWNPU7oPT!kF3JGiG5Sl zqlD*a3!D1~8U-Y=#HF_?XD+XOnrQAM+>Rd~om+mKa}*tG5t8WA{=lhn-mZ^JKPEw) zyLaFid<0E+hVy^?t;iYPvgRweu4nX1jY5(5T0 zVw1{k?MVq)dBwCLmbQ384(&C|8u95T%@PHtulqNaJN<8-_1`+_9rnR4%=M*66XmuD z`A5jWOn=CRuk+8NfrwW;TTPbcKe<@y`LVE|i@qKL00vVH`OF!oV$OnsH-iQMqXH3x z2x^9L3mz> zCrLJHhH$`|=wYcq39TfwO(6u6N{q~AJF0{Qe)ktbh*gM(3K$eZaN{1Zznghij)Fhf z4uj#x2vPxqLJ*mR%_5PQ$5b@DXB#5UFD^_43<^P}Rp%RNJmVuMc=t9$m|vLgg+U=m zpx5aj7N+0bfwOJ51WJ*E3S_xt`JrGx48Jy*eZ7u=)Ae`+9<@^|U9`eGKw@vxkf0#@ zO$D%*3Y-L-#2~$AG8uDdQj7rT<~UXy0&)vAo)MkV?OoC3@Nyau4EF6Q(a*8RieavS z(BOmILPG&`bH*efb2jO*tI^r_k1?_pM~L%5e(0hMn~VS?-V*SfGoL=|&TuQsD!Y@_ z=!Nr{hYLr28G1{3OC=~A^b6>NXABqWr3l-wY5tzp&|m3cHDn-M>tNRKtBo}ZLoX*P zCkw@`r&(ba_w?rff0v;28Qs{u6m)Ynq}oo^(2g*wW-JC|hK^~*HCA9NnCN(6R$)$P z3+N(-6~hUc(M2pPmJ{mTv3}k3-u>TN8A;;Z@E%a|P_m3-8M_5VIYu@0DT+rv_|+VJ zOJfL6b9^Zrp3Tt@_~@gTaFT@dPx}UqCc{&QfB8R*cZKv&c8n?*iwXIE#Zt$sG{!l~ zOkW{oq%tRjr;A2Nqurt{yDitQ+r!AiJGY5%lUcn*Z;vyA;@ELJU$M=R3r<`Ku1gHP z9IM>U!~?y2PBNw$6nH8qoWXR`zfb7UcJXby`1W0V*Dn6vZ=Q(?IGS|U-^PblBHBPZ zxCc5wC+Gq+a38?v&(7)i&7bo1^jV-^+yQsNZ2)It-owI*K(euOaB`uK2b>S|_45yM zIOJ$&@BF8$)8S(-M~+?~T}%Y=;4(-6p}`?ur#wC6&YmNlNdc)K8K|jRTj~FyxKBk{ zLmjWArz>;7NLm&vviF3Pq=~WjX?aTvcQ;%t2n#1iUWx-TAR-Dx16vzIgCyW%YNoA4 z0et)dC_&Br2hC54ic5&;5EOWXF}&PDRiFfvf?`k${sz}U0k{Uzz*Ue9azGZy0Dpl@ nkOy)>I;a4Z;0h=NMW7tq1ZAL(j=lkIfojkS@<9y%^#JxiUP|;z literal 0 HcmV?d00001 diff --git a/components/bldc_current_sense/example/CMakeLists.txt b/components/bldc_current_sense/example/CMakeLists.txt new file mode 100644 index 000000000..dc5a3475a --- /dev/null +++ b/components/bldc_current_sense/example/CMakeLists.txt @@ -0,0 +1,22 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +# add the component directories that we want to use +set(EXTRA_COMPONENT_DIRS + "../../../components/" +) + +set( + COMPONENTS + "main esptool_py task monitor bldc_types bldc_driver bldc_current_sense" + CACHE STRING + "List of components to include" + ) + +project(bldc_current_sense_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/bldc_current_sense/example/README.md b/components/bldc_current_sense/example/README.md new file mode 100644 index 000000000..ee99a5e0b --- /dev/null +++ b/components/bldc_current_sense/example/README.md @@ -0,0 +1,34 @@ +# BLDC Current Sense Example + +This example exercises the `espp::CurrentSense` FOC current sensor without any +motor hardware. It: + +- statically asserts that `espp::CurrentSense` satisfies the + `CurrentSensorConcept` required by `espp::BldcMotor`, +- builds a small software model of the analog front-end that produces phase + currents from a commanded `(Id, Iq)` at a given electrical angle (with a fixed + per-channel bias, and phase C unmeasured to exercise reconstruction), +- runs `driver_align()` to capture the zero-current offsets, +- commands a known q-axis current and prints the recovered `Id`/`Iq` and signed + DC current across a range of electrical angles, confirming the offset removal + + Clarke/Park + phase-reconstruction pipeline. + +## How to use example + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +``` +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type ``Ctrl-]``.) + +## Example Output + +The recovered `Iq` should track the commanded value (with `Id ~ 0`) at every +electrical angle, demonstrating the current-sense math end to end. diff --git a/components/bldc_current_sense/example/main/CMakeLists.txt b/components/bldc_current_sense/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/bldc_current_sense/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/bldc_current_sense/example/main/bldc_current_sense_example.cpp b/components/bldc_current_sense/example/main/bldc_current_sense_example.cpp new file mode 100644 index 000000000..cf3642f91 --- /dev/null +++ b/components/bldc_current_sense/example/main/bldc_current_sense_example.cpp @@ -0,0 +1,90 @@ +#include +#include +#include +#include + +#include "bldc_concepts.hpp" +#include "bldc_driver.hpp" +#include "current_sense.hpp" +#include "logger.hpp" + +using namespace std::chrono_literals; + +// Verify at compile time that espp::CurrentSense satisfies the +// CurrentSensorConcept that espp::BldcMotor requires for its current sense. +static_assert(espp::CurrentSensorConcept>, + "CurrentSense must satisfy CurrentSensorConcept"); + +// --------------------------------------------------------------------------- +// Simple software model of the analog front-end so the example can run without +// any motor hardware. It produces phase currents (in amps) from a commanded +// (Id, Iq) at a given electrical angle, plus a fixed per-channel bias to +// exercise the offset calibration. Phase C is reported as NAN to exercise the +// two-phase reconstruction path. +// --------------------------------------------------------------------------- +struct SimFrontEnd { + std::atomic id{0.0f}; + std::atomic iq{0.0f}; + std::atomic angle{0.0f}; + // analog bias of the sense channels (what offset calibration should remove) + float bias_a{1.65f}; + float bias_b{1.66f}; + + espp::PhaseCurrent read() const { + const float th = angle.load(); + const float a = id.load() * std::cos(th) - iq.load() * std::sin(th); // alpha + const float b = id.load() * std::sin(th) + iq.load() * std::cos(th); // beta + // inverse (amplitude-invariant) Clarke -> phase currents + espp::PhaseCurrent pc; + pc.a = a + bias_a; + pc.b = (-0.5f * a + espp::_SQRT3_2 * b) + bias_b; + pc.c = NAN; // phase C not measured on this "board" + return pc; + } +}; + +extern "C" void app_main(void) { + espp::Logger logger( + {.tag = "BLDC Current Sense Example", .level = espp::Logger::Verbosity::INFO}); + logger.info("Starting example!"); + + //! [bldc current sense example] + static SimFrontEnd front_end; + + espp::CurrentSense current_sense({ + // the read callback returns the latest sampled phase currents (amps). On + // real hardware you would trigger this read from + // BldcDriver::register_pwm_sample_callback() so it is PWM-synchronized. + .read_phase_currents = []() -> espp::PhaseCurrent { return front_end.read(); }, + .driver = nullptr, // no driver needed for this offline example + .calibration_samples = 200, + .log_level = espp::Logger::Verbosity::INFO, + }); + + // 1) Calibrate the zero-current offsets while the "motor" is at rest. + front_end.id = 0.0f; + front_end.iq = 0.0f; + bool ok = current_sense.driver_align(1.0f); + logger.info("Calibration {}", ok ? "succeeded" : "failed"); + + // 2) Command a known (Id, Iq) at a few electrical angles and confirm the + // current sense recovers it (Id ~ 0, Iq ~ commanded), proving the + // offset removal + Clarke/Park + phase reconstruction pipeline. + const float commanded_iq = 2.5f; + front_end.id = 0.0f; + front_end.iq = commanded_iq; + for (float angle = 0.0f; angle < espp::_2PI; angle += espp::_PI_3) { + front_end.angle = angle; + auto dq = current_sense.get_foc_currents(angle); + float dc = current_sense.get_dc_current(angle); + logger.info( + "angle={:5.2f} rad -> Id={:6.3f} A, Iq={:6.3f} A (commanded {:.3f}), |Idc|={:6.3f} A", + angle, dq.d, dq.q, commanded_iq, dc); + } + //! [bldc current sense example] + + logger.info("Example complete!"); + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/bldc_current_sense/example/sdkconfig.defaults b/components/bldc_current_sense/example/sdkconfig.defaults new file mode 100644 index 000000000..6826604fa --- /dev/null +++ b/components/bldc_current_sense/example/sdkconfig.defaults @@ -0,0 +1,14 @@ +CONFIG_IDF_TARGET="esp32s3" + +CONFIG_COMPILER_OPTIMIZATION_PERF=y + +CONFIG_FREERTOS_HZ=1000 + +CONFIG_ESPTOOLPY_FLASHSIZE_8MB=y +CONFIG_ESPTOOLPY_FLASHSIZE="8MB" + +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ=240 + +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 diff --git a/components/bldc_current_sense/idf_component.yml b/components/bldc_current_sense/idf_component.yml new file mode 100644 index 000000000..bb084a536 --- /dev/null +++ b/components/bldc_current_sense/idf_component.yml @@ -0,0 +1,25 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "FOC current sense for BLDC motor control (espp)" +url: "https://github.com/esp-cpp/espp/tree/main/components/bldc_current_sense" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/bldc/bldc_current_sense.html" +examples: + - path: example +tags: + - cpp + - Component + - BLDC + - Motor + - Control + - FOC + - Current +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/math: '>=1.0' + espp/bldc_driver: '>=1.0' + espp/bldc_types: '>=1.0' diff --git a/components/bldc_current_sense/include/current_sense.hpp b/components/bldc_current_sense/include/current_sense.hpp new file mode 100644 index 000000000..87e485429 --- /dev/null +++ b/components/bldc_current_sense/include/current_sense.hpp @@ -0,0 +1,211 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "fast_math.hpp" + +#include "bldc_driver.hpp" +#include "foc_utils.hpp" + +namespace espp { + +/// \brief Generic field-oriented-control (FOC) current sensor for a BLDC motor. +/// \details This class turns raw, per-phase current measurements (in amps) into +/// the quantities the FOC current loop needs: the d/q currents +/// (get_foc_currents()) and the signed DC current magnitude +/// (get_dc_current()). It implements the CurrentSensorConcept used by +/// espp::BldcMotor, so it can be supplied as the motor's current sense +/// to enable detail::TorqueControlType::DC_CURRENT and +/// detail::TorqueControlType::FOC_CURRENT control. +/// +/// The class is intentionally decoupled from any specific ADC: you +/// provide a `read_phase_currents` callback that returns the most +/// recently sampled phase currents (already converted to amps). For +/// accurate low-side current sensing those samples must be taken while +/// the low-side FETs are conducting (the PWM center). Use +/// espp::BldcDriver::register_pwm_sample_callback() to trigger your ADC +/// read at that instant and have the `read_phase_currents` callback +/// return the latest result. +/// +/// For boards that only measure two of the three phases, return NAN for +/// the unmeasured phase and it will be reconstructed assuming +/// Ia + Ib + Ic = 0. +/// +/// \note This component is EXPERIMENTAL. It performs zero-current offset +/// calibration in driver_align(), but automatic phase-ordering / gain-sign +/// discovery is not yet implemented (provide phase_gain_signs in the +/// Config if your wiring inverts a channel). Current-mode torque control +/// depends on accurate, PWM-synchronized sampling and per-board tuning and +/// must be validated on hardware. +/// +/// \tparam Driver The BLDC driver type (defaults to espp::BldcDriver). Only used +/// (optionally) during driver_align(). +/// +/// \section bldc_current_sense_ex1 Current Sense Example +/// \snippet bldc_current_sense_example.cpp bldc current sense example +template class CurrentSense : public BaseComponent { +public: + /// \brief Callback returning the most recently sampled phase currents in amps. + /// \details The returned currents may include the analog bias/offset of the + /// sense circuit; the offset captured by driver_align() is subtracted + /// internally. Set any phase that is not physically measured to NAN + /// and it will be reconstructed from the others (Ia + Ib + Ic = 0). + typedef std::function read_phase_currents_fn; + + /// \brief Configuration for the current sense. + struct Config { + read_phase_currents_fn read_phase_currents{ + nullptr}; ///< Callback returning raw phase currents (amps). + std::shared_ptr driver{nullptr}; ///< Optional driver, used by driver_align(). + size_t calibration_samples{500}; ///< Number of samples averaged for offset calibration. + std::array phase_gain_signs{ + 1.0f, 1.0f, 1.0f}; ///< Per-phase (a,b,c) gain sign to correct inverted sense channels. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Log verbosity. + }; + + /// \brief Construct a new CurrentSense. + /// \param config The configuration. + explicit CurrentSense(const Config &config) + : BaseComponent("CurrentSense", config.log_level) + , read_phase_currents_(config.read_phase_currents) + , driver_(config.driver) + , calibration_samples_(config.calibration_samples) + , gain_sign_(config.phase_gain_signs) {} + + /// \brief Align the current sense with the driver. + /// \param voltage Alignment voltage (reserved for future active phase/gain + /// discovery; currently unused beyond logging). + /// \return True if calibration succeeded. + /// \details Captures the zero-current ADC offset for each measured phase by + /// averaging `calibration_samples` reads. The motor must be at rest + /// with no phase current while this runs (the BldcMotor calls this + /// after the phases have been driven to zero). + /// \note Non-const because it mutates the captured calibration state, as + /// required by the CurrentSensorConcept. + bool driver_align(float voltage) { + if (!read_phase_currents_) { + logger_.error("No read_phase_currents callback provided, cannot calibrate"); + return false; + } + logger_.info("Calibrating current sense offsets ({} samples, align voltage {:.2f})", + calibration_samples_, voltage); + using namespace std::chrono_literals; + double sum_a = 0, sum_b = 0, sum_c = 0; + size_t n_a = 0, n_b = 0, n_c = 0; + for (size_t i = 0; i < calibration_samples_; i++) { + PhaseCurrent raw = read_phase_currents_(); + if (!std::isnan(raw.a)) { + sum_a += raw.a; + n_a++; + } + if (!std::isnan(raw.b)) { + sum_b += raw.b; + n_b++; + } + if (!std::isnan(raw.c)) { + sum_c += raw.c; + n_c++; + } + std::this_thread::sleep_for(1ms); + } + // if a phase was never measured, leave its offset at 0 (it is reconstructed) + offset_.a = n_a ? (float)(sum_a / n_a) : 0.0f; + offset_.b = n_b ? (float)(sum_b / n_b) : 0.0f; + offset_.c = n_c ? (float)(sum_c / n_c) : 0.0f; + if (n_a == 0 && n_b == 0 && n_c == 0) { + logger_.error("No phase currents were measured during calibration"); + return false; + } + if ((n_a == 0) + (n_b == 0) + (n_c == 0) > 1) { + logger_.error("Need at least two measured phases for FOC current sensing"); + return false; + } + logger_.info("Current sense offsets (A): a={:.4f} b={:.4f} c={:.4f}", offset_.a, offset_.b, + offset_.c); + calibrated_ = true; + return true; + } + + /// \brief Get the calibrated phase currents (amps). + /// \return Offset-removed, gain-sign-corrected phase currents, with the + /// unmeasured phase (if any) reconstructed from Ia + Ib + Ic = 0. + PhaseCurrent get_phase_currents() const { + PhaseCurrent raw = read_phase_currents_ ? read_phase_currents_() : PhaseCurrent{NAN, NAN, NAN}; + float a = (raw.a - offset_.a) * gain_sign_[0]; + float b = (raw.b - offset_.b) * gain_sign_[1]; + float c = (raw.c - offset_.c) * gain_sign_[2]; + // reconstruct a single unmeasured (NAN) phase assuming Ia + Ib + Ic = 0 + const bool na = std::isnan(a), nb = std::isnan(b), nc = std::isnan(c); + const int num_nan = (int)na + (int)nb + (int)nc; + if (num_nan == 1) { + if (na) + a = -(b + c); + else if (nb) + b = -(a + c); + else + c = -(a + b); + } + return {a, b, c}; + } + + /// \brief Get the d/q currents for the given electrical angle. + /// \param electrical_angle The motor electrical angle (radians). + /// \return The d (direct) and q (quadrature) currents in amps. + DqCurrent get_foc_currents(float electrical_angle) const { + float i_alpha, i_beta; + clarke(get_phase_currents(), i_alpha, i_beta); + // Park transform + const float ct = fast_cos(electrical_angle); + const float st = fast_sin(electrical_angle); + DqCurrent dq; + dq.d = i_alpha * ct + i_beta * st; + dq.q = i_beta * ct - i_alpha * st; + return dq; + } + + /// \brief Get the signed DC current magnitude. + /// \param electrical_angle The motor electrical angle (radians), used to give + /// the magnitude a sign relative to the drive (q) direction. + /// \return The signed current magnitude in amps. + float get_dc_current(float electrical_angle) const { + float i_alpha, i_beta; + clarke(get_phase_currents(), i_alpha, i_beta); + const float magnitude = std::sqrt(i_alpha * i_alpha + i_beta * i_beta); + // sign of the q-axis (drive direction) current + const float i_q = i_beta * fast_cos(electrical_angle) - i_alpha * fast_sin(electrical_angle); + return (i_q >= 0.0f ? 1.0f : -1.0f) * magnitude; + } + + /// \brief Whether the current sense has been calibrated (driver_align ran). + /// \return True if calibrated. + bool is_calibrated() const { return calibrated_; } + +protected: + /// \brief Amplitude-invariant Clarke transform (abc -> alpha/beta). + /// \details Removes the common-mode (zero-sequence) component before + /// projecting, so it is robust whether all three phases are measured + /// or the third was reconstructed. + void clarke(const PhaseCurrent &i, float &i_alpha, float &i_beta) const { + const float mid = (1.0f / 3.0f) * (i.a + i.b + i.c); + const float a = i.a - mid; + const float b = i.b - mid; + i_alpha = a; + i_beta = _1_SQRT3 * a + _2_SQRT3 * b; + } + + read_phase_currents_fn read_phase_currents_{nullptr}; + std::shared_ptr driver_{nullptr}; + size_t calibration_samples_{500}; + PhaseCurrent offset_{0.0f, 0.0f, 0.0f}; + std::array gain_sign_{1.0f, 1.0f, 1.0f}; + bool calibrated_{false}; +}; + +} // namespace espp diff --git a/components/bldc_driver/include/bldc_driver.hpp b/components/bldc_driver/include/bldc_driver.hpp index 3f2324d9a..fc2db2332 100644 --- a/components/bldc_driver/include/bldc_driver.hpp +++ b/components/bldc_driver/include/bldc_driver.hpp @@ -3,9 +3,11 @@ #include #include #include +#include #include "driver/gpio.h" #include "driver/mcpwm_prelude.h" +#include "esp_attr.h" #include "esp_idf_version.h" #ifndef ESP_IDF_VERSION_VAL #define ESP_IDF_VERSION_VAL(major, minor, patch) (((major) << 16) | ((minor) << 8) | (patch)) @@ -151,6 +153,57 @@ class BldcDriver : public BaseComponent { return gpio_get_level((gpio_num_t)gpio_fault_) == 1; } + /** + * @brief Register a callback fired once per PWM period at the timer "empty" + * (TEZ) event, i.e. when the counter reaches zero. + * @param callback Function to call at each TEZ event. Pass nullptr to clear a + * previously registered callback. + * @return True if the callback was registered (or cleared) successfully. + * @details For this driver's center-aligned (up/down) configuration the + * low-side FETs are conducting around the TEZ event, which is the + * correct instant to sample low-side shunt currents for FOC current + * sensing. Wire this to start / read your current-sense ADC so the + * samples are synchronized to the PWM. + * @note The callback runs in interrupt context. Keep it short (e.g. kick off + * an ADC conversion or read a pre-triggered result) and non-blocking. If + * CONFIG_MCPWM_ISR_IRAM_SAFE is enabled, the callback and everything it + * touches must be in IRAM. + * @note Registering the callback briefly disables and re-enables the MCPWM + * timer (the ESP-IDF API requires the timer to be in the init state to + * attach event callbacks), so register it during setup before the motor + * is spinning. + */ + bool register_pwm_sample_callback(std::function callback) { + pwm_sample_callback_ = std::move(callback); + // the trampoline only needs to be attached once; subsequent calls just swap + // the stored std::function. + if (sample_cbs_registered_) { + return true; + } + // the ESP-IDF MCPWM API requires the timer to be in the init (disabled) + // state to register event callbacks, so disable it if necessary and + // re-enable afterwards. + bool was_enabled = enabled_; + if (was_enabled) { + disable(); + } + mcpwm_timer_event_callbacks_t cbs = {}; + cbs.on_empty = &BldcDriver::on_timer_empty; + auto err = mcpwm_timer_register_event_callbacks(timer_, &cbs, this); + if (err != ESP_OK) { + logger_.error("Failed to register PWM sample callback: {}", esp_err_to_name(err)); + if (was_enabled) { + enable(); + } + return false; + } + sample_cbs_registered_ = true; + if (was_enabled) { + enable(); + } + return true; + } + /** * @brief This function does nothing, merely exists for later if we choose * to implement it as part of the FOC control algorithm. @@ -267,6 +320,18 @@ class BldcDriver : public BaseComponent { protected: static int GROUP_ID; + // ISR trampoline for the MCPWM timer "empty" (TEZ) event. Runs in interrupt + // context and forwards to the user-registered PWM sample callback (if any). + static bool IRAM_ATTR on_timer_empty(mcpwm_timer_handle_t, const mcpwm_timer_event_data_t *, + void *user_ctx) { + auto *self = static_cast(user_ctx); + if (self && self->pwm_sample_callback_) { + self->pwm_sample_callback_(); + } + // we did not wake a higher priority task + return false; + } + void init(const Config &config) { #if defined(SOC_MCPWM_GROUPS) if (GROUP_ID >= SOC_MCPWM_GROUPS) { @@ -460,5 +525,9 @@ class BldcDriver : public BaseComponent { std::array operators_; std::array comparators_; std::array generators_; + + // PWM-synchronized current-sampling support + std::function pwm_sample_callback_{nullptr}; + bool sample_cbs_registered_{false}; }; } // namespace espp diff --git a/components/bldc_haptics/CMakeLists.txt b/components/bldc_haptics/CMakeLists.txt index a4fc9a32e..2293fd1c7 100644 --- a/components/bldc_haptics/CMakeLists.txt +++ b/components/bldc_haptics/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" - REQUIRES base_component math pid task bldc_motor + REQUIRES base_component math pid task bldc_types bldc_motor ) diff --git a/components/bldc_haptics/example/CMakeLists.txt b/components/bldc_haptics/example/CMakeLists.txt index 8ce1af882..1eeb68115 100644 --- a/components/bldc_haptics/example/CMakeLists.txt +++ b/components/bldc_haptics/example/CMakeLists.txt @@ -12,7 +12,7 @@ set(EXTRA_COMPONENT_DIRS set( COMPONENTS - "main esptool_py task monitor mt6701 bldc_motor bldc_driver bldc_haptics i2c" + "main esptool_py task monitor mt6701 bldc_motor bldc_driver bldc_haptics i2c motorgo-mini motorgo-axis" CACHE STRING "List of components to include" ) diff --git a/components/bldc_haptics/example/main/Kconfig.projbuild b/components/bldc_haptics/example/main/Kconfig.projbuild index eaedcec09..e440d5db2 100644 --- a/components/bldc_haptics/example/main/Kconfig.projbuild +++ b/components/bldc_haptics/example/main/Kconfig.projbuild @@ -10,24 +10,52 @@ menu "Example Configuration" depends on IDF_TARGET_ESP32S3 bool "BLDC Motor Test Stand (TinyS3)" + config EXAMPLE_HARDWARE_MOTORGO_MINI + depends on IDF_TARGET_ESP32S3 + bool "MotorGo Mini" + + config EXAMPLE_HARDWARE_MOTORGO_AXIS + depends on IDF_TARGET_ESP32S3 + bool "MotorGo Axis" + config EXAMPLE_HARDWARE_CUSTOM bool "Custom" endchoice + choice EXAMPLE_MOTOR_CHANNEL + prompt "Motor channel" + depends on EXAMPLE_HARDWARE_MOTORGO_MINI || EXAMPLE_HARDWARE_MOTORGO_AXIS + default EXAMPLE_MOTOR_CHANNEL_1 + help + Select which motor channel of the MotorGo board to drive. + + config EXAMPLE_MOTOR_CHANNEL_1 + bool "Motor 1" + + config EXAMPLE_MOTOR_CHANNEL_2 + bool "Motor 2" + endchoice + config EXAMPLE_I2C_SCL_GPIO int "SCL GPIO Num" range 0 50 default 9 if EXAMPLE_HARDWARE_TEST_STAND default 19 if EXAMPLE_HARDWARE_CUSTOM + default 0 + depends on EXAMPLE_HARDWARE_TEST_STAND || EXAMPLE_HARDWARE_CUSTOM help - GPIO number for I2C Master clock line. + GPIO number for I2C Master clock line. Only used for the test-stand / + custom wiring; the MotorGo boards use their own SSI encoder bus. config EXAMPLE_I2C_SDA_GPIO int "SDA GPIO Num" range 0 50 default 8 if EXAMPLE_HARDWARE_TEST_STAND default 22 if EXAMPLE_HARDWARE_CUSTOM + default 0 + depends on EXAMPLE_HARDWARE_TEST_STAND || EXAMPLE_HARDWARE_CUSTOM help - GPIO number for I2C Master data line. + GPIO number for I2C Master data line. Only used for the test-stand / + custom wiring; the MotorGo boards use their own SSI encoder bus. endmenu diff --git a/components/bldc_haptics/example/main/bldc_haptics_example.cpp b/components/bldc_haptics/example/main/bldc_haptics_example.cpp index e7db74c08..d1f0d50b6 100644 --- a/components/bldc_haptics/example/main/bldc_haptics_example.cpp +++ b/components/bldc_haptics/example/main/bldc_haptics_example.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -9,39 +10,87 @@ #include "mt6701.hpp" #include "task.hpp" +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI +#include "motorgo-mini.hpp" +#elif CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +#include "motorgo-axis.hpp" +#endif + using namespace std::chrono_literals; +// The MotorGo boards route the magnetic encoder over an SSI bus, while the +// test-stand / custom wiring uses an I2C MT6701. +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +using Encoder = espp::Mt6701; +#else +using Encoder = espp::Mt6701<>; +#endif +using BldcMotor = espp::BldcMotor; + +// Which MotorGo channel to drive (index 0 == "Motor 1", index 1 == "Motor 2"). +#if CONFIG_EXAMPLE_MOTOR_CHANNEL_2 +static constexpr size_t example_motor_index = 1; +#else +static constexpr size_t example_motor_index = 0; +#endif + extern "C" void app_main(void) { espp::Logger logger({.tag = "BLDC Haptics Example", .level = espp::Logger::Verbosity::DEBUG}); constexpr int num_seconds_to_run = 20; { logger.info("Running BLDC Haptics example for {} seconds!", num_seconds_to_run); + static constexpr float core_update_period = 0.001f; // seconds + + // The motor and driver are set up below depending on the selected hardware. + std::shared_ptr driver; + std::shared_ptr motor; + // Objects which must outlive the motor for the standalone (I2C) wiring. + std::unique_ptr i2c; + std::shared_ptr standalone_encoder; + +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI + using Board = espp::MotorGoMini; + logger.info("Using MotorGo Mini, motor channel {}", example_motor_index + 1); +#else + using Board = espp::MotorGoAxis; + logger.info("Using MotorGo Axis, motor channel {}", example_motor_index + 1); +#endif + // Both MotorGo boards expose the same symmetric, index-based API, so the + // rest of the setup is identical regardless of which board is selected. + auto &board = Board::get(); + board.set_log_level(espp::Logger::Verbosity::INFO); + board.initialize_encoders(); // start the encoder update task(s) + board.initialize_motors(); // create the motor driver(s) + auto motor_config = board.default_motor_config(example_motor_index); + // tweak motor_config here if desired (PID gains, current limit, etc.) + motor = board.initialize_motor(example_motor_index, motor_config); + driver = board.motor_driver(example_motor_index); +#else + logger.info("Using test-stand / custom wiring (I2C MT6701 + TMC6300)"); // make the I2C that we'll use to communicate with the mt6701 (magnetic encoder) logger.info("initializing i2c driver..."); - espp::I2c i2c({ + i2c = std::make_unique(espp::I2c::Config{ .port = I2C_NUM_1, .sda_io_num = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO, .scl_io_num = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO, .clk_speed = 1 * 1000 * 1000, // MT6701 supports 1 MHz I2C }); - static constexpr float core_update_period = 0.001f; // seconds - // now make the mt6701 which decodes the data - using Encoder = espp::Mt6701<>; std::error_code ec; auto encoder_device = - i2c.add_device({.device_address = Encoder::DEFAULT_ADDRESS, - .timeout_ms = static_cast(i2c.config().timeout_ms), - .scl_speed_hz = i2c.config().clk_speed, - .log_level = espp::Logger::Verbosity::WARN}, - ec); + i2c->add_device({.device_address = Encoder::DEFAULT_ADDRESS, + .timeout_ms = static_cast(i2c->config().timeout_ms), + .scl_speed_hz = i2c->config().clk_speed, + .log_level = espp::Logger::Verbosity::WARN}, + ec); if (!encoder_device) { logger.error("Failed to initialize MT6701 I2C device: {}", ec.message()); return; } - std::shared_ptr mt6701 = std::make_shared( + standalone_encoder = std::make_shared( Encoder::Config{.write = espp::make_i2c_addressed_write(encoder_device), .read = espp::make_i2c_addressed_read(encoder_device), .velocity_filter = nullptr, // no filtering @@ -49,25 +98,23 @@ extern "C" void app_main(void) { .log_level = espp::Logger::Verbosity::WARN}); // now make the bldc driver - std::shared_ptr driver = - std::make_shared(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::DEBUG}); + driver = std::make_shared(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::DEBUG}); // now make the bldc motor - using BldcMotor = espp::BldcMotor; - auto motor = std::make_shared(BldcMotor::Config{ + motor = std::make_shared(BldcMotor::Config{ // measured by setting it into ANGLE_OPENLOOP and then counting how many // spots you feel when rotating it. .num_pole_pairs = 7, @@ -82,7 +129,7 @@ extern "C" void app_main(void) { // this is a test .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM, .driver = driver, - .sensor = mt6701, + .sensor = standalone_encoder, .velocity_pid_config = { .kp = 0.010f, @@ -104,6 +151,12 @@ extern "C" void app_main(void) { .output_max = 20.0, // angle pid works on velocity (rad/s) }, .log_level = espp::Logger::Verbosity::DEBUG}); +#endif + + if (!motor || !driver) { + logger.error("Motor / driver were not initialized, cannot run example!"); + return; + } auto print_detent_config = [&logger](const auto &detent_config) { if (detent_config == espp::detail::UNBOUNDED_NO_DETENTS) { @@ -189,6 +242,8 @@ extern "C" void app_main(void) { //! [bldc_haptics_example_2] } + haptic_motor.stop(); + driver->disable(); } diff --git a/components/bldc_haptics/idf_component.yml b/components/bldc_haptics/idf_component.yml index 53bef7428..96a807124 100644 --- a/components/bldc_haptics/idf_component.yml +++ b/components/bldc_haptics/idf_component.yml @@ -20,6 +20,7 @@ dependencies: idf: version: '>=5.0' espp/base_component: '>=1.0' + espp/bldc_types: '>=1.0' espp/bldc_motor: '>=1.0' espp/math: '>=1.0' espp/pid: '>=1.0' diff --git a/components/bldc_haptics/include/bldc_haptics.hpp b/components/bldc_haptics/include/bldc_haptics.hpp index e02947578..f5520e4ae 100644 --- a/components/bldc_haptics/include/bldc_haptics.hpp +++ b/components/bldc_haptics/include/bldc_haptics.hpp @@ -5,27 +5,13 @@ #include #include "base_component.hpp" +#include "bldc_concepts.hpp" #include "bldc_motor.hpp" #include "detent_config.hpp" #include "haptic_config.hpp" #include "task.hpp" namespace espp { -/// @brief Concept for a motor that can be used for haptics -template -concept MotorConcept = requires { - static_cast(&FOO::enable); ///< Enable the motor - static_cast(&FOO::disable); ///< Disable the motor - static_cast( - &FOO::move); ///< Move the motor to a new target (position, velocity, or torque depending on - ///< the motor control type) - static_cast( - &FOO::set_motion_control_type); ///< Set the motion control type - static_cast(&FOO::loop_foc); ///< Run the FOC loop - static_cast(&FOO::get_shaft_angle); ///< Get the shaft angle - static_cast(&FOO::get_shaft_velocity); ///< Get the shaft velocity - static_cast(&FOO::get_electrical_angle); ///< Get the electrical angle -}; /// @brief Class which creates haptic feedback for the user by vibrating the /// motor This class is based on the work at diff --git a/components/bldc_motor/CMakeLists.txt b/components/bldc_motor/CMakeLists.txt index dec80c1be..7a1b9f390 100644 --- a/components/bldc_motor/CMakeLists.txt +++ b/components/bldc_motor/CMakeLists.txt @@ -1,4 +1,4 @@ idf_component_register( INCLUDE_DIRS "include" - REQUIRES base_component math pid task + REQUIRES base_component bldc_types math pid task ) diff --git a/components/bldc_motor/example/CMakeLists.txt b/components/bldc_motor/example/CMakeLists.txt index 13e1a13e7..c7492099d 100644 --- a/components/bldc_motor/example/CMakeLists.txt +++ b/components/bldc_motor/example/CMakeLists.txt @@ -12,7 +12,7 @@ set(EXTRA_COMPONENT_DIRS set( COMPONENTS - "main esptool_py filters task monitor mt6701 bldc_motor bldc_driver i2c timer" + "main esptool_py filters task monitor mt6701 bldc_motor bldc_driver i2c timer motorgo-mini motorgo-axis" CACHE STRING "List of components to include" ) diff --git a/components/bldc_motor/example/main/Kconfig.projbuild b/components/bldc_motor/example/main/Kconfig.projbuild index eaedcec09..e440d5db2 100644 --- a/components/bldc_motor/example/main/Kconfig.projbuild +++ b/components/bldc_motor/example/main/Kconfig.projbuild @@ -10,24 +10,52 @@ menu "Example Configuration" depends on IDF_TARGET_ESP32S3 bool "BLDC Motor Test Stand (TinyS3)" + config EXAMPLE_HARDWARE_MOTORGO_MINI + depends on IDF_TARGET_ESP32S3 + bool "MotorGo Mini" + + config EXAMPLE_HARDWARE_MOTORGO_AXIS + depends on IDF_TARGET_ESP32S3 + bool "MotorGo Axis" + config EXAMPLE_HARDWARE_CUSTOM bool "Custom" endchoice + choice EXAMPLE_MOTOR_CHANNEL + prompt "Motor channel" + depends on EXAMPLE_HARDWARE_MOTORGO_MINI || EXAMPLE_HARDWARE_MOTORGO_AXIS + default EXAMPLE_MOTOR_CHANNEL_1 + help + Select which motor channel of the MotorGo board to drive. + + config EXAMPLE_MOTOR_CHANNEL_1 + bool "Motor 1" + + config EXAMPLE_MOTOR_CHANNEL_2 + bool "Motor 2" + endchoice + config EXAMPLE_I2C_SCL_GPIO int "SCL GPIO Num" range 0 50 default 9 if EXAMPLE_HARDWARE_TEST_STAND default 19 if EXAMPLE_HARDWARE_CUSTOM + default 0 + depends on EXAMPLE_HARDWARE_TEST_STAND || EXAMPLE_HARDWARE_CUSTOM help - GPIO number for I2C Master clock line. + GPIO number for I2C Master clock line. Only used for the test-stand / + custom wiring; the MotorGo boards use their own SSI encoder bus. config EXAMPLE_I2C_SDA_GPIO int "SDA GPIO Num" range 0 50 default 8 if EXAMPLE_HARDWARE_TEST_STAND default 22 if EXAMPLE_HARDWARE_CUSTOM + default 0 + depends on EXAMPLE_HARDWARE_TEST_STAND || EXAMPLE_HARDWARE_CUSTOM help - GPIO number for I2C Master data line. + GPIO number for I2C Master data line. Only used for the test-stand / + custom wiring; the MotorGo boards use their own SSI encoder bus. endmenu diff --git a/components/bldc_motor/example/main/bldc_motor_example.cpp b/components/bldc_motor/example/main/bldc_motor_example.cpp index 5278994a8..9e7e09e81 100644 --- a/components/bldc_motor/example/main/bldc_motor_example.cpp +++ b/components/bldc_motor/example/main/bldc_motor_example.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -12,41 +13,89 @@ #include "task.hpp" #include "task_monitor.hpp" +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI +#include "motorgo-mini.hpp" +#elif CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +#include "motorgo-axis.hpp" +#endif + using namespace std::chrono_literals; +// The MotorGo boards route the magnetic encoder over an SSI bus, while the +// test-stand / custom wiring uses an I2C MT6701. +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +using Encoder = espp::Mt6701; +#else +using Encoder = espp::Mt6701<>; +#endif +using BldcMotor = espp::BldcMotor; + +// Which MotorGo channel to drive (index 0 == "Motor 1", index 1 == "Motor 2"). +#if CONFIG_EXAMPLE_MOTOR_CHANNEL_2 +static constexpr size_t example_motor_index = 1; +#else +static constexpr size_t example_motor_index = 0; +#endif + extern "C" void app_main(void) { espp::Logger logger({.tag = "BLDC Motor example", .level = espp::Logger::Verbosity::DEBUG}); constexpr int num_seconds_to_run = 120; { logger.info("Running BLDC Motor (FOC) example for {} seconds!", num_seconds_to_run); + static constexpr uint64_t core_update_period_us = 1000; // microseconds + static constexpr float core_update_period = core_update_period_us / 1e6f; // seconds + + // The motor and driver are set up below depending on the selected hardware. + std::shared_ptr driver; + std::shared_ptr motor; + // Objects which must outlive the motor for the standalone (I2C) wiring. + std::unique_ptr i2c; + std::shared_ptr standalone_encoder; + + //! [bldc_motor example] +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS +#if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI + using Board = espp::MotorGoMini; + logger.info("Using MotorGo Mini, motor channel {}", example_motor_index + 1); +#else + using Board = espp::MotorGoAxis; + logger.info("Using MotorGo Axis, motor channel {}", example_motor_index + 1); +#endif + // Both MotorGo boards expose the same symmetric, index-based API, so the + // rest of the setup is identical regardless of which board is selected. + auto &board = Board::get(); + board.set_log_level(espp::Logger::Verbosity::INFO); + board.initialize_encoders(); // start the encoder update task(s) + board.initialize_motors(); // create the motor driver(s) + auto motor_config = board.default_motor_config(example_motor_index); + // tweak motor_config here if desired (PID gains, current limit, etc.) + motor = board.initialize_motor(example_motor_index, motor_config); + driver = board.motor_driver(example_motor_index); +#else + logger.info("Using test-stand / custom wiring (I2C MT6701 + TMC6300)"); // make the I2C that we'll use to communicate with the mt6701 (magnetic encoder) logger.info("initializing i2c driver..."); - espp::I2c i2c({ + i2c = std::make_unique(espp::I2c::Config{ .port = I2C_NUM_1, .sda_io_num = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO, .scl_io_num = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO, .clk_speed = 1 * 1000 * 1000, // MT6701 supports 1 MHz I2C }); - static constexpr uint64_t core_update_period_us = 1000; // microseconds - static constexpr float core_update_period = core_update_period_us / 1e6f; // seconds - - //! [bldc_motor example] // now make the mt6701 which decodes the data - using Encoder = espp::Mt6701<>; std::error_code ec; auto encoder_device = - i2c.add_device({.device_address = Encoder::DEFAULT_ADDRESS, - .timeout_ms = static_cast(i2c.config().timeout_ms), - .scl_speed_hz = i2c.config().clk_speed, - .log_level = espp::Logger::Verbosity::WARN}, - ec); + i2c->add_device({.device_address = Encoder::DEFAULT_ADDRESS, + .timeout_ms = static_cast(i2c->config().timeout_ms), + .scl_speed_hz = i2c->config().clk_speed, + .log_level = espp::Logger::Verbosity::WARN}, + ec); if (!encoder_device) { logger.error("Failed to initialize MT6701 I2C device: {}", ec.message()); return; } - std::shared_ptr mt6701 = std::make_shared( + standalone_encoder = std::make_shared( Encoder::Config{.write = espp::make_i2c_addressed_write(encoder_device), .read = espp::make_i2c_addressed_read(encoder_device), .velocity_filter = nullptr, // no filtering @@ -54,25 +103,23 @@ extern "C" void app_main(void) { .log_level = espp::Logger::Verbosity::WARN}); // now make the bldc driver - std::shared_ptr driver = - std::make_shared(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}); + driver = std::make_shared(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; - auto motor = BldcMotor(BldcMotor::Config{ + motor = std::make_shared(BldcMotor::Config{ // measured by setting it into ANGLE_OPENLOOP and then counting how many // spots you feel when rotating it. .num_pole_pairs = 7, @@ -87,7 +134,7 @@ extern "C" void app_main(void) { // test .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM, .driver = driver, - .sensor = mt6701, + .sensor = standalone_encoder, .velocity_pid_config = { .kp = 0.010f, @@ -109,6 +156,12 @@ extern "C" void app_main(void) { .output_max = 20.0, // angle pid works on velocity (rad/s) }, .log_level = espp::Logger::Verbosity::DEBUG}); +#endif + + if (!motor || !driver) { + logger.error("Motor / driver were not initialized, cannot run example!"); + return; + } static const auto motion_control_type = espp::detail::MotionControlType::VELOCITY; // static const auto motion_control_type = espp::detail::MotionControlType::ANGLE; @@ -117,22 +170,22 @@ extern "C" void app_main(void) { // Set the motion control type and create a target for the motor (will be // updated in the target update task below) - motor.set_motion_control_type(motion_control_type); + motor->set_motion_control_type(motion_control_type); std::atomic target = 0; // enable the motor - motor.enable(); + motor->enable(); auto motor_task_fn = [&motor, &target]() -> bool { if (motion_control_type == espp::detail::MotionControlType::VELOCITY || motion_control_type == espp::detail::MotionControlType::VELOCITY_OPENLOOP) { // if it's a velocity setpoint, convert it from RPM to rad/s - motor.move(target * espp::RPM_TO_RADS); + motor->move(target * espp::RPM_TO_RADS); } else { - motor.move(target); + motor->move(target); } // command the motor - motor.loop_foc(); + motor->loop_foc(); // don't want to stop the task return false; }; @@ -157,7 +210,7 @@ extern "C" void app_main(void) { break; case espp::detail::MotionControlType::ANGLE: case espp::detail::MotionControlType::ANGLE_OPENLOOP: - target = motor.get_shaft_angle(); + target = motor->get_shaft_angle(); break; default: break; @@ -212,9 +265,9 @@ extern "C" void app_main(void) { static auto start = std::chrono::high_resolution_clock::now(); auto now = std::chrono::high_resolution_clock::now(); auto seconds = std::chrono::duration(now - start).count(); - auto radians = motor.get_shaft_angle(); + auto radians = motor->get_shaft_angle(); auto degrees = radians * 180.0f / M_PI; - auto rpm = filter(motor.get_shaft_velocity() * espp::RADS_TO_RPM); + auto rpm = filter(motor->get_shaft_velocity() * espp::RADS_TO_RPM); auto _target = target.load(); fmt::print("{:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}\n", seconds, radians, degrees, _target, rpm); diff --git a/components/bldc_motor/idf_component.yml b/components/bldc_motor/idf_component.yml index 6ee9d2543..7c76df34e 100644 --- a/components/bldc_motor/idf_component.yml +++ b/components/bldc_motor/idf_component.yml @@ -18,6 +18,7 @@ dependencies: idf: version: '>=5.0' espp/base_component: '>=1.0' + espp/bldc_types: '>=1.0' espp/math: '>=1.0' espp/pid: '>=1.0' espp/task: '>=1.0' diff --git a/components/bldc_motor/include/bldc_motor.hpp b/components/bldc_motor/include/bldc_motor.hpp index 544d47442..99614b6c8 100644 --- a/components/bldc_motor/include/bldc_motor.hpp +++ b/components/bldc_motor/include/bldc_motor.hpp @@ -7,54 +7,13 @@ #include "fast_math.hpp" #include "pid.hpp" +#include "bldc_concepts.hpp" #include "bldc_types.hpp" #include "foc_utils.hpp" #include "sensor_direction.hpp" namespace espp { -/** - * @brief Concept defining the required interfaces for the Driver for a BLDC Motor. - */ -template -concept DriverConcept = requires { - static_cast(&FOO::enable); - static_cast(&FOO::disable); - static_cast(&FOO::set_voltage); - static_cast(&FOO::set_phase_state); - static_cast(&FOO::get_voltage_limit); -}; - -/** - * @brief Concept defining the required interfaces for a Sensor on a BLDC Motor. - */ -template -concept SensorConcept = requires { - static_cast(&FOO::update); - static_cast(&FOO::needs_zero_search); - static_cast(&FOO::get_radians); - static_cast(&FOO::get_rpm); - static_cast(&FOO::get_mechanical_radians); -}; - -/** - * @brief Concept defining the required interfaces for a Current Sensor on a BLDC Motor. - */ -template -concept CurrentSensorConcept = requires { - static_cast(&FOO::get_dc_current); - static_cast(&FOO::get_foc_currents); - static_cast(&FOO::driver_align); -}; - -// Provide a dummy current sense type here (as default) if you don't want to -// use (or your hardware doesn't support) an actual current sensor for FOC. -struct DummyCurrentSense { - float get_dc_current(float) const { return 0.0f; } - DqCurrent get_foc_currents(float) const { return {0.0f, 0.0f}; } - bool driver_align(float v) const { return true; } -}; - /** * @brief Motor control class for a Brushless DC (BLDC) motor, implementing * the field-oriented control (FOC) algorithm. Must be provided a @@ -513,8 +472,16 @@ class BldcMotor : public BaseComponent { /** * @brief Main FOC loop for implementing the torque control, based on the * configured detail::TorqueControlType. - * @note Only detail::TorqueControlType::VOLTAGE is supported right now, because the - * other types require current sense. + * @note detail::TorqueControlType::VOLTAGE requires no current sensing and is + * fully supported. detail::TorqueControlType::DC_CURRENT and + * detail::TorqueControlType::FOC_CURRENT close a current loop and + * therefore require a current sensor (CurrentSensorConcept, e.g. + * espp::CurrentSense from the bldc_current_sense component) to be + * provided; they are experimental and must be validated on hardware. + * @note For correct current-mode control this should be called at a steady, + * relatively high rate (several kHz), ideally synchronized to the + * current-sampling instant (PWM center). See BldcDriver's PWM sample + * callback. */ void loop_foc() { if (run_sensor_update_) { diff --git a/components/bldc_types/CMakeLists.txt b/components/bldc_types/CMakeLists.txt new file mode 100644 index 000000000..bd98715bb --- /dev/null +++ b/components/bldc_types/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES format + ) diff --git a/components/bldc_types/idf_component.yml b/components/bldc_types/idf_component.yml new file mode 100644 index 000000000..ba1b05233 --- /dev/null +++ b/components/bldc_types/idf_component.yml @@ -0,0 +1,19 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Shared types and concepts for BLDC motor control (espp)" +url: "https://github.com/esp-cpp/espp/tree/main/components/bldc_types" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/bldc/bldc_types.html" +tags: + - cpp + - Component + - BLDC + - Motor + - Control + - FOC +dependencies: + idf: + version: '>=5.0' + espp/format: '>=1.0' diff --git a/components/bldc_types/include/bldc_concepts.hpp b/components/bldc_types/include/bldc_concepts.hpp new file mode 100644 index 000000000..0afad61f9 --- /dev/null +++ b/components/bldc_types/include/bldc_concepts.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include + +#include "bldc_types.hpp" +#include "foc_utils.hpp" + +namespace espp { + +/** + * @brief Concept defining the required interfaces for the Driver for a BLDC Motor. + */ +template +concept DriverConcept = requires { + static_cast(&FOO::enable); + static_cast(&FOO::disable); + static_cast(&FOO::set_voltage); + static_cast(&FOO::set_phase_state); + static_cast(&FOO::get_voltage_limit); +}; + +/** + * @brief Concept defining the required interfaces for a Sensor on a BLDC Motor. + */ +template +concept SensorConcept = requires { + static_cast(&FOO::update); + static_cast(&FOO::needs_zero_search); + static_cast(&FOO::get_radians); + static_cast(&FOO::get_rpm); + static_cast(&FOO::get_mechanical_radians); +}; + +/** + * @brief Concept defining the required interfaces for a Current Sensor on a BLDC Motor. + */ +// NOTE: driver_align() is intentionally non-const: a real current sensor must +// capture zero-current offsets and phase/gain calibration while aligning, which +// mutates the sensor's state. The reading methods remain const. +template +concept CurrentSensorConcept = requires { + static_cast(&FOO::get_dc_current); + static_cast(&FOO::get_foc_currents); + static_cast(&FOO::driver_align); +}; + +/** + * @brief Concept defining the required interfaces for a Motor used for haptics. + */ +template +concept MotorConcept = requires { + static_cast(&FOO::enable); ///< Enable the motor + static_cast(&FOO::disable); ///< Disable the motor + static_cast( + &FOO::move); ///< Move the motor to a new target (position, velocity, or torque depending on + ///< the motor control type) + static_cast( + &FOO::set_motion_control_type); ///< Set the motion control type + static_cast(&FOO::loop_foc); ///< Run the FOC loop + static_cast(&FOO::get_shaft_angle); ///< Get the shaft angle + static_cast(&FOO::get_shaft_velocity); ///< Get the shaft velocity + static_cast(&FOO::get_electrical_angle); ///< Get the electrical angle +}; + +// Provide a dummy current sense type here (as default) if you don't want to +// use (or your hardware doesn't support) an actual current sensor for FOC. +struct DummyCurrentSense { + float get_dc_current(float) const { return 0.0f; } + DqCurrent get_foc_currents(float) const { return {0.0f, 0.0f}; } + bool driver_align(float v) { return true; } +}; + +} // namespace espp diff --git a/components/bldc_motor/include/bldc_types.hpp b/components/bldc_types/include/bldc_types.hpp similarity index 79% rename from components/bldc_motor/include/bldc_types.hpp rename to components/bldc_types/include/bldc_types.hpp index 56832cc37..395240185 100644 --- a/components/bldc_motor/include/bldc_types.hpp +++ b/components/bldc_types/include/bldc_types.hpp @@ -19,12 +19,20 @@ enum class MotionControlType { }; /// \brief How the torque is controlled. -/// \note VOLTAGE is the only one supported right now, since the other two -/// require current sense. +/// \details VOLTAGE control requires no current sensing and is fully supported: +/// the torque command is converted to a q-axis voltage (using the phase +/// resistance and estimated back-EMF if those are provided). DC_CURRENT and +/// FOC_CURRENT close a current loop and therefore require a current sensor +/// implementing the CurrentSensorConcept (for example espp::CurrentSense +/// from the bldc_current_sense component) to be supplied to the BldcMotor. +/// \note DC_CURRENT and FOC_CURRENT are experimental: they depend on accurate, +/// PWM-synchronized current sampling and per-board calibration, so they +/// must be validated on hardware before use. If no current sensor is +/// provided, only VOLTAGE control is functional. enum class TorqueControlType { - VOLTAGE, //!< Torque control using voltage - DC_CURRENT, //!< Torque control using DC current (one current magnitude) - FOC_CURRENT //!< Torque control using DQ currents + VOLTAGE, //!< Torque control using voltage (no current sense required) + DC_CURRENT, //!< Torque control using DC current (one current magnitude; needs current sense) + FOC_CURRENT //!< Torque control using DQ currents (needs current sense) }; /// \brief How the voltages / pwms are calculated based on the magnitude and diff --git a/components/bldc_motor/include/foc_utils.hpp b/components/bldc_types/include/foc_utils.hpp similarity index 100% rename from components/bldc_motor/include/foc_utils.hpp rename to components/bldc_types/include/foc_utils.hpp diff --git a/components/bldc_motor/include/sensor_direction.hpp b/components/bldc_types/include/sensor_direction.hpp similarity index 100% rename from components/bldc_motor/include/sensor_direction.hpp rename to components/bldc_types/include/sensor_direction.hpp diff --git a/components/motorgo-axis/CMakeLists.txt b/components/motorgo-axis/CMakeLists.txt index 72c39bcec..b1cd903a8 100755 --- a/components/motorgo-axis/CMakeLists.txt +++ b/components/motorgo-axis/CMakeLists.txt @@ -1,6 +1,6 @@ idf_component_register( INCLUDE_DIRS "include" SRC_DIRS "src" - REQUIRES base_component bldc_driver esp_driver_gpio i2c led lsm6dso math mt6701 spi task + REQUIRES base_component bldc_driver bldc_motor esp_driver_gpio i2c led lsm6dso math mt6701 pid spi task REQUIRED_IDF_TARGETS "esp32s3" ) diff --git a/components/motorgo-axis/README.md b/components/motorgo-axis/README.md index d5c3a6318..04ec359b3 100755 --- a/components/motorgo-axis/README.md +++ b/components/motorgo-axis/README.md @@ -23,6 +23,47 @@ those documented pin mappings, plus convenient helpers for: breathing effect - accessing the external Qwiic and internal I2C buses +## Symmetric API (shared with MotorGo Mini) + +`espp::MotorGoAxis` and `espp::MotorGoMini` expose the same zero-based, +index-based API so the same code can drive either board. All of the +`MotorGoAxis` methods listed below also exist on `MotorGoMini` (which +additionally keeps its original 1-based named accessors for backwards +compatibility). + +The shared API (motor channels are zero-based: `0` == "Motor 1", `1` == "Motor 2"): + +- Types: `Encoder`, `MotorDriver`, `BldcMotor` +- `num_motor_channels()`, `driver_default_power_supply_voltage()`, + `driver_default_voltage_limit()`, `default_motor_dead_zone_ns()` +- `initialize_encoders(run_tasks = true)`, `initialize_motors(...)` +- `encoder(index)`, `motor_driver(index)`, `motor(index)` +- `reset_encoder_accumulator(index)` +- `motor_driver_enabled(index)`, `enable_motor_driver(index)`, + `disable_motor_driver(index)`, `enable_all_motor_drivers()`, + `disable_all_motor_drivers()` +- `default_motor_config(index)`, `initialize_motor(index, config)` + +```cpp +// Works unchanged on MotorGo Axis or MotorGo Mini: +using Board = espp::MotorGoAxis; // or espp::MotorGoMini +auto &board = Board::get(); +board.initialize_encoders(); // start the encoder update task(s) +board.initialize_motors(); // create the motor driver(s) +size_t index = 0; // 0 == Motor 1, 1 == Motor 2 +auto config = board.default_motor_config(index); +// tweak config (PID gains, current limit, ...) here if desired +auto motor = board.initialize_motor(index, config); // shared_ptr +auto driver = board.motor_driver(index); +``` + +`initialize_motor()` creates (and calibrates) the FOC motor controller for the +channel, initializing that channel's encoder and driver first if they have not +been initialized yet, so a single call is enough to get a ready-to-use motor. +This is new on MotorGo Axis: the board now owns the `BldcMotor` objects, in +addition to the per-channel `motor_driver(index)` / `encoder(index)` helpers it +already exposed. + ## Example The [example](./example) initializes the board, logs the MotorGo Axis pin map, diff --git a/components/motorgo-axis/idf_component.yml b/components/motorgo-axis/idf_component.yml index f5f5cf3ea..d0728b7d3 100755 --- a/components/motorgo-axis/idf_component.yml +++ b/components/motorgo-axis/idf_component.yml @@ -23,6 +23,8 @@ dependencies: version: '>=5.0' espp/base_component: '>=1.0' espp/bldc_driver: '>=1.0' + espp/bldc_motor: '>=1.0' + espp/pid: '>=1.0' espp/i2c: '>=1.0' espp/led: '>=1.0' espp/lsm6dso: '>=1.0' diff --git a/components/motorgo-axis/include/motorgo-axis.hpp b/components/motorgo-axis/include/motorgo-axis.hpp index 0f08a7317..3ee4a49f5 100644 --- a/components/motorgo-axis/include/motorgo-axis.hpp +++ b/components/motorgo-axis/include/motorgo-axis.hpp @@ -12,6 +12,7 @@ #include "base_component.hpp" #include "bldc_driver.hpp" +#include "bldc_motor.hpp" #include "gaussian.hpp" #include "i2c.hpp" #include "led.hpp" @@ -42,6 +43,10 @@ class MotorGoAxis : public BaseComponent { using Encoder = espp::Mt6701; /// Alias for the 6-PWM BLDC motor driver helper used on each motor channel. using MotorDriver = espp::BldcDriver; + /// Alias for the FOC BLDC motor controller used on each motor channel. + /// \note Shares the same type as espp::MotorGoMini::BldcMotor so that code can + /// be written generically against either board. + using BldcMotor = espp::BldcMotor; /// Alias for the onboard hidden-bus IMU helper. using Imu = espp::Lsm6dso; @@ -177,6 +182,33 @@ class MotorGoAxis : public BaseComponent { /// Disable all initialized motor driver helpers. void disable_all_motor_drivers(); + /// Get a default FOC motor configuration for one channel. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return A BldcMotor::Config pre-populated with sensible defaults for the + /// board's gimbal motors. The driver and sensor fields are filled in + /// by initialize_motor(), so they may be left as-is. + /// \note Provided for API symmetry with espp::MotorGoMini so the same code can + /// drive either board. + BldcMotor::Config default_motor_config(size_t index) const; + + /// Create the FOC motor controller for one channel. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \param config The motor configuration. Its driver and sensor fields are + /// overridden with the board's per-channel driver and encoder. + /// \return Shared pointer to the created (and initialized) BldcMotor, or + /// `nullptr` if the index is invalid or the channel's driver/encoder + /// could not be initialized. + /// \details If the encoders / motor drivers have not been initialized yet, + /// this initializes them first (with default settings), so a single + /// call is enough to get a ready-to-use motor. + std::shared_ptr initialize_motor(size_t index, const BldcMotor::Config &config); + + /// Get one FOC motor controller. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return Shared pointer to the requested motor, or `nullptr` if the index is + /// invalid or initialize_motor() has not been called for it. + std::shared_ptr motor(size_t index); + /// Initialize the shared encoder bus and create the two MT6701 SSI helpers. /// \param run_tasks If true, each encoder starts its own update task after /// initialization. If false, callers must invoke @@ -371,6 +403,7 @@ class MotorGoAxis : public BaseComponent { }}; std::array, 2> motor_drivers_{}; + std::array, 2> motors_{}; std::shared_ptr indicator_leds_; std::unique_ptr led_task_; std::atomic breathing_period_{3.5f}; diff --git a/components/motorgo-axis/src/motorgo-axis.cpp b/components/motorgo-axis/src/motorgo-axis.cpp index dcefc6cfb..26da999d0 100755 --- a/components/motorgo-axis/src/motorgo-axis.cpp +++ b/components/motorgo-axis/src/motorgo-axis.cpp @@ -174,6 +174,84 @@ void MotorGoAxis::reset_encoder_accumulator(size_t index) { encoder_ptr->reset_accumulator(); } +MotorGoAxis::BldcMotor::Config MotorGoAxis::default_motor_config(size_t index) const { + if (index >= num_motor_channels()) { + logger_.error("Invalid motor index: {}", index); + } + // Sensible defaults for the board's gimbal motors. The driver and sensor are + // filled in by initialize_motor(). These mirror the espp::MotorGoMini default + // motor configuration so the same code drives either board. + return BldcMotor::Config{ + .num_pole_pairs = 7, + .phase_resistance = 4.0f, + .kv_rating = 320, + .current_limit = 1.0f, + .foc_type = espp::detail::FocType::SPACE_VECTOR_PWM, + .driver = nullptr, // filled in by initialize_motor() + .sensor = nullptr, // filled in by initialize_motor() + .run_sensor_update = false, // the board runs the encoder update task + .velocity_pid_config = + { + .kp = 0.010f, + .ki = 0.100f, + .kd = 0.000f, + .integrator_min = -1.0f, + .integrator_max = 1.0f, + .output_min = -1.0, + .output_max = 1.0, + }, + .angle_pid_config = + { + .kp = 5.000f, + .ki = 1.000f, + .kd = 0.000f, + .integrator_min = -10.0f, + .integrator_max = 10.0f, + .output_min = -20.0, + .output_max = 20.0, + }, + .log_level = get_log_level(), + }; +} + +std::shared_ptr +MotorGoAxis::initialize_motor(size_t index, const MotorGoAxis::BldcMotor::Config &config) { + if (index >= num_motor_channels()) { + logger_.error("Invalid motor index: {}", index); + return nullptr; + } + // make sure the encoder and driver for this channel exist; initialize the + // board defaults if the user has not done so already. + if (!encoder(index)) { + initialize_encoders(); + } + if (!motor_driver(index)) { + initialize_motors(); + } + auto enc = encoder(index); + auto drv = motor_driver(index); + if (!enc || !drv) { + logger_.error("Could not initialize driver / encoder for motor {}", index + 1); + return nullptr; + } + // the board owns the driver + encoder (and the encoder runs its own update + // task), so override those fields regardless of what the caller passed. + auto motor_config = config; + motor_config.driver = drv; + motor_config.sensor = enc; + motor_config.run_sensor_update = false; + motors_[index] = std::make_shared(motor_config); + return motors_[index]; +} + +std::shared_ptr MotorGoAxis::motor(size_t index) { + if (index >= motors_.size()) { + logger_.error("Invalid motor index: {}", index); + return nullptr; + } + return motors_[index]; +} + bool MotorGoAxis::initialize_imu(const Imu::filter_fn &orientation_filter, const Imu::ImuConfig &imu_config) { if (imu_) { diff --git a/components/motorgo-mini/README.md b/components/motorgo-mini/README.md index ace19e75f..892f6d7b7 100644 --- a/components/motorgo-mini/README.md +++ b/components/motorgo-mini/README.md @@ -11,6 +11,43 @@ It's pretty sweet and the component provides the implementation of the two channel FOC motor controller, along with other peripheral classes such as the ADC, LEDs, and I2C. +## Symmetric API (shared with MotorGo Axis) + +`espp::MotorGoMini` and `espp::MotorGoAxis` expose the same zero-based, +index-based API so the same code can drive either board. The original 1-based +named accessors (`motor1()`, `init_motor_channel_1()`, `encoder1()`, +`default_motor1_config`, …) remain available for backwards compatibility. + +The shared API (motor channels are zero-based: `0` == "Motor 1", `1` == "Motor 2"): + +- Types: `Encoder`, `MotorDriver`, `BldcMotor` +- `num_motor_channels()`, `driver_default_power_supply_voltage()`, + `driver_default_voltage_limit()`, `default_motor_dead_zone_ns()` +- `initialize_encoders(run_tasks = true)`, `initialize_motors(...)` +- `encoder(index)`, `motor_driver(index)`, `motor(index)` +- `reset_encoder_accumulator(index)` +- `motor_driver_enabled(index)`, `enable_motor_driver(index)`, + `disable_motor_driver(index)`, `enable_all_motor_drivers()`, + `disable_all_motor_drivers()` +- `default_motor_config(index)`, `initialize_motor(index, config)` + +```cpp +// Works unchanged on MotorGo Mini or MotorGo Axis: +using Board = espp::MotorGoMini; // or espp::MotorGoAxis +auto &board = Board::get(); +board.initialize_encoders(); // start the encoder update task(s) +board.initialize_motors(); // create the motor driver(s) +size_t index = 0; // 0 == Motor 1, 1 == Motor 2 +auto config = board.default_motor_config(index); +// tweak config (PID gains, current limit, ...) here if desired +auto motor = board.initialize_motor(index, config); // shared_ptr +auto driver = board.motor_driver(index); +``` + +`initialize_motor()` creates (and calibrates) the FOC motor controller for the +channel, initializing that channel's encoder and driver first if they have not +been initialized yet, so a single call is enough to get a ready-to-use motor. + ## Example This example demonstrates how to use the `espp::MotorGoMini` component to diff --git a/components/motorgo-mini/example/main/motorgo_mini_example.cpp b/components/motorgo-mini/example/main/motorgo_mini_example.cpp index 06568f8ff..834e2ea7e 100644 --- a/components/motorgo-mini/example/main/motorgo_mini_example.cpp +++ b/components/motorgo-mini/example/main/motorgo_mini_example.cpp @@ -23,8 +23,8 @@ extern "C" void app_main(void) { motor1_config.phase_resistance = 4.0f; // ohms motor1_config.current_limit = 1.0f; // amps // velocity PID config: - motor1_config.velocity_pid_config.kp = 0.020f; - motor1_config.velocity_pid_config.ki = 0.700f; + motor1_config.velocity_pid_config.kp = 0.010f; + motor1_config.velocity_pid_config.ki = 0.100f; motor1_config.velocity_pid_config.kd = 0.000f; // angle PID config: motor1_config.angle_pid_config.kp = 5.000f; @@ -35,8 +35,8 @@ extern "C" void app_main(void) { motor2_config.phase_resistance = 4.0f; // ohms motor2_config.current_limit = 1.0f; // amps // velocity PID config: - motor2_config.velocity_pid_config.kp = 0.020f; - motor2_config.velocity_pid_config.ki = 0.700f; + motor2_config.velocity_pid_config.kp = 0.010f; + motor2_config.velocity_pid_config.ki = 0.100f; motor2_config.velocity_pid_config.kd = 0.000f; // angle PID config: motor2_config.angle_pid_config.kp = 5.000f; diff --git a/components/motorgo-mini/include/motorgo-mini.hpp b/components/motorgo-mini/include/motorgo-mini.hpp index be0975b68..f8b769e5f 100644 --- a/components/motorgo-mini/include/motorgo-mini.hpp +++ b/components/motorgo-mini/include/motorgo-mini.hpp @@ -50,6 +50,10 @@ class MotorGoMini : public BaseComponent { /// Alias for the BLDC motor type using BldcMotor = espp::BldcMotor; + /// Alias for the 6-PWM BLDC motor driver helper used on each motor channel. + /// \note Provided for API symmetry with espp::MotorGoAxis. + using MotorDriver = espp::BldcDriver; + /// Alias for the velocity filter type using VelocityFilter = espp::SimpleLowpassFilter; @@ -170,6 +174,19 @@ class MotorGoMini : public BaseComponent { float limit_voltage; ///< The limit voltage in volts }; + /// Get the number of supported motor channels. + /// \return The number of motor channels exposed by the board. + static constexpr size_t num_motor_channels() { return 2; } + /// Get the default motor supply voltage used for the board definition. + /// \return Default power supply voltage in volts. + static constexpr float driver_default_power_supply_voltage() { return 5.0f; } + /// Get the default motor voltage limit used for the board definition. + /// \return Default motor voltage limit in volts. + static constexpr float driver_default_voltage_limit() { return 5.0f; } + /// Get the default BLDC driver dead-time. + /// \return Default dead-time in nanoseconds. + static constexpr uint64_t default_motor_dead_zone_ns() { return 100; } + /// Default configuration for the MotorGo-Mini's BLDC motor on channel 1 const BldcMotor::Config default_motor1_config{ .num_pole_pairs = 7, @@ -181,8 +198,8 @@ class MotorGoMini : public BaseComponent { .sensor = encoder1_, // NOTE: user cannot override this .velocity_pid_config = { - .kp = 0.020f, - .ki = 0.700f, + .kp = 0.010f, + .ki = 0.100f, .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) @@ -214,8 +231,8 @@ class MotorGoMini : public BaseComponent { .sensor = encoder2_, // NOTE: user cannot override this .velocity_pid_config = { - .kp = 0.020f, - .ki = 0.700f, + .kp = 0.010f, + .ki = 0.100f, .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) @@ -312,6 +329,91 @@ class MotorGoMini : public BaseComponent { /// of 0 to 2*pi. void reset_encoder2_accumulator(); + ///////////////////////////////////////////////////////////////////////////// + // Symmetric (index-based) API + // + // The methods below mirror the espp::MotorGoAxis API so the same code can + // drive either board. Motor channels are zero-based here (index 0 == "Motor + // 1", index 1 == "Motor 2"), unlike the 1-based named accessors above which + // remain available for backwards compatibility. + ///////////////////////////////////////////////////////////////////////////// + + /// Initialize the magnetic encoders for both motor channels. + /// \param run_tasks If true, each encoder starts its own update task. + /// \return True if both encoders were initialized successfully. + /// \details Encoders that are already initialized are left untouched. + bool initialize_encoders(bool run_tasks = true); + + /// Initialize the BLDC motor drivers for both motor channels. + /// \param power_supply_voltage Supply voltage (V) provided to the drivers. + /// \param limit_voltage Maximum voltage (V) allowed to the motors. + /// \param dead_zone_ns Dead-time applied to both sides of each phase PWM. + /// \return True if both drivers were initialized successfully. + /// \details Drivers that are already initialized are left untouched. + bool initialize_motors(float power_supply_voltage = driver_default_power_supply_voltage(), + float limit_voltage = driver_default_voltage_limit(), + uint64_t dead_zone_ns = default_motor_dead_zone_ns()); + + /// Get one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return Shared pointer to the requested driver, or `nullptr` if the index + /// is invalid or that driver has not been initialized. + std::shared_ptr motor_driver(size_t index); + + /// Get one encoder helper. + /// \param index Zero-based encoder index in the range [0, num_motor_channels()). + /// \return Shared pointer to the requested encoder, or `nullptr` if the index + /// is invalid or that encoder has not been initialized. + std::shared_ptr encoder(size_t index); + + /// Reset one encoder's accumulator. + /// \param index Zero-based encoder index in the range [0, num_motor_channels()). + void reset_encoder_accumulator(size_t index); + + /// Check whether one motor driver helper is currently enabled. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return True if the requested driver exists and is enabled. + bool motor_driver_enabled(size_t index); + + /// Enable one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return True if the driver exists and was enabled. + bool enable_motor_driver(size_t index); + + /// Disable one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + void disable_motor_driver(size_t index); + + /// Enable all initialized motor driver helpers. + void enable_all_motor_drivers(); + + /// Disable all initialized motor driver helpers. + void disable_all_motor_drivers(); + + /// Get a default FOC motor configuration for one channel. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return A BldcMotor::Config pre-populated with the board defaults for that + /// channel (including the per-channel velocity / angle filters). + BldcMotor::Config default_motor_config(size_t index) const; + + /// Create the FOC motor controller for one channel. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \param config The motor configuration. Its driver and sensor fields are + /// overridden with the board's per-channel driver and encoder. + /// \return Shared pointer to the created (and initialized) BldcMotor, or + /// `nullptr` if the index is invalid or the channel's driver/encoder + /// could not be initialized. + /// \details If the encoder / driver for the channel have not been initialized + /// yet, this initializes them first, so a single call is enough to + /// get a ready-to-use motor. + std::shared_ptr initialize_motor(size_t index, const BldcMotor::Config &config); + + /// Get one FOC motor controller. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return Shared pointer to the requested motor, or `nullptr` if the index is + /// invalid or the motor has not been initialized. + std::shared_ptr motor(size_t index); + ///////////////////////////////////////////////////////////////////////////// // Motor Current Sense ///////////////////////////////////////////////////////////////////////////// @@ -372,6 +474,12 @@ class MotorGoMini : public BaseComponent { void always_init(); void init_spi(); + // Index-based helpers for the symmetric API. They create the per-channel + // encoder / driver only if it has not been created yet. + bool init_encoder(size_t index, bool run_tasks); + bool init_driver(size_t index, float power_supply_voltage, float limit_voltage, + uint64_t dead_zone_ns); + float breathe(float breathing_period, uint64_t start_us, bool restart = false); bool read_encoder(const std::shared_ptr &encoder_device, uint8_t *data, size_t size); diff --git a/components/motorgo-mini/src/motorgo-mini.cpp b/components/motorgo-mini/src/motorgo-mini.cpp index 905b39a35..9853f2bf0 100644 --- a/components/motorgo-mini/src/motorgo-mini.cpp +++ b/components/motorgo-mini/src/motorgo-mini.cpp @@ -144,6 +144,193 @@ float MotorGoMini::motor2_current_w_amps() { return adc_1.read_mv(current_sense_m2_w_).value() * CURRENT_SENSE_MV_TO_A; } +///////////////////////////////////////////////////////////////////////////// +// Symmetric (index-based) API +///////////////////////////////////////////////////////////////////////////// + +bool MotorGoMini::init_encoder(size_t index, bool run_tasks) { + std::error_code ec; + if (index == 0) { + if (!encoder1_) { + encoder1_ = std::make_shared(encoder1_config_); + encoder1_->initialize(run_tasks, ec); + } + } else if (index == 1) { + if (!encoder2_) { + encoder2_ = std::make_shared(encoder2_config_); + encoder2_->initialize(run_tasks, ec); + } + } else { + logger_.error("Invalid encoder index: {}", index); + return false; + } + if (ec) { + logger_.error("Could not initialize encoder {}: {}", index + 1, ec.message()); + return false; + } + return true; +} + +bool MotorGoMini::init_driver(size_t index, float power_supply_voltage, float limit_voltage, + uint64_t dead_zone_ns) { + if (index == 0) { + if (!motor1_driver_) { + motor1_driver_config_.power_supply_voltage = power_supply_voltage; + motor1_driver_config_.limit_voltage = limit_voltage; + motor1_driver_config_.dead_zone_ns = dead_zone_ns; + motor1_driver_ = std::make_shared(motor1_driver_config_); + } + } else if (index == 1) { + if (!motor2_driver_) { + motor2_driver_config_.power_supply_voltage = power_supply_voltage; + motor2_driver_config_.limit_voltage = limit_voltage; + motor2_driver_config_.dead_zone_ns = dead_zone_ns; + motor2_driver_ = std::make_shared(motor2_driver_config_); + } + } else { + logger_.error("Invalid motor index: {}", index); + return false; + } + return true; +} + +bool MotorGoMini::initialize_encoders(bool run_tasks) { + bool ok = true; + ok &= init_encoder(0, run_tasks); + ok &= init_encoder(1, run_tasks); + return ok; +} + +bool MotorGoMini::initialize_motors(float power_supply_voltage, float limit_voltage, + uint64_t dead_zone_ns) { + bool ok = true; + ok &= init_driver(0, power_supply_voltage, limit_voltage, dead_zone_ns); + ok &= init_driver(1, power_supply_voltage, limit_voltage, dead_zone_ns); + return ok; +} + +std::shared_ptr MotorGoMini::motor_driver(size_t index) { + if (index == 0) + return motor1_driver_; + if (index == 1) + return motor2_driver_; + logger_.error("Invalid motor index: {}", index); + return nullptr; +} + +std::shared_ptr MotorGoMini::encoder(size_t index) { + if (index == 0) + return encoder1_; + if (index == 1) + return encoder2_; + logger_.error("Invalid encoder index: {}", index); + return nullptr; +} + +void MotorGoMini::reset_encoder_accumulator(size_t index) { + auto enc = encoder(index); + if (!enc) { + logger_.error("Encoder {} not initialized", index + 1); + return; + } + enc->reset_accumulator(); +} + +bool MotorGoMini::motor_driver_enabled(size_t index) { + auto drv = motor_driver(index); + return drv && drv->is_enabled(); +} + +bool MotorGoMini::enable_motor_driver(size_t index) { + auto drv = motor_driver(index); + if (!drv) { + logger_.error("Motor driver {} not initialized", index + 1); + return false; + } + drv->enable(); + return drv->is_enabled(); +} + +void MotorGoMini::disable_motor_driver(size_t index) { + auto drv = motor_driver(index); + if (!drv) { + logger_.error("Motor driver {} not initialized", index + 1); + return; + } + drv->disable(); +} + +void MotorGoMini::enable_all_motor_drivers() { + for (size_t i = 0; i < num_motor_channels(); i++) { + auto drv = motor_driver(i); + if (drv) { + drv->enable(); + } + } +} + +void MotorGoMini::disable_all_motor_drivers() { + for (size_t i = 0; i < num_motor_channels(); i++) { + auto drv = motor_driver(i); + if (drv) { + drv->disable(); + } + } +} + +MotorGoMini::BldcMotor::Config MotorGoMini::default_motor_config(size_t index) const { + if (index == 0) + return default_motor1_config; + if (index == 1) + return default_motor2_config; + logger_.error("Invalid motor index: {}", index); + return default_motor1_config; +} + +std::shared_ptr +MotorGoMini::initialize_motor(size_t index, const MotorGoMini::BldcMotor::Config &config) { + if (index >= num_motor_channels()) { + logger_.error("Invalid motor index: {}", index); + return nullptr; + } + // make sure the encoder and driver for this channel exist; initialize the + // board defaults if the user has not done so already. + if (!encoder(index)) { + init_encoder(index, true); + } + if (!motor_driver(index)) { + init_driver(index, driver_default_power_supply_voltage(), driver_default_voltage_limit(), + default_motor_dead_zone_ns()); + } + auto enc = encoder(index); + auto drv = motor_driver(index); + if (!enc || !drv) { + logger_.error("Could not initialize driver / encoder for motor {}", index + 1); + return nullptr; + } + // the board owns the driver + encoder, so override those fields regardless of + // what the caller passed. + auto motor_config = config; + motor_config.driver = drv; + motor_config.sensor = enc; + auto m = std::make_shared(motor_config); + if (index == 0) { + motor1_ = m; + } else { + motor2_ = m; + } + return m; +} + +std::shared_ptr MotorGoMini::motor(size_t index) { + if (index == 0) + return motor1_; + if (index == 1) + return motor2_; + logger_.error("Invalid motor index: {}", index); + return nullptr; +} + void MotorGoMini::always_init() { start_breathing(); init_spi(); diff --git a/doc/Doxyfile b/doc/Doxyfile index 88295eee1..a7e4a809f 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -84,6 +84,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/lp5817/example/main/lp5817_example.cpp \ $(PROJECT_PATH)/components/binary-log/example/main/binary_log_example.cpp \ $(PROJECT_PATH)/components/ble_gatt_server/example/main/ble_gatt_server_example.cpp \ + $(PROJECT_PATH)/components/bldc_current_sense/example/main/bldc_current_sense_example.cpp \ $(PROJECT_PATH)/components/bldc_motor/example/main/bldc_motor_example.cpp \ $(PROJECT_PATH)/components/bldc_haptics/example/main/bldc_haptics_example.cpp \ $(PROJECT_PATH)/components/bm8563/example/main/bm8563_example.cpp \ @@ -209,13 +210,16 @@ INPUT = \ $(PROJECT_PATH)/components/ble_gatt_server/include/ble_gatt_server_menu.hpp \ $(PROJECT_PATH)/components/ble_gatt_server/include/device_info_service.hpp \ $(PROJECT_PATH)/components/ble_gatt_server/include/generic_access_service.hpp \ + $(PROJECT_PATH)/components/bldc_current_sense/include/current_sense.hpp \ $(PROJECT_PATH)/components/bldc_driver/include/bldc_driver.hpp \ $(PROJECT_PATH)/components/bldc_haptics/include/bldc_haptics.hpp \ $(PROJECT_PATH)/components/bldc_haptics/include/detent_config.hpp \ $(PROJECT_PATH)/components/bldc_haptics/include/haptic_config.hpp \ $(PROJECT_PATH)/components/bldc_motor/include/bldc_motor.hpp \ - $(PROJECT_PATH)/components/bldc_motor/include/bldc_types.hpp \ - $(PROJECT_PATH)/components/bldc_motor/include/sensor_direction.hpp \ + $(PROJECT_PATH)/components/bldc_types/include/bldc_types.hpp \ + $(PROJECT_PATH)/components/bldc_types/include/foc_utils.hpp \ + $(PROJECT_PATH)/components/bldc_types/include/bldc_concepts.hpp \ + $(PROJECT_PATH)/components/bldc_types/include/sensor_direction.hpp \ $(PROJECT_PATH)/components/bm8563/include/bm8563.hpp \ $(PROJECT_PATH)/components/bmi270/include/bmi270.hpp \ $(PROJECT_PATH)/components/bmi270/include/bmi270_detail.hpp \ diff --git a/doc/en/bldc/bldc_current_sense.rst b/doc/en/bldc/bldc_current_sense.rst new file mode 100644 index 000000000..4465f87fa --- /dev/null +++ b/doc/en/bldc/bldc_current_sense.rst @@ -0,0 +1,43 @@ +BLDC Current Sense +****************** + +The `CurrentSense` class provides a field-oriented control (FOC) current sensor +for BLDC motors. It implements the `CurrentSensorConcept` used by `BldcMotor`, so +it can be supplied as a motor's current sense to enable current-feedback torque +control (`TorqueControlType::DC_CURRENT` and `TorqueControlType::FOC_CURRENT`). + +`CurrentSense` is decoupled from any specific ADC: the user provides a +`read_phase_currents` callback that returns the most recently sampled phase +currents (in amps). The class subtracts the per-phase zero-current offset +captured during `driver_align()`, applies optional per-phase gain-sign +correction, reconstructs an unmeasured phase (return ``NAN`` for it) assuming +``Ia + Ib + Ic = 0``, and runs the Clarke + Park transforms to produce the d/q +currents (`get_foc_currents()`) and the signed DC current magnitude +(`get_dc_current()`). + +For accurate low-side current sensing the samples must be taken while the +low-side FETs conduct (the PWM center). Use +`BldcDriver::register_pwm_sample_callback()` to trigger the ADC read at the +timer "empty" (TEZ) event and have the `read_phase_currents` callback return the +latest result. + +.. note:: + + This component is **experimental**. Zero-current offset calibration is + implemented, but automatic phase-ordering / gain-sign discovery is not (set + ``phase_gain_signs`` in the ``Config`` if a sense channel is inverted). + Current-mode torque control depends on accurate, PWM-synchronized sampling and + per-board tuning and must be validated on hardware. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + bldc_current_sense_example + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/current_sense.inc diff --git a/doc/en/bldc/bldc_current_sense_example.md b/doc/en/bldc/bldc_current_sense_example.md new file mode 100644 index 000000000..2579d286a --- /dev/null +++ b/doc/en/bldc/bldc_current_sense_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/bldc_current_sense/example/README.md +``` diff --git a/doc/en/bldc/bldc_motor.rst b/doc/en/bldc/bldc_motor.rst index 2947159ef..dab6297b5 100644 --- a/doc/en/bldc/bldc_motor.rst +++ b/doc/en/bldc/bldc_motor.rst @@ -11,9 +11,15 @@ changed dynamically): * Open-loop angle * Open-loop velocity -Note: currently the code has some support for Torque control, but that requires -current sense - for which I don't yet have the hardware to support the -development of. +Torque control is supported in two forms. Voltage-mode torque +(``TorqueControlType::VOLTAGE``) requires no current sensing and is fully +supported. Current-feedback torque (``TorqueControlType::DC_CURRENT`` and +``TorqueControlType::FOC_CURRENT``) closes a current loop and therefore requires +a current sensor implementing the ``CurrentSensorConcept`` (for example +``espp::CurrentSense`` from the ``bldc_current_sense`` component). The +current-feedback modes are experimental: they depend on accurate, +PWM-synchronized current sampling and per-board calibration, and must be +validated on hardware. The `BldcMotor` should be configured with a `BldcDriver` and optional `Sensor` (for angle & speed of the motor), and optional `CurrentSensor` (for measuring @@ -31,5 +37,3 @@ API Reference ------------- .. include-build-file:: inc/bldc_motor.inc -.. include-build-file:: inc/bldc_types.inc -.. include-build-file:: inc/sensor_direction.inc diff --git a/doc/en/bldc/bldc_types.rst b/doc/en/bldc/bldc_types.rst new file mode 100644 index 000000000..43e82c7b8 --- /dev/null +++ b/doc/en/bldc/bldc_types.rst @@ -0,0 +1,28 @@ +BLDC Types & Concepts +********************* + +The `bldc_types` component holds the shared building blocks used across the BLDC +motor components (`bldc_motor`, `bldc_haptics`, and `bldc_current_sense`): + +- the FOC value types (``DqCurrent``, ``DqVoltage``, ``PhaseCurrent``) and the + FOC math helpers / constants in ``foc_utils.hpp``, +- the control enumerations (``MotionControlType``, ``TorqueControlType``, + ``FocType``) in ``bldc_types.hpp`` and the ``SensorDirection`` enum, +- the interface concepts in ``bldc_concepts.hpp`` (``DriverConcept``, + ``SensorConcept``, ``CurrentSensorConcept``, and ``MotorConcept``) plus the + ``DummyCurrentSense`` default. + +Keeping these in a small, dependency-light component lets the other BLDC +components share the same interface contracts without depending on each other +(for example, ``bldc_current_sense`` implements ``CurrentSensorConcept`` without +pulling in ``bldc_motor``). + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/bldc_types.inc +.. include-build-file:: inc/foc_utils.inc +.. include-build-file:: inc/bldc_concepts.inc +.. include-build-file:: inc/sensor_direction.inc diff --git a/doc/en/bldc/index.rst b/doc/en/bldc/index.rst index ba8cf458d..36d60b2bb 100644 --- a/doc/en/bldc/index.rst +++ b/doc/en/bldc/index.rst @@ -5,8 +5,10 @@ Motor (BDC + BLDC) APIs :maxdepth: 1 bdc_driver + bldc_types bldc_driver bldc_motor + bldc_current_sense These components provide interfaces by which the user can control brushed DC (BDC) and brushless DC (BLDC) motors. The driver component(s) implement the low-level From 52f83005ae81b7e4d7846a73eaec2354e73b166f Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 29 Jun 2026 10:45:30 -0500 Subject: [PATCH 2/3] fix sa --- .../example/main/bldc_haptics_example.cpp | 14 ++++---------- .../bldc_motor/example/main/bldc_motor_example.cpp | 11 +++-------- 2 files changed, 7 insertions(+), 18 deletions(-) diff --git a/components/bldc_haptics/example/main/bldc_haptics_example.cpp b/components/bldc_haptics/example/main/bldc_haptics_example.cpp index d1f0d50b6..cf1affe26 100644 --- a/components/bldc_haptics/example/main/bldc_haptics_example.cpp +++ b/components/bldc_haptics/example/main/bldc_haptics_example.cpp @@ -40,14 +40,9 @@ extern "C" void app_main(void) { { logger.info("Running BLDC Haptics example for {} seconds!", num_seconds_to_run); - static constexpr float core_update_period = 0.001f; // seconds - // The motor and driver are set up below depending on the selected hardware. std::shared_ptr driver; std::shared_ptr motor; - // Objects which must outlive the motor for the standalone (I2C) wiring. - std::unique_ptr i2c; - std::shared_ptr standalone_encoder; #if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS #if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI @@ -69,6 +64,9 @@ extern "C" void app_main(void) { driver = board.motor_driver(example_motor_index); #else logger.info("Using test-stand / custom wiring (I2C MT6701 + TMC6300)"); + // Objects which must outlive the motor for the standalone (I2C) wiring. + std::unique_ptr i2c; + std::shared_ptr standalone_encoder; // make the I2C that we'll use to communicate with the mt6701 (magnetic encoder) logger.info("initializing i2c driver..."); i2c = std::make_unique(espp::I2c::Config{ @@ -90,6 +88,7 @@ extern "C" void app_main(void) { logger.error("Failed to initialize MT6701 I2C device: {}", ec.message()); return; } + static constexpr float core_update_period = 0.001f; // seconds standalone_encoder = std::make_shared( Encoder::Config{.write = espp::make_i2c_addressed_write(encoder_device), .read = espp::make_i2c_addressed_read(encoder_device), @@ -153,11 +152,6 @@ extern "C" void app_main(void) { .log_level = espp::Logger::Verbosity::DEBUG}); #endif - if (!motor || !driver) { - logger.error("Motor / driver were not initialized, cannot run example!"); - return; - } - auto print_detent_config = [&logger](const auto &detent_config) { if (detent_config == espp::detail::UNBOUNDED_NO_DETENTS) { logger.info("Setting detent config to UNBOUNDED_NO_DETENTS"); diff --git a/components/bldc_motor/example/main/bldc_motor_example.cpp b/components/bldc_motor/example/main/bldc_motor_example.cpp index 9e7e09e81..a5e602c1f 100644 --- a/components/bldc_motor/example/main/bldc_motor_example.cpp +++ b/components/bldc_motor/example/main/bldc_motor_example.cpp @@ -49,9 +49,6 @@ extern "C" void app_main(void) { // The motor and driver are set up below depending on the selected hardware. std::shared_ptr driver; std::shared_ptr motor; - // Objects which must outlive the motor for the standalone (I2C) wiring. - std::unique_ptr i2c; - std::shared_ptr standalone_encoder; //! [bldc_motor example] #if CONFIG_EXAMPLE_HARDWARE_MOTORGO_MINI || CONFIG_EXAMPLE_HARDWARE_MOTORGO_AXIS @@ -74,6 +71,9 @@ extern "C" void app_main(void) { driver = board.motor_driver(example_motor_index); #else logger.info("Using test-stand / custom wiring (I2C MT6701 + TMC6300)"); + // Objects which must outlive the motor for the standalone (I2C) wiring. + std::unique_ptr i2c; + std::shared_ptr standalone_encoder; // make the I2C that we'll use to communicate with the mt6701 (magnetic encoder) logger.info("initializing i2c driver..."); i2c = std::make_unique(espp::I2c::Config{ @@ -158,11 +158,6 @@ extern "C" void app_main(void) { .log_level = espp::Logger::Verbosity::DEBUG}); #endif - if (!motor || !driver) { - logger.error("Motor / driver were not initialized, cannot run example!"); - return; - } - static const auto motion_control_type = espp::detail::MotionControlType::VELOCITY; // static const auto motion_control_type = espp::detail::MotionControlType::ANGLE; // static const auto motion_control_type = espp::detail::MotionControlType::VELOCITY_OPENLOOP; From 2517a3aae7b96e265d3d709d63edd90bff7b676b Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 29 Jun 2026 11:08:47 -0500 Subject: [PATCH 3/3] ignore .cache and remove from repo --- .gitignore | 1 + ...urrent_sense_example.cpp.C08DCA03C693871A.idx | Bin 3156 -> 0 bytes .../interrupt_example.cpp.CC43D66CE96D9977.idx | Bin 3476 -> 0 bytes 3 files changed, 1 insertion(+) delete mode 100644 components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx delete mode 100644 components/interrupt/example/.cache/clangd/index/interrupt_example.cpp.CC43D66CE96D9977.idx diff --git a/.gitignore b/.gitignore index 374145953..e4c7f6946 100644 --- a/.gitignore +++ b/.gitignore @@ -47,3 +47,4 @@ lib/pc _build/ __pycache__/ managed_components/ +.cache diff --git a/components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx b/components/bldc_current_sense/example/.cache/clangd/index/bldc_current_sense_example.cpp.C08DCA03C693871A.idx deleted file mode 100644 index 545220055623517308e1959e1be8ec630644dd35..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3156 zcmZ8i2{@GN7oT^C8Osy4|9U6y{`MET8CYU zo76HPO_2_@RHNO`Q&x=(UxaooKVb`F!<46Qx^Mg=MnXHEteoN&DAKA9WIHX4q||0! zEg4R!w3+2}%iznn-ZK6LuZS1Th>NON36c83Z;W`+T9r79zM$*+enB=VHs)_QMg~(E z${}$uC*a!IuQtP6+Jp9W$oo!J^@+1fAqAQtK?47IW$~x->4qb0U6N}0NW*0t>Q;!6 zfLn!>4u8-4h0@F%O?4QeGBPG={Zd)pCTqTUOH~r){TjaNocn=d4&TyNX#_mKCC{Vt z^5p*N*HI#Gu+6V?Hc}q;uz1U5__2dlA1>fHgXMNWk7guXFsF zL#ph6uLgc0A)Mm-SNA*0YX#+@^CO%5H2tK3s*vh1q;5L--_{3tVU?FDMWf<_KFH78rXhZc#O8@C9kjYmQEb;53ul5 zWY@;!g`HwbKgOwR`41av{z3ftZ;R|FHTNxUkuLbJD zW0vRK5QWqBp*4hyKYU-wr+iYKH`I5uyZPO+Got1h%O&}kjOeZQJD23#U5U0O%kFMH zfvKpxoQEz04TDA#2lMYo);7NU*w&ceshN{7XBt1I%j8ZWm-__=j637HZ9hW+-&=0?1tfn=5b(<|kVxkvG^T01I{pwR$N{?R+8)_UR;|jSeWn#GHJd)K)+vccvgYWgU(TKvpxqE&Z zqA7-Z&79Ae@NGS8)bo2pGW++Zov+GcC+x(b+R)hOxyU5zlKT6V*oMZ^w!8M{#qV-w z*QU7^303XcQ)e;!*7T8sqK_o$(OxENLa9rPxM&kRxOO;xp1C8soj=(HXXQmLMV}Gp zC^Kfurj(75H(sP(3V6lWkW@6EZP`1@Tvo3f*BqR|6HjT6?wWzqWNPU7oPT!kF3JGiG5Sl zqlD*a3!D1~8U-Y=#HF_?XD+XOnrQAM+>Rd~om+mKa}*tG5t8WA{=lhn-mZ^JKPEw) zyLaFid<0E+hVy^?t;iYPvgRweu4nX1jY5(5T0 zVw1{k?MVq)dBwCLmbQ384(&C|8u95T%@PHtulqNaJN<8-_1`+_9rnR4%=M*66XmuD z`A5jWOn=CRuk+8NfrwW;TTPbcKe<@y`LVE|i@qKL00vVH`OF!oV$OnsH-iQMqXH3x z2x^9L3mz> zCrLJHhH$`|=wYcq39TfwO(6u6N{q~AJF0{Qe)ktbh*gM(3K$eZaN{1Zznghij)Fhf z4uj#x2vPxqLJ*mR%_5PQ$5b@DXB#5UFD^_43<^P}Rp%RNJmVuMc=t9$m|vLgg+U=m zpx5aj7N+0bfwOJ51WJ*E3S_xt`JrGx48Jy*eZ7u=)Ae`+9<@^|U9`eGKw@vxkf0#@ zO$D%*3Y-L-#2~$AG8uDdQj7rT<~UXy0&)vAo)MkV?OoC3@Nyau4EF6Q(a*8RieavS z(BOmILPG&`bH*efb2jO*tI^r_k1?_pM~L%5e(0hMn~VS?-V*SfGoL=|&TuQsD!Y@_ z=!Nr{hYLr28G1{3OC=~A^b6>NXABqWr3l-wY5tzp&|m3cHDn-M>tNRKtBo}ZLoX*P zCkw@`r&(ba_w?rff0v;28Qs{u6m)Ynq}oo^(2g*wW-JC|hK^~*HCA9NnCN(6R$)$P z3+N(-6~hUc(M2pPmJ{mTv3}k3-u>TN8A;;Z@E%a|P_m3-8M_5VIYu@0DT+rv_|+VJ zOJfL6b9^Zrp3Tt@_~@gTaFT@dPx}UqCc{&QfB8R*cZKv&c8n?*iwXIE#Zt$sG{!l~ zOkW{oq%tRjr;A2Nqurt{yDitQ+r!AiJGY5%lUcn*Z;vyA;@ELJU$M=R3r<`Ku1gHP z9IM>U!~?y2PBNw$6nH8qoWXR`zfb7UcJXby`1W0V*Dn6vZ=Q(?IGS|U-^PblBHBPZ zxCc5wC+Gq+a38?v&(7)i&7bo1^jV-^+yQsNZ2)It-owI*K(euOaB`uK2b>S|_45yM zIOJ$&@BF8$)8S(-M~+?~T}%Y=;4(-6p}`?ur#wC6&YmNlNdc)K8K|jRTj~FyxKBk{ zLmjWArz>;7NLm&vviF3Pq=~WjX?aTvcQ;%t2n#1iUWx-TAR-Dx16vzIgCyW%YNoA4 z0et)dC_&Br2hC54ic5&;5EOWXF}&PDRiFfvf?`k${sz}U0k{Uzz*Ue9azGZy0Dpl@ nkOy)>I;a4Z;0h=NMW7tq1ZAL(j=lkIfojkS@<9y%^#JxiUP|;z diff --git a/components/interrupt/example/.cache/clangd/index/interrupt_example.cpp.CC43D66CE96D9977.idx b/components/interrupt/example/.cache/clangd/index/interrupt_example.cpp.CC43D66CE96D9977.idx deleted file mode 100644 index 529c237f0dda9e2b0ce50abb31c6117470fa34c3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3476 zcmcJQi$BxrAHes>7&DfQVaQxkiwVm$xt3cfVMs*gc9@uiVwL;7+!Ch3iR3oQwL>oH zlse9F?LWj&j^uoIey{O8f57kidTp=g{r-IR+`iA}nWOc-eHj=8!ra$7Duf>4 z&5uAJ@X&`D5gw=~fI!r$AP`X{cikr#6MERsE%?WkXB?k<%1eHTpMPB261H%QtusM5 zr?~wR(OTJx9JxGya+_@yduxCdonwM^>(%#9B0Lh=`x*O&)OWLWMn)oM(KIr0%>3fa zD4#}M()|6?`tcKUFZo$S4Z+ZNdn{i4f%Gw(Ske7`=ZapZhga=~fFCEcRufc*Z!A8u zyKvl(iDQ=U%%_tZ1DG|ti9a;G3_3?6ew_Fq>hAJ!bLpeHMW3&M8MNUoGp6QIioy8M zvECiK-LlsDng)&C@5W>E#&EY?F^d7;#(j+Ljo(;YqW_q7pbcl27Zblne+|hXd>s8G zP~YD){rux;{XGo92dc$go-?E0WAv-vFAEPWeh`1l^0FU4-d2>|SrZc=<3+5qEBUog z|7?}xyJ{aYTE*7WJwPdGrZ-m2rihl&B~om21WVLrr2aZ{6BU2T+aj=SEWc56>VnzJ z-S*@QO;N_3S7ni#f7TcqSn%+Z9*>i*#*vpfvxJkbC?MSVLzl$qMwab=-Z?hg?W9*wT_1CvHQjO zt})sTE=_4CC_GI$$&uAMCQ5Ul5qi5nvbUF8SPo~A_~(552m_s(Ux2^ClU=5*W%CCJ zSCwkhs&l=Q#kvD+ITA>*fe_=Sge#cJe97g`T(n>gcIVg96OEoYejr9S)m;&znjWWl zL^QrX!lvFyGubtMH{1@(Pqo(VJtNi~;?^Q+cw3ios;WfVp;}~zI&Mw;eM-_q8Tz&% zRVODhp=B6rmfLhD_y^)-sG0%hiY=A$CPk~anLNGpAb7=%F&6mA>wITj*9uxEq8FWo zeB09@_4==s@i)~u6Mj?0E3cY8l&LQI9LC9n*gq22gq^XOi4wq&{4#WZZ-T1xo9x`g zFXgO+IiL5~IJ37aPcW3!3_2*5V1@>p9Qf&|YPVar{H4b`MK9mRx-^;1-pf0vtIEKI z_`bVnbLdf9^H3k|E5~Dy_8L8|@&)ZWnA2*iAk=!Z>450p3z4zbVhH2q)sE4fgZ7-K zvahww7$j`}o1?F25$2l-cpKT@XlvM&UvAW4R3B_P%FOMwGQ1e~krn>ws=9r3PV}Bg z^}VMS&L#F$r*?|Jt(W&Sz`l^sf07tcu_Z`#Y&Y(F{pa+vo=Hdj zpRxKzeEItGXYpjQS!Xrf*XP5cOU})@iN&f$i>|eeId;td7Q{F1sOaf}41d3HR$|Fs zDIz9baD4L&Rkdj3(wytd!YWB8I^D$hx|-HxnPFL??UF-Pg|qd7Qo7yn_6I377IiEO z-JE?G{)%UxYs|k}Nptu!hWhX43FYJuxte0`Ft4b7E12xKHnz-(F|8^<#gwGUkyB(6 zN`^$sytHpIby6xX;tnK>?mCNASv#)ouotOO)xKse5Z`feQSN2+!luc!BND=+iOx#D zkEN|@G@V^|r2ZtRY?%+8FQ|3cdv4ZOsG>i})O%U^Ma%_e9;#TlRNGR>Jo|N)f-Aug z*!u?CMx_1KU7w+1Z&P`5%lzrjL9XT8Zq24ks)`L zbJ1}{DmnM3e!-8hvX5maHUzw}!wsU?^^CG+gwc@OkvM%SG4ilRMsrDJ*JtA%uhSs%lKO#P)kfwNR*9j3s4L~3o01k_j`rkJc{Pzi``!Rcv2!y5D zQnmj*1V0dI0F40K2qbwKf151d+_zkU5`-dn6kc?_PYBKMRxq?6T99DqM0A3q7tsrj z(ZpytvH_6|)2SULR(Um^uXHcGEca@wE+6Ejl%(`{9zvolQS<8l z^1kKh2glxQOTPL1S;Srxw_8P-9k4VKinJMq6ch!HhQfw8SaiSeemK&EX>be^4ufMJ z5Y7YZ;#--CQds1l*8i?RQhsB_Dn-hXby_xj&|XLxNEz`)gOI3A)Zwi_rNiMWW%auq z3i1*{5*S#T99nMu)k0B!v_BjJ&;f9a2k7{XlK`cs0d(4bL^(j0|3};a=o=eiS!nc6 zn+Cim_uZue3 z>!PvCmiQLgU-W;j1o!)fIsH0c+qkC!?~HfjEu+h6MEZ1r$bA7kREQ)?MV^PyhvWmt zAW{$^6JgtdCZNeMQ;Lv{15GuTBV^l$_w`rQ`+2Xr*X4vVjsCtP9!pJJyX4Pfc-8S~U=@ z22#9XFc@mJ?PBf=Zsa-;y$%Sx$e=4#Twqn`J2w(58cXE)qmed6r(~FG+$y6h($_Bl z2>q1()Ol&)^k8r9qM6~xn2y{=J#^*s9uBPCw9($w01mlq@>Ge-Tte1?dg!ig0#It* zMh2pBqkSyIH~1;kV>PIOrp~vCA1NS+5)wv>U^a_82LuMu{rrEjw%KoKWq;7g&erje z!-3G_VP`=c>kME7ha5TV;jVbp$J;9bB!YOLw!^|)XV+F0@^*^)HdU&&g1*5PlB5)Y zCNHV!Z