diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 7769d61376..1a805be167 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -77,6 +77,8 @@ jobs: target: esp32 - path: 'components/basicmicro/example' target: esp32 + - path: 'components/mcp266/example' + target: esp32 - path: 'components/bdc_driver/example' target: esp32s3 - path: 'components/binary-log/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 5e8eb4b844..50ddb165f2 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -56,6 +56,7 @@ jobs: components/base_component components/base_peripheral components/basicmicro + components/mcp266 components/bdc_driver components/binary-log components/bldc_current_sense diff --git a/components/mcp266/CMakeLists.txt b/components/mcp266/CMakeLists.txt new file mode 100644 index 0000000000..84a9ba6d0a --- /dev/null +++ b/components/mcp266/CMakeLists.txt @@ -0,0 +1,7 @@ +# NOTE: like canopen, this component's detail/ lives INSIDE include/ +# (include/detail/mcp266_core.hpp), so registering "include" alone makes +# `#include "detail/mcp266_core.hpp"` resolve for consumers. +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES base_component canopen +) diff --git a/components/mcp266/README.md b/components/mcp266/README.md new file mode 100644 index 0000000000..d77ce9879b --- /dev/null +++ b/components/mcp266/README.md @@ -0,0 +1,63 @@ +# MCP266 CANopen Motor Controller Component + +[![Badge](https://components.espressif.com/components/espp/mcp266/badge.svg)](https://components.espressif.com/components/espp/mcp266) + +The `Mcp266` class is a dual-channel controller for a Basicmicro **MCP266** +(RoboClaw family) brushed-DC motor driver over **CANopen**. It is layered on +`espp::CanopenClient` (like `espp::Ds402Drive`), so it is transport-agnostic: +the application owns the CAN transport, feeds received frames to the client's +`process_frame()`, and the client's node id selects the MCP266. + +Both motor channels (`M1`, `M2`) are driven symmetrically. M2's CiA 402 +objects mirror M1's at `+0x800`, handled through `Ds402Drive`'s object offset. + +## What works, and what does not + +**Position control** uses the standard CiA 402 profile position mode +(`move_to_position`) and is the supported, validated capability. It needs the +position loop configured first (`configure_position_loop`). + +**Velocity / duty control is not functional** on the MCP266 firmware tested. +The standard target objects and the manufacturer speed/duty command mirror are +both accepted by the drive but leave the velocity generator idle even with the +drive in Operation Enabled. Supported-drive-modes (`0x6502`) advertises only +the cyclic-sync modes, so velocity likely requires csv mode with cyclic +SYNC/PDO updates, which is undocumented for this device. `drive_speed` / +`drive_duty` are implemented but are currently a no-op for motion; use position +mode. + +## Device specifics + +The MCP266's control-loop parameters are **not** standard CiA 402 objects. The +MCP mirrors its packet-serial command set into the manufacturer region of the +object dictionary at index `0x2000 + command number` (see +`include/detail/mcp266_core.hpp`, which is host-buildable and unit-tested). +This component uses that to: + +* configure the position PID (commands 61-64), +* issue the manufacturer speed/duty commands (32/33, 35/36), and +* read telemetry: main battery (24) and temperature (82). + +Two device quirks are handled by `configure_position_loop()`, which must be +called once per boot (the MCP reverts to its EEPROM configuration on power-up): + +* The position PID's `MinPos`/`MaxPos` clamp defaults to `[0, 0]`, which forces + every position target to zero — it is widened here. +* The setter (commands 61/62) uses field order `D, P, I` while the readback + (63/64) uses `P, I, D`, so a naive read-modify-write of the record would move + `P` into the `D` slot and zero `P`. The correct field shuffle (and seeding a + non-zero `P` when the record has none) is done here. + +## Example + +The [example](./example) brings up an `espp::Twai` transport and an +`espp::CanopenClient`, NMT-starts the node, reads telemetry, configures the M1 +position loop, and runs a small profile-position sequence. + +## Related components + +* `espp/canopen` — the CANopen client and the `Ds402Drive` CiA 402 helper this + component builds on. +* `espp/basicmicro` — the Basicmicro **packet serial** protocol driver (a + different transport to the same controller family), including velocity and + position PID configuration over UART. diff --git a/components/mcp266/example/CMakeLists.txt b/components/mcp266/example/CMakeLists.txt new file mode 100644 index 0000000000..f8b767656c --- /dev/null +++ b/components/mcp266/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 mcp266 canopen twai" + CACHE STRING + "List of components to include" + ) + +project(mcp266_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/mcp266/example/README.md b/components/mcp266/example/README.md new file mode 100644 index 0000000000..1279938007 --- /dev/null +++ b/components/mcp266/example/README.md @@ -0,0 +1,41 @@ +# MCP266 CANopen Example + +This example demonstrates how to use the `espp::Mcp266` component to drive a +Basicmicro MCP266 (RoboClaw-family) motor controller over **CANopen**. It: + +1. brings up an `espp::Twai` transport and an `espp::CanopenClient`, wiring the + received frames into the client, +2. NMT-starts the node and clears any latched CiA 402 faults, +3. reads the main battery voltage and board temperature, +4. configures the M1 position loop (widening the position clamp and seeding a + non-zero P gain — required once per boot), and +5. runs a small profile-position sequence on M1, reporting arrival at each + target. + +See the [component README](../README.md) for the device specifics (the +manufacturer command-object mirror, the position-PID field-order quirk, and the +velocity-control limitation). + +## Requirements + +- An ESP target with a TWAI (CAN) controller. +- A 3.3 V CAN transceiver (e.g. SN65HVD230) wired to the configured TX/RX + GPIOs, on a properly terminated bus. +- A Basicmicro MCP266 configured for CANopen (CAN pins, bit rate, and node id + set in Basicmicro Motion Studio), with an encoder and a tuned velocity PID. + +## Hardware + +Edit the `tx_gpio`, `rx_gpio`, `baudrate`, and `node_id` in +`main/mcp266_example.cpp` to match your board and MCP266 configuration. + +## Build + +Build the project and flash it to the board, then run monitor to view the +serial output: + +```sh +idf.py -p PORT flash monitor +``` + +Replace PORT with the serial port of your board. diff --git a/components/mcp266/example/main/CMakeLists.txt b/components/mcp266/example/main/CMakeLists.txt new file mode 100644 index 0000000000..a941e22ba7 --- /dev/null +++ b/components/mcp266/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/mcp266/example/main/mcp266_example.cpp b/components/mcp266/example/main/mcp266_example.cpp new file mode 100644 index 0000000000..08c145776f --- /dev/null +++ b/components/mcp266/example/main/mcp266_example.cpp @@ -0,0 +1,136 @@ +#include +#include +#include + +#include "canopen_client.hpp" +#include "mcp266.hpp" +#include "twai.hpp" + +using namespace std::chrono_literals; + +extern "C" void app_main(void) { + static espp::Logger logger({.tag = "MCP266 Example", .level = espp::Logger::Verbosity::INFO}); + logger.info("Starting MCP266 CANopen example!"); + + //! [mcp266 example] + // The CANopen node id configured on the MCP266 (Motion Studio -> CAN + // settings). Change to match your device. + static constexpr uint8_t node_id = 10; + + // Forward-declared handle so the Twai on_receive callback (registered at + // Twai construction) can feed frames to the client constructed below. + static espp::CanopenClient *client_ptr = nullptr; + + // Bring up the TWAI (CAN 2.0) peripheral. Talking to a real MCP266 requires + // Mode::NORMAL with a 3.3 V CAN transceiver on the tx/rx GPIOs, a properly + // terminated bus, and a matching baudrate (set in Motion Studio). + // NOTE: twai / client / mcp are function-local statics: the Twai receive + // task and the client's send lambda reference them, and app_main() has + // early-return paths, so static storage keeps them alive for every callback. + static espp::Twai twai({ + .tx_gpio = 17, // change to match your board / transceiver + .rx_gpio = 16, + .baudrate = 1000000, + .mode = espp::Twai::Mode::NORMAL, + .tx_queue_depth = 10, + .on_receive = + [](const espp::Twai::Message &msg) { + if (client_ptr) { + client_ptr->process_frame(espp::CanopenClient::CanFrame{ + .id = msg.id, + .extended = msg.extended, + .rtr = msg.rtr, + .dlc = msg.dlc, + .data = msg.data, + }); + } + }, + .log_level = espp::Logger::Verbosity::WARN, + }); + + static espp::CanopenClient client({ + .node_id = node_id, + // captureless: twai has static storage duration and is referenced + // directly (capturing a static is ill-formed under -Werror) + .send = + [](const espp::CanopenClient::CanFrame &frame) { + espp::Twai::Message msg{ + .id = frame.id, + .extended = frame.extended, + .rtr = frame.rtr, + .dlc = frame.dlc, + .data = frame.data, + }; + std::error_code tx_ec; + return twai.transmit(msg, tx_ec); + }, + .sdo_timeout = 500ms, + .log_level = espp::Logger::Verbosity::WARN, + }); + client_ptr = &client; + + static espp::Mcp266 mcp(client, {.log_level = espp::Logger::Verbosity::INFO}); + + std::error_code ec; + if (!twai.initialize(ec)) { + logger.error("Failed to initialize TWAI: {}", ec.message()); + return; + } + // NMT-start the node and clear any latched faults on both axes. + if (!mcp.start(ec)) { + logger.error("Failed to start MCP266: {} -- is the node on the bus?", ec.message()); + return; + } + + // Telemetry sanity check. + float volts = 0.0f, temp_c = 0.0f; + if (mcp.read_main_battery_voltage(volts, ec)) { + logger.info("Main battery: {:.1f} V", volts); + } + if (mcp.read_temperature(temp_c, ec)) { + logger.info("Board temperature: {:.1f} C", temp_c); + } + + using Axis = espp::Mcp266::Axis; + + // One-time per-boot position-loop setup on M1: widen the MinPos/MaxPos clamp + // (factory [0, 0] forces every target to zero) and ensure a non-zero + // position P gain. The MCP reverts to EEPROM on power-up, so this must run + // every boot before commanding moves. Clear any latched e-stop first. + mcp.reset_estop(ec); + if (!mcp.configure_position_loop(Axis::M1, -2'000'000'000, 2'000'000'000, ec)) { + logger.error("Failed to configure M1 position loop: {}", ec.message()); + return; + } + mcp.set_position_limits(Axis::M1, -20'000, 20'000, ec); + + // Run a small profile-position sequence and report arrival. + static constexpr int32_t targets[] = {10'000, -10'000, 0}; + static constexpr uint32_t profile_velocity = 500; // counts/s + static constexpr uint32_t profile_accel = 500; // counts/s^2 + static constexpr uint32_t profile_decel = 500; // counts/s^2 + static constexpr int32_t tolerance = 100; // counts + for (int32_t target : targets) { + logger.info("Moving M1 to {}", target); + if (!mcp.move_to_position(Axis::M1, target, profile_velocity, profile_accel, profile_decel, + ec)) { + logger.error("Move command rejected: {}", ec.message()); + continue; + } + const auto deadline = std::chrono::steady_clock::now() + 30s; + while (std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(250ms); + int32_t position = 0; + if (mcp.read_encoder(Axis::M1, position, ec) && std::abs(position - target) <= tolerance) { + logger.info(" reached {} (position={})", target, position); + break; + } + } + } + //! [mcp266 example] + + logger.info("MCP266 example complete!"); + while (true) { + std::this_thread::sleep_for(1s); + } +} diff --git a/components/mcp266/example/sdkconfig.defaults b/components/mcp266/example/sdkconfig.defaults new file mode 100644 index 0000000000..c3667f3e33 --- /dev/null +++ b/components/mcp266/example/sdkconfig.defaults @@ -0,0 +1,4 @@ +# Common ESP-related +# +CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096 +CONFIG_ESP_MAIN_TASK_STACK_SIZE=8192 diff --git a/components/mcp266/idf_component.yml b/components/mcp266/idf_component.yml new file mode 100644 index 0000000000..3a61c31c3c --- /dev/null +++ b/components/mcp266/idf_component.yml @@ -0,0 +1,25 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Dual-channel Basicmicro MCP266 (RoboClaw family) motor controller over CANopen (CiA 402 profile position)" +url: "https://github.com/esp-cpp/espp/tree/main/components/mcp266" +repository: "https://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/motorcontrol/mcp266.html" +examples: + - path: example +tags: + - cpp + - Component + - Basicmicro + - MCP266 + - RoboClaw + - CANopen + - CiA402 + - DS402 + - Motor +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/canopen: '>=1.0' diff --git a/components/mcp266/include/detail/mcp266_core.hpp b/components/mcp266/include/detail/mcp266_core.hpp new file mode 100644 index 0000000000..336cfda2c0 --- /dev/null +++ b/components/mcp266/include/detail/mcp266_core.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include + +/// \file mcp266_core.hpp +/// \brief Host-buildable, ESP-independent core for the Basicmicro MCP266 +/// CANopen mapping: the manufacturer command-object mirror, per-axis +/// object addresses, and the position-PID field-order remap. Pure +/// constexpr so it can be unit-tested on the host. + +namespace espp { +namespace detail { +namespace mcp266 { + +/// \brief The MCP266 mirrors its packet-serial command set into the +/// manufacturer region of the CANopen object dictionary at index +/// 0x2000 + command number. Verified on MCP266 firmware: command 61 +/// (set M1 position PID) = 0x203D, 55 (read M1 velocity PID) = 0x2037, +/// 24 (read main battery) = 0x2018, 200 (e-stop reset) = 0x20C8. +/// \param command The Basicmicro packet-serial command byte. +/// \return The corresponding manufacturer object index. +inline constexpr uint16_t command_object(uint8_t command) { + return static_cast(0x2000 + command); +} + +/// \brief Object offset added to a CiA 402 device-profile index (0x60xx) to +/// select a motor axis. M1 is at the standard indices; M2 mirrors them +/// at +0x800 (e.g. controlword 0x6040 -> 0x6840). +/// @{ +inline constexpr uint16_t kAxisOffsetM1 = 0x000; +inline constexpr uint16_t kAxisOffsetM2 = 0x800; +/// @} + +/// \brief Device-level (non-axis) telemetry / maintenance objects, mirrored +/// from their packet-serial commands. +/// @{ +inline constexpr uint16_t kMainBatteryObject = command_object(24); ///< tenths of a volt (u16) +inline constexpr uint16_t kTemperatureObject = command_object(82); ///< tenths of a degree C (u16) +inline constexpr uint16_t kEStopResetObject = command_object(200); ///< write-only +/// @} + +/// \brief The manufacturer command objects and CiA 402 offset for one axis. +struct AxisObjects { + uint16_t object_offset; ///< 0 for M1, 0x800 for M2 (added to 0x60xx objects). + uint16_t position_pid_set; ///< Position PID setter (command 61/62), write-only. + uint16_t position_pid_get; ///< Position PID readback (command 63/64). + uint16_t drive_duty; ///< Signed-duty command (32/33). + uint16_t drive_speed; ///< Signed-speed command (35/36). +}; + +/// \brief Objects for motor 1 (the standard axis). +inline constexpr AxisObjects axis_m1() { + return {kAxisOffsetM1, command_object(61), command_object(63), command_object(32), + command_object(35)}; +} +/// \brief Objects for motor 2 (mirrored at +0x800 / command n+1). +inline constexpr AxisObjects axis_m2() { + return {kAxisOffsetM2, command_object(62), command_object(64), command_object(33), + command_object(36)}; +} + +/// \brief Remap a position-PID record from the readback order to the setter +/// order. +/// \details The readback (commands 63/64) reports +/// [P, I, D, MaxI, Deadzone, MinPos, MaxPos] but the setter +/// (commands 61/62) takes the packet-serial write order +/// [D, P, I, MaxI, Deadzone, MinPos, MaxPos]. Writing the readback +/// array straight back would put P into the D slot and leave P at 0 +/// (no loop output). This performs the correct field shuffle. +/// \param readback The seven values as read from the readback object. +/// \return The seven values in setter order. +inline constexpr std::array +position_pid_readback_to_setter(const std::array &readback) { + return {readback[2], readback[0], readback[1], readback[3], + readback[4], readback[5], readback[6]}; +} + +} // namespace mcp266 +} // namespace detail +} // namespace espp diff --git a/components/mcp266/include/mcp266.hpp b/components/mcp266/include/mcp266.hpp new file mode 100644 index 0000000000..0d5e09aff8 --- /dev/null +++ b/components/mcp266/include/mcp266.hpp @@ -0,0 +1,379 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "canopen_client.hpp" +#include "detail/mcp266_core.hpp" +#include "ds402.hpp" + +namespace espp { + +/// \brief Dual-channel controller for a Basicmicro MCP266 (RoboClaw family) +/// brushed-DC motor driver over CANopen. +/// \details Layered on a CanopenClient (like Ds402Drive), so it is +/// transport-agnostic: the application owns the CAN transport, feeds +/// received frames to the client's process_frame(), and the client's +/// node id selects the MCP266. Both motor channels (M1, M2) are +/// driven symmetrically; M2's CiA 402 objects mirror M1's at +0x800, +/// handled through Ds402Drive's object offset. +/// +/// Position control uses the standard CiA 402 profile position mode +/// and is the supported, validated capability. The MCP266's +/// control-loop parameters are NOT standard CiA 402 objects: the MCP +/// mirrors its packet-serial command set into the manufacturer region +/// at object index 0x2000 + command number, which this class uses to +/// configure the position PID (commands 61-64), issue the +/// manufacturer speed/duty commands (32/33, 35/36), and read +/// telemetry (24, 82). See detail/mcp266_core.hpp. +/// +/// \b Important: two device quirks must be handled, both done by +/// configure_position_loop(): +/// - The position PID's MinPos/MaxPos clamp defaults to [0, 0], +/// which forces every position target to zero. +/// - The setter (commands 61/62) uses field order D, P, I while the +/// readback (63/64) uses P, I, D, so a naive read-modify-write of +/// the record moves P into the D slot and zeros P. +/// The MCP reverts to its EEPROM configuration on power-up, so call +/// configure_position_loop() once per boot before commanding moves. +/// +/// \b Note: the manufacturer speed/duty command mirror +/// (drive_speed / drive_duty) is implemented but does NOT produce +/// motion on the MCP266 firmware tested (the command is accepted but +/// the velocity generator stays idle). Use position mode for motion. +/// +/// \section mcp266_ex0 MCP266 Example +/// \snippet mcp266_example.cpp mcp266 example +class Mcp266 : public BaseComponent { +public: + /// \brief Motor channel selector. + enum class Axis { M1, M2 }; + + /// \brief Configuration for the Mcp266 controller. + struct Config { + std::chrono::milliseconds state_timeout{1000}; ///< Per-axis CiA 402 transition timeout. + std::chrono::milliseconds poll_period{25}; ///< Statusword polling period. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Logger verbosity. + }; + + /// \brief Create an MCP266 controller. + /// \param client The CANopen client for the MCP266's node. Must outlive this + /// object, and its process_frame() must be driven from another task + /// (e.g. the transport's receive task) per CanopenClient's contract. + /// \param config The configuration. + explicit Mcp266(CanopenClient &client, const Config &config) + : BaseComponent("Mcp266", config.log_level) + , client_(client) + , m1_(client, detail::mcp266::axis_m1(), "M1", config) + , m2_(client, detail::mcp266::axis_m2(), "M2", config) {} + + /// \brief Create an MCP266 controller with the default configuration. + explicit Mcp266(CanopenClient &client) + : Mcp266(client, Config{}) {} + + /// \brief NMT-start the node and clear any latched CiA 402 faults on both + /// axes. Call once after the transport and client are up. + /// \param ec Set on failure. + /// \return True on success. + bool start(std::error_code &ec) { + ec.clear(); + if (!client_.nmt_start(ec)) { + // Fail fast: reset_faults() below clears ec, so a swallowed NMT failure + // would otherwise be reported as overall success. + logger_.error("NMT start failed: {}", ec.message()); + return false; + } + return reset_faults(ec); + } + + /// \brief Clear any latched CiA 402 fault on both axes. \param ec Set on + /// failure. \return True on success. + bool reset_faults(std::error_code &ec) { + ec.clear(); + for (AxisState *a : {&m1_, &m2_}) { + const auto state = a->drive.get_state(ec); + if (ec) { + return false; + } + if (state == Ds402Drive::State::Fault || state == Ds402Drive::State::FaultReactionActive) { + logger_.warn("{}: clearing fault", a->name); + if (!a->drive.fault_reset(ec)) { + return false; + } + } + } + return true; + } + + /// \brief Attempt an E-stop reset (mirrored packet-serial command 200 at + /// 0x20C8). Harmless when nothing is latched. \param ec Set on + /// failure. \return True if accepted. + bool reset_estop(std::error_code &ec) { + ec.clear(); + // Command 200 has no payload, so the SDO scalar width is unknown; try the + // common ones. + bool ok = client_.write_u8(detail::mcp266::kEStopResetObject, 0, 1, ec); + if (!ok) { + ec.clear(); // don't let the first attempt's error leak into the retry + ok = client_.write_u32(detail::mcp266::kEStopResetObject, 0, 1, ec); + } + logger_.info("E-stop reset {}", ok ? "accepted" : "rejected"); + return ok; // on success ec is clear; on failure ec holds the last error + } + + /// @name Position control (CiA 402 profile position mode) + /// @{ + + /// \brief Configure an axis's position loop for use: widen the MinPos/MaxPos + /// clamp (factory [0, 0] forces every target to zero) and, only if the + /// drive's stored position P gain reads back as zero, seed a non-zero + /// P so the loop produces output. The record is written through the + /// setter's D, P, I field order and the clamp verified via the + /// readback. The MCP reverts to EEPROM on power-up, so call once per + /// boot. + /// \note \p fallback_p is a coarse starting value used ONLY when the drive + /// has no stored P gain; it is not motor-tuned. For good motion, tune + /// the position PID in Basicmicro Motion Studio (or pass a value + /// appropriate for your motor / encoder) -- an unsuitable P gain can + /// leave the axis sluggish or make it oscillate. A drive whose P gain + /// is already non-zero keeps its stored gains untouched. + /// \param axis The motor channel. + /// \param min_pos Minimum commandable position. + /// \param max_pos Maximum commandable position. + /// \param ec Set on failure. + /// \param fallback_p Position P gain to seed when the stored gain is zero. + /// \return True on success. + bool configure_position_loop(Axis axis, int32_t min_pos, int32_t max_pos, std::error_code &ec, + int32_t fallback_p = kDefaultPositionP) { + ec.clear(); + AxisState &a = axis_state(axis); + std::array readback{}; + for (uint8_t sub = 1; sub <= 7; ++sub) { + readback[sub - 1] = client_.read_i32(a.objects.position_pid_get, sub, ec); + if (ec) { + logger_.error("{}: position PID read 0x{:04X}:{} failed: {}", a.name, + a.objects.position_pid_get, sub, ec.message()); + return false; + } + } + // readback order is [P, I, D, MaxI, Deadzone, MinPos, MaxPos] + if (readback[0] == 0) { + readback[0] = fallback_p; // seed P only if the record has none + logger_.warn("{}: stored position P gain was 0; seeding coarse fallback {} (tune for your " + "motor)", + a.name, fallback_p); + } + readback[5] = min_pos; + readback[6] = max_pos; + const auto setter = detail::mcp266::position_pid_readback_to_setter(readback); + for (uint8_t sub = 1; sub <= 7; ++sub) { + if (!client_.write_i32(a.objects.position_pid_set, sub, setter[sub - 1], ec)) { + logger_.error("{}: position PID write 0x{:04X}:{} rejected: {}", a.name, + a.objects.position_pid_set, sub, ec.message()); + return false; + } + } + // verify via the readback's field order (min/max are subs 6/7 there too) + const int32_t got_min = client_.read_i32(a.objects.position_pid_get, 6, ec); + const int32_t got_max = client_.read_i32(a.objects.position_pid_get, 7, ec); + if (ec || got_min != min_pos || got_max != max_pos) { + logger_.error("{}: position clamp did not take (read [{}, {}], wanted [{}, {}])", a.name, + got_min, got_max, min_pos, max_pos); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + logger_.info("{}: position loop configured (P={}, clamp=[{}, {}])", a.name, readback[0], + min_pos, max_pos); + return true; + } + + /// \brief Set the CiA 402 software position limits (0x607D:1/:2) for an axis. + /// \param axis The motor channel. \param min_pos Lower limit. \param max_pos + /// Upper limit. \param ec Set on failure. \return True on success. + bool set_position_limits(Axis axis, int32_t min_pos, int32_t max_pos, std::error_code &ec) { + ec.clear(); + if (min_pos > max_pos) { + ec = std::make_error_code(std::errc::invalid_argument); + return false; + } + const uint16_t obj = static_cast(0x607D + axis_state(axis).objects.object_offset); + return client_.write_i32(obj, 1, min_pos, ec) && client_.write_i32(obj, 2, max_pos, ec); + } + + /// \brief Command a profile-position move: enable the axis, set the motion + /// profile, and issue the target with the new-set-point handshake. + /// \param axis The motor channel. + /// \param target_position Absolute target position (encoder counts). + /// \param profile_velocity Cruise velocity. + /// \param profile_acceleration Acceleration. + /// \param profile_deceleration Deceleration. + /// \param ec Set on failure. + /// \return True once the set-point is accepted. + bool move_to_position(Axis axis, int32_t target_position, uint32_t profile_velocity, + uint32_t profile_acceleration, uint32_t profile_deceleration, + std::error_code &ec) { + ec.clear(); + AxisState &a = axis_state(axis); + if (!enable(a, Ds402Drive::OperatingMode::ProfilePosition, ec)) { + return false; + } + if (!(a.drive.set_profile_acceleration(profile_acceleration, ec) && + a.drive.set_profile_deceleration(profile_deceleration, ec) && + a.drive.set_profile_velocity(profile_velocity, ec))) { + return false; + } + return a.drive.set_target_position(target_position, ec); + } + + /// @} + + /// @name Manufacturer speed / duty command mirror + /// @note Accepted by the drive but inert on the MCP266 firmware tested; kept + /// for completeness and in case a firmware update activates them. + /// @{ + + /// \brief Closed-loop speed via the mirrored packet-serial command (35/36). + bool drive_speed(Axis axis, int32_t qpps, std::error_code &ec) { + ec.clear(); + AxisState &a = axis_state(axis); + if (qpps != 0 && !enable(a, Ds402Drive::OperatingMode::ProfileVelocity, ec)) { + return false; + } + return client_.write_i32(a.objects.drive_speed, 0, qpps, ec); + } + + /// \brief Open-loop duty via the mirrored packet-serial command (32/33). + bool drive_duty(Axis axis, int16_t duty, std::error_code &ec) { + ec.clear(); + AxisState &a = axis_state(axis); + if (duty != 0 && !enable(a, Ds402Drive::OperatingMode::ProfileVelocity, ec)) { + return false; + } + return client_.write_i16(a.objects.drive_duty, 0, duty, ec); + } + + /// @} + + /// @name Feedback + /// @{ + + /// \brief Read the actual position (0x6064 / 0x6864). \param axis Channel. + /// \param count Out: encoder counts. \param ec Set on failure. \return True on success. + bool read_encoder(Axis axis, int32_t &count, std::error_code &ec) { + ec.clear(); + count = axis_state(axis).drive.get_position_actual(ec); + return !ec; + } + /// \brief Read the actual velocity (0x606C / 0x686C). \param axis Channel. + /// \param qpps Out: counts/s. \param ec Set on failure. \return True on success. + bool read_speed(Axis axis, int32_t &qpps, std::error_code &ec) { + ec.clear(); + qpps = axis_state(axis).drive.get_velocity_actual(ec); + return !ec; + } + /// \brief Read the CiA 402 statusword (0x6041 / 0x6841). \param axis Channel. + /// \param statusword Out. \param ec Set on failure. \return True on success. + bool read_statusword(Axis axis, uint16_t &statusword, std::error_code &ec) { + ec.clear(); + statusword = axis_state(axis).drive.get_statusword(ec); + return !ec; + } + + /// @} + + /// @name Device telemetry + /// @{ + + /// \brief Read the main battery voltage (mirrored command 24). \param volts + /// Out: volts. \param ec Set on failure. \return True on success. + bool read_main_battery_voltage(float &volts, std::error_code &ec) { + ec.clear(); + volts = static_cast(client_.read_u16(detail::mcp266::kMainBatteryObject, 0, ec)) / 10.0f; + return !ec; + } + /// \brief Read the board temperature (mirrored command 82). \param temp_c + /// Out: degrees C. \param ec Set on failure. \return True on success. + bool read_temperature(float &temp_c, std::error_code &ec) { + ec.clear(); + temp_c = + static_cast(client_.read_u16(detail::mcp266::kTemperatureObject, 0, ec)) / 10.0f; + return !ec; + } + /// \brief Read the standard device type (0x1000) and name (0x1008). + bool read_device_info(std::string &device_name, uint32_t &device_type, std::error_code &ec) { + ec.clear(); + device_type = client_.read_u32(0x1000, 0, ec); + if (ec) { + return false; + } + device_name = client_.read_string(0x1008, 0, ec); + return !ec; + } + + /// @} + + /// \brief Access an axis's underlying Ds402Drive for advanced CiA 402 use. + /// \param axis The motor channel. \return Reference to the axis drive helper. + Ds402Drive &drive(Axis axis) { return axis_state(axis).drive; } + +private: + /// Coarse fallback position P gain, used only when the drive's stored gain + /// reads back as zero (see configure_position_loop()). It is a non-tuned + /// starting point that produces motion out of the box, not a good gain for + /// any particular motor; callers should tune and pass their own. + static constexpr int32_t kDefaultPositionP = 0x3C83; + + /// Per-axis state: the manufacturer object addresses and a Ds402Drive whose + /// object offset selects M1 (0) or M2 (0x800). + struct AxisState { + detail::mcp266::AxisObjects objects; + Ds402Drive drive; + const char *name; + + AxisState(CanopenClient &client, const detail::mcp266::AxisObjects &objs, const char *n, + const Config &cfg) + : objects(objs) + , drive(client, Ds402Drive::Config{.state_timeout = cfg.state_timeout, + .poll_period = cfg.poll_period, + .object_offset = objs.object_offset, + .log_level = cfg.log_level}) + , name(n) {} + }; + + AxisState &axis_state(Axis axis) { return axis == Axis::M1 ? m1_ : m2_; } + + /// Write the axis mode of operation directly (the MCP does not echo the + /// requested mode in 0x6061, so Ds402Drive::set_mode() -- which verifies the + /// display -- would time out), clear any fault, and walk to Operation + /// Enabled. + bool enable(AxisState &a, Ds402Drive::OperatingMode mode, std::error_code &ec) { + const uint16_t mode_obj = static_cast(0x6060 + a.objects.object_offset); + if (!client_.write_i8(mode_obj, 0, static_cast(mode), ec)) { + logger_.error("{}: failed to set mode: {}", a.name, ec.message()); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + const auto state = a.drive.get_state(ec); + if (ec) { + return false; + } + if (state == Ds402Drive::State::Fault || state == Ds402Drive::State::FaultReactionActive) { + if (!a.drive.fault_reset(ec)) { + return false; + } + } + return a.drive.enable_operation(ec); + } + + CanopenClient &client_; + AxisState m1_; + AxisState m2_; +}; + +} // namespace espp diff --git a/components/mcp266/test/mcp266_host_test.cpp b/components/mcp266/test/mcp266_host_test.cpp new file mode 100644 index 0000000000..1ba8a0bb21 --- /dev/null +++ b/components/mcp266/test/mcp266_host_test.cpp @@ -0,0 +1,92 @@ +// Host-buildable unit tests for the MCP266 CANopen mapping core. Build & run: +// c++ -std=c++20 -I../include mcp266_host_test.cpp -o test && ./test +// +// These tests exercise detail/mcp266_core.hpp directly (no ESP-IDF headers). +// The object addresses were verified against a live MCP266's SDO object +// dictionary; the manufacturer region mirrors the packet-serial command set at +// index 0x2000 + command number. + +#include +#include +#include + +#include "detail/mcp266_core.hpp" + +using namespace espp::detail::mcp266; + +static int g_failures = 0; +#define CHECK(cond) \ + do { \ + if (!(cond)) { \ + std::printf(" FAIL: %s (line %d)\n", #cond, __LINE__); \ + ++g_failures; \ + } \ + } while (0) + +static void test_command_object() { + std::printf("test_command_object\n"); + // command N mirrors to 0x2000 + N (verified anchors) + CHECK(command_object(61) == 0x203D); // set M1 position PID + CHECK(command_object(62) == 0x203E); // set M2 position PID + CHECK(command_object(63) == 0x203F); // read M1 position PID + CHECK(command_object(64) == 0x2040); // read M2 position PID + CHECK(command_object(32) == 0x2020); // drive M1 duty + CHECK(command_object(35) == 0x2023); // drive M1 speed + CHECK(command_object(24) == 0x2018); // read main battery + CHECK(command_object(82) == 0x2052); // read temperature + CHECK(command_object(200) == 0x20C8); // e-stop reset + CHECK(kMainBatteryObject == 0x2018); + CHECK(kTemperatureObject == 0x2052); + CHECK(kEStopResetObject == 0x20C8); +} + +static void test_axis_objects() { + std::printf("test_axis_objects\n"); + const auto m1 = axis_m1(); + const auto m2 = axis_m2(); + // M1 at the standard offset, M2 mirrored at +0x800 + CHECK(m1.object_offset == 0x000); + CHECK(m2.object_offset == 0x800); + // per-axis command objects follow the 0x2000 + command mapping (cmd n / n+1) + CHECK(m1.position_pid_set == 0x203D); + CHECK(m1.position_pid_get == 0x203F); + CHECK(m1.drive_duty == 0x2020); + CHECK(m1.drive_speed == 0x2023); + CHECK(m2.position_pid_set == 0x203E); + CHECK(m2.position_pid_get == 0x2040); + CHECK(m2.drive_duty == 0x2021); + CHECK(m2.drive_speed == 0x2024); + // the CiA 402 offset applied to a device-profile object selects the axis + CHECK(static_cast(0x6040 + m2.object_offset) == 0x6840); // controlword + CHECK(static_cast(0x607A + m2.object_offset) == 0x687A); // target position +} + +static void test_position_pid_remap() { + std::printf("test_position_pid_remap\n"); + // readback order [P, I, D, MaxI, Deadzone, MinPos, MaxPos] -> + // setter order [D, P, I, MaxI, Deadzone, MinPos, MaxPos] + const std::array readback{100, 20, 3, 4, 5, -1000, 1000}; + const auto setter = position_pid_readback_to_setter(readback); + CHECK(setter[0] == 3); // D + CHECK(setter[1] == 100); // P + CHECK(setter[2] == 20); // I + CHECK(setter[3] == 4); // MaxI + CHECK(setter[4] == 5); // Deadzone + CHECK(setter[5] == -1000); // MinPos + CHECK(setter[6] == 1000); // MaxPos + // constexpr-evaluable + static_assert(position_pid_readback_to_setter({7, 8, 9, 0, 0, 0, 0})[0] == 9); + static_assert(position_pid_readback_to_setter({7, 8, 9, 0, 0, 0, 0})[1] == 7); +} + +int main() { + test_command_object(); + test_axis_objects(); + test_position_pid_remap(); + if (g_failures) { + std::printf("%d FAILURES\n", g_failures); + return 1; + } + std::printf("ALL PASSED\n"); + return 0; +} diff --git a/doc/Doxyfile b/doc/Doxyfile index f0604ddc88..ba074b4d14 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -81,6 +81,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/at581x/example/main/at581x_example.cpp \ $(PROJECT_PATH)/components/aw9523/example/main/aw9523_example.cpp \ $(PROJECT_PATH)/components/basicmicro/example/main/basicmicro_example.cpp \ + $(PROJECT_PATH)/components/mcp266/example/main/mcp266_example.cpp \ $(PROJECT_PATH)/components/bdc_driver/example/main/bdc_driver_example.cpp \ $(PROJECT_PATH)/components/lp5817/example/main/lp5817_example.cpp \ $(PROJECT_PATH)/components/binary-log/example/main/binary_log_example.cpp \ @@ -228,6 +229,8 @@ INPUT = \ $(PROJECT_PATH)/components/base_peripheral/include/base_peripheral.hpp \ $(PROJECT_PATH)/components/basicmicro/include/basicmicro.hpp \ $(PROJECT_PATH)/components/basicmicro/include/detail/basicmicro_core.hpp \ + $(PROJECT_PATH)/components/mcp266/include/mcp266.hpp \ + $(PROJECT_PATH)/components/mcp266/include/detail/mcp266_core.hpp \ $(PROJECT_PATH)/components/bdc_driver/include/bdc_driver.hpp \ $(PROJECT_PATH)/components/binary-log/include/binary-log.hpp \ $(PROJECT_PATH)/components/ble_gatt_server/include/battery_service.hpp \ diff --git a/doc/en/motor_control/index.rst b/doc/en/motor_control/index.rst index 2666951a22..209068d8bc 100644 --- a/doc/en/motor_control/index.rst +++ b/doc/en/motor_control/index.rst @@ -11,6 +11,7 @@ Motor-control algorithms and controller interfaces. See also the pid adrc basicmicro + mcp266 odrive_ascii odrive_native trajectory_planner diff --git a/doc/en/motor_control/mcp266.rst b/doc/en/motor_control/mcp266.rst new file mode 100644 index 0000000000..3dfef5c57f --- /dev/null +++ b/doc/en/motor_control/mcp266.rst @@ -0,0 +1,65 @@ +MCP266 CANopen Motor Controller Component +========================================= + +Overview +-------- + +``espp::Mcp266`` is a dual-channel controller for a Basicmicro **MCP266** +(RoboClaw family) brushed DC motor driver over **CANopen**. It is layered on +``espp::CanopenClient`` (like ``espp::Ds402Drive``), so it is transport-agnostic: +the application owns the CAN transport, feeds received frames to the client's +``process_frame()``, and the client's node id selects the MCP266. + +Both motor channels (``M1``, ``M2``) are driven symmetrically. M2's CiA 402 +objects mirror M1's at ``+0x800``, handled through ``Ds402Drive``'s object +offset. The reverse-engineered object mapping lives in ``espp::detail`` inside +``include/detail/mcp266_core.hpp``, a host-buildable core that depends only on +the C++20 standard library and is unit-tested off-target. + +What works, and what does not +----------------------------- + +**Position control** uses the standard CiA 402 profile position mode +(``move_to_position``) and is the supported, validated capability. It needs the +position loop configured first (``configure_position_loop``). + +**Velocity / duty control is not functional** on the MCP266 firmware tested. +Both the standard target objects and the manufacturer speed/duty command mirror +are accepted by the drive but leave the velocity generator idle even with the +drive in Operation Enabled. Supported-drive-modes (``0x6502``) advertises only +the cyclic-sync modes, so velocity likely requires csv mode with cyclic +SYNC/PDO updates, which is undocumented for this device. ``drive_speed`` / +``drive_duty`` are implemented but are currently a no-op for motion. + +Device specifics +---------------- + +The MCP266's control-loop parameters are **not** standard CiA 402 objects. The +MCP mirrors its packet-serial command set into the manufacturer region of the +object dictionary at index ``0x2000 + command number``. This component uses +that to configure the position PID (commands 61-64), issue the manufacturer +speed/duty commands (32/33, 35/36), and read telemetry (main battery 24, +temperature 82). + +Two device quirks are handled by ``configure_position_loop()``, which must be +called once per boot (the MCP reverts to its EEPROM configuration on power-up): + +- The position PID's ``MinPos``/``MaxPos`` clamp defaults to ``[0, 0]``, which + forces every position target to zero — it is widened here. +- The setter (commands 61/62) uses field order ``D, P, I`` while the readback + (63/64) uses ``P, I, D``, so a naive read-modify-write of the record would + move ``P`` into the ``D`` slot and zero ``P``. The correct field shuffle (and + seeding a non-zero ``P`` when the record has none) is done here. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + mcp266_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/mcp266.inc diff --git a/doc/en/motor_control/mcp266_example.md b/doc/en/motor_control/mcp266_example.md new file mode 100644 index 0000000000..9e1e153560 --- /dev/null +++ b/doc/en/motor_control/mcp266_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/mcp266/example/README.md +```