diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index f95d008263..9b2e840048 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -64,6 +64,8 @@ jobs: target: esp32s3 - path: 'components/aw9523/example' target: esp32 + - path: 'components/basicmicro/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 5fe19fbcc9..8b2dff400f 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -49,6 +49,7 @@ jobs: components/aw9523 components/base_component components/base_peripheral + components/basicmicro components/bdc_driver components/binary-log components/bldc_current_sense diff --git a/components/basicmicro/CMakeLists.txt b/components/basicmicro/CMakeLists.txt new file mode 100644 index 0000000000..6e5a3f5d41 --- /dev/null +++ b/components/basicmicro/CMakeLists.txt @@ -0,0 +1,7 @@ +# NOTE: like odrive_native, this component's detail/ lives INSIDE include/ +# (include/detail/*.hpp) so that the host-buildable wire core can be included +# as `#include "detail/basicmicro_core.hpp"` by consumers and by the host test. +idf_component_register( + INCLUDE_DIRS "include" + REQUIRES base_component +) diff --git a/components/basicmicro/README.md b/components/basicmicro/README.md new file mode 100644 index 0000000000..a80d0b4128 --- /dev/null +++ b/components/basicmicro/README.md @@ -0,0 +1,78 @@ +# Basicmicro (MCP / RoboClaw) Motor Controller Component + +[![Badge](https://components.espressif.com/components/espp/basicmicro/badge.svg)](https://components.espressif.com/components/espp/basicmicro) + +`espp::Basicmicro` is a driver for Basicmicro **MCP236 / MCP266** (and other +RoboClaw-family) brushed DC motor controllers speaking their **PACKET SERIAL** +protocol, typically over UART. + +The component is transport-agnostic: it performs no I/O itself and instead +calls user-provided `write` / `read` functions for each transaction, so it +works over a UART driver, USB CDC, an RS-232 adapter, etc. All wire-format +logic (CRC16, packet building, reply validation, big-endian codecs) lives in +`include/detail/basicmicro_core.hpp`, a host-buildable core with zero ESP +dependencies that is unit-tested off-target. + + +**Table of Contents** + +- [Basicmicro (MCP / RoboClaw) Motor Controller Component](#basicmicro-mcp--roboclaw-motor-controller-component) + - [Features](#features) + - [Protocol](#protocol) + - [API](#api) + - [Example](#example) + - [Testing](#testing) + + + +## Features + +- Duty-cycle drive (commands 32/33/34) and closed-loop speed drive in + quadrature pulses per second (35/36/37), with optional acceleration ramps + (38/39/40) +- Buffered speed / accel / distance motion commands (41-46) plus buffer-state + readback (47) +- Encoder support: counts (16/17/78), speeds (18/19/79), reset (20) and + encoder-mode readback (91) +- Velocity PID get/set with automatic 16.16 fixed-point conversion (28/29, + 55/56) +- Telemetry: firmware version (21), main/logic battery voltage (24/25), motor + currents (49), motor PWMs (48), board temperatures (82/83) and unit status + (90) +- Management: write settings to EEPROM (94), E-Stop reset (200) +- No exceptions; all methods report errors via `std::error_code` +- Thread-safe: each transaction (request + ACK/reply) is serialized by an + internal mutex + +## Protocol + +The packet serial protocol (MCP Series User Manual, section 2.2): + +- Write commands send `[Address, Command, Data..., CRC16]`; the controller + replies with a single `0xFF` ACK byte only when the packet was valid. +- Read commands send `[Address, Command]` (no CRC); the reply is the data + followed by a CRC16 computed over the *sent* address + command bytes plus the + reply data. +- All multi-byte values (including the CRC) are big-endian ("high byte first"). +- CRC16 is CRC-16/XMODEM (poly `0x1021`, init `0`, non-reflected). +- Error recovery: the controller discards a partial packet after a 10 ms + inter-byte gap, so the configured receive timeout (>= 10 ms, default 20 ms) + doubles as the recovery mechanism. + +## API + +See the [documentation](https://esp-cpp.github.io/espp/motor_control/basicmicro.html). + +## Example + +The [example](./example) shows how to wire the driver to the ESP-IDF UART +driver, read the firmware version / battery voltage / status, run a gentle +duty-cycle ramp on motor 1 with encoder readback, and stop. + +## Testing + +The wire core is host-buildable and unit-tested without ESP-IDF: + +```sh +c++ -std=c++20 -I include -o /tmp/bm_test test/basicmicro_host_test.cpp && /tmp/bm_test +``` diff --git a/components/basicmicro/example/CMakeLists.txt b/components/basicmicro/example/CMakeLists.txt new file mode 100644 index 0000000000..33f1050d56 --- /dev/null +++ b/components/basicmicro/example/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "${CMAKE_CURRENT_LIST_DIR}/../.." +) + +set( + COMPONENTS + "main esptool_py esp_driver_uart basicmicro" + CACHE STRING + "List of components to include" + ) + +project(basicmicro_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/basicmicro/example/README.md b/components/basicmicro/example/README.md new file mode 100644 index 0000000000..bc34f79c6e --- /dev/null +++ b/components/basicmicro/example/README.md @@ -0,0 +1,55 @@ +# Basicmicro (MCP / RoboClaw) Example + +This example demonstrates how to use the `espp::Basicmicro` component to talk +to a Basicmicro MCP236 / MCP266 (or RoboClaw-family) motor controller over +UART using the packet serial protocol. It: + +1. reads the firmware version, main battery voltage and unit status, +2. resets the encoders, +3. runs a gentle duty-cycle ramp on motor 1 while reading back the encoder + count and speed, then ramps back down and stops, and +4. periodically logs the motor currents and board temperature. + + +**Table of Contents** + +- [Basicmicro (MCP / RoboClaw) Example](#basicmicro-mcp--roboclaw-example) + - [Requirements](#requirements) + - [Hardware](#hardware) + - [Build](#build) + - [Flash and Monitor](#flash-and-monitor) + + + +## Requirements + +- ESP-IDF installed and `get_idf` available in your shell +- A Basicmicro MCP / RoboClaw controller configured for **packet serial** mode + at 38400 baud, address `0x80` (the defaults used by this example) + +## Hardware + +| ESP32 (this example) | MCP / RoboClaw | +|----------------------|----------------| +| GPIO 17 (UART1 TX) | S1 (RX) | +| GPIO 16 (UART1 RX) | S2 (TX) | +| GND | GND | + +Adjust the pins / port / baud rate at the top of +[basicmicro_example.cpp](./main/basicmicro_example.cpp) to match your wiring. + +## Build + +```sh +# From repo root +cd components/basicmicro/example +get_idf +idf.py set-target esp32 +idf.py build +``` + +## Flash and Monitor + +```sh +idf.py flash monitor +``` diff --git a/components/basicmicro/example/main/CMakeLists.txt b/components/basicmicro/example/main/CMakeLists.txt new file mode 100644 index 0000000000..4b68de3f00 --- /dev/null +++ b/components/basicmicro/example/main/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRC_DIRS "." + INCLUDE_DIRS "." +) diff --git a/components/basicmicro/example/main/basicmicro_example.cpp b/components/basicmicro/example/main/basicmicro_example.cpp new file mode 100644 index 0000000000..18ebd46188 --- /dev/null +++ b/components/basicmicro/example/main/basicmicro_example.cpp @@ -0,0 +1,122 @@ +#include +#include + +#include "driver/uart.h" + +#include "basicmicro.hpp" +#include "logger.hpp" + +using namespace std::chrono_literals; + +// UART wiring for the MCP: it defaults to packet serial mode at 38400 baud; S1 +// (controller RX) goes to our TX pin and S2 (controller TX) goes to our RX pin. +// File-scope so the captureless transport lambdas below can reference them +// without any capture-semantics ambiguity. +static constexpr uart_port_t uart_port = UART_NUM_1; +static constexpr int uart_tx_pin = 17; // -> MCP S1 +static constexpr int uart_rx_pin = 16; // <- MCP S2 +static constexpr int uart_baud = 38400; + +extern "C" void app_main(void) { + espp::Logger logger({.tag = "Basicmicro Example", .level = espp::Logger::Verbosity::INFO}); + logger.info("Starting basicmicro example"); + + //! [basicmicro example] + + uart_config_t uart_config = {}; + uart_config.baud_rate = uart_baud; + uart_config.data_bits = UART_DATA_8_BITS; + uart_config.parity = UART_PARITY_DISABLE; + uart_config.stop_bits = UART_STOP_BITS_1; + uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE; + uart_config.source_clk = UART_SCLK_DEFAULT; + ESP_ERROR_CHECK(uart_driver_install(uart_port, 256, 0, 0, nullptr, 0)); + ESP_ERROR_CHECK(uart_param_config(uart_port, &uart_config)); + ESP_ERROR_CHECK( + uart_set_pin(uart_port, uart_tx_pin, uart_rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE)); + + espp::Basicmicro mcp({ + .address = 0x80, // default packet serial address (0x80 - 0x87) + .write = + [](std::span data) { + const int written = uart_write_bytes( + uart_port, reinterpret_cast(data.data()), data.size()); + return written == static_cast(data.size()); + }, + .read = [](std::span data, std::chrono::milliseconds timeout) -> size_t { + const int read = + uart_read_bytes(uart_port, data.data(), data.size(), pdMS_TO_TICKS(timeout.count())); + return read < 0 ? 0 : static_cast(read); + }, + // must be >= 10 ms: a 10 ms quiet gap is also what clears the + // controller's packet buffer after a communication error + .timeout = 20ms, + .log_level = espp::Logger::Verbosity::INFO, + }); + + std::error_code ec; + + // identify the controller + std::string version; + if (mcp.read_firmware_version(version, ec)) { + logger.info("Firmware version: '{}'", version); + } else { + logger.error("Could not read firmware version: {}", ec.message()); + logger.error("Is the controller connected, powered, and in packet serial mode?"); + } + + float volts{0}; + if (mcp.read_main_battery_voltage(volts, ec)) + logger.info("Main battery: {:.1f} V", volts); + + uint32_t status{0}; + if (mcp.read_status(status, ec)) + logger.info("Status: 0x{:08X}{}", status, status == 0 ? " (normal)" : ""); + + // start from a known encoder state + if (mcp.reset_encoders(ec)) + logger.info("Encoders reset"); + + // gentle speed ramp on M1 (up to ~12.5% duty) with encoder readback, then + // back down to a stop. Duty-cycle drive works without a tuned velocity PID; + // if your encoders + PID are configured, try drive_m1_speed() instead. + static constexpr int16_t max_duty = 4096; // of 32767 + static constexpr int16_t step = 512; + for (int16_t duty = 0; duty <= max_duty; duty = static_cast(duty + step)) { + if (!mcp.drive_m1_duty(duty, ec)) { + logger.error("drive_m1_duty({}) failed: {}", duty, ec.message()); + break; + } + std::this_thread::sleep_for(250ms); + uint32_t count{0}; + uint8_t enc_status{0}; + int32_t speed{0}; + uint8_t direction{0}; + if (mcp.read_encoder_m1(count, enc_status, ec) && + mcp.read_encoder_speed_m1(speed, direction, ec)) { + logger.info("duty {:5d}: encoder count = {:10d}, speed = {} pulses/s ({})", duty, count, + speed, direction ? "backward" : "forward"); + } + } + for (int16_t duty = max_duty; duty >= 0; duty = static_cast(duty - step)) { + if (!mcp.drive_m1_duty(duty, ec)) + break; + std::this_thread::sleep_for(100ms); + } + + // make sure the motor is stopped + if (mcp.drive_m1_duty(0, ec)) + logger.info("Motor stopped"); + + //! [basicmicro example] + + // periodically log some telemetry + while (true) { + float amps_m1{0}, amps_m2{0}, temperature{0}; + if (mcp.read_currents(amps_m1, amps_m2, ec)) + logger.info("Currents: M1 = {:.2f} A, M2 = {:.2f} A", amps_m1, amps_m2); + if (mcp.read_temperature(temperature, ec)) + logger.info("Temperature: {:.1f} C", temperature); + std::this_thread::sleep_for(5s); + } +} diff --git a/components/basicmicro/example/sdkconfig.defaults b/components/basicmicro/example/sdkconfig.defaults new file mode 100644 index 0000000000..c3667f3e33 --- /dev/null +++ b/components/basicmicro/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/basicmicro/idf_component.yml b/components/basicmicro/idf_component.yml new file mode 100644 index 0000000000..912db0858e --- /dev/null +++ b/components/basicmicro/idf_component.yml @@ -0,0 +1,25 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "Driver for Basicmicro MCP236 / MCP266 (and RoboClaw-family) brushed DC motor controllers using the packet serial protocol over UART" +url: "https://github.com/esp-cpp/espp/tree/main/components/basicmicro" +repository: "https://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/motor_control/basicmicro.html" +examples: + - path: example +tags: + - cpp + - Component + - Basicmicro + - RoboClaw + - MCP236 + - MCP266 + - Motor + - Encoder + - UART + - Serial +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' diff --git a/components/basicmicro/include/basicmicro.hpp b/components/basicmicro/include/basicmicro.hpp new file mode 100644 index 0000000000..a00cac984b --- /dev/null +++ b/components/basicmicro/include/basicmicro.hpp @@ -0,0 +1,952 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "base_component.hpp" +#include "detail/basicmicro_core.hpp" + +namespace espp { + +/** + * @brief Driver for Basicmicro (MCP236 / MCP266 and RoboClaw-family) brushed + * DC motor controllers speaking the PACKET SERIAL protocol, typically + * over UART. + * + * The component is transport-agnostic: it performs no I/O itself and instead + * calls the user-provided write / read functions for each transaction, so it + * works over a UART driver, USB CDC, RS-232 adapter, etc. Wire-format logic + * (CRC16, packet building, reply validation, big-endian codecs) lives in + * `include/detail/basicmicro_core.hpp`, a host-buildable core with zero ESP + * dependencies. + * + * Protocol summary (MCP Series User Manual section 2.2): + * - Write commands send [Address, Command, Data..., CRC16] and the controller + * replies with a single 0xFF ACK byte only when the packet was valid. + * - Read commands send [Address, Command] (no CRC) and the controller replies + * with the data followed by a CRC16 seeded with the sent address + command. + * - Error recovery: a >=10 ms gap between bytes makes the controller discard + * any partial packet, so the configured receive timeout (>=10 ms, default + * 20 ms) doubles as the recovery mechanism — after a timed-out transaction + * the controller's packet buffer has already cleared itself and the next + * packet starts fresh. + * + * All methods report errors via a std::error_code out-parameter (no + * exceptions) and return true on success. + * + * @note Thread safety: every public method runs one complete transaction + * (write the request, then read the ACK / reply) while holding an + * internal mutex, so concurrent calls from multiple tasks serialize + * cleanly and replies cannot interleave. The user-provided read / write + * functions ARE called with that mutex held — this is intentional, since + * the transaction is precisely the I/O — so they must not call back into + * this component. + * + * \section basicmicro_ex1 Basicmicro Example + * \snippet basicmicro_example.cpp basicmicro example + */ +class Basicmicro : public BaseComponent { +public: + /// Command bytes (verified against the MCP Series User Manual). + using Command = detail::BasicmicroCommand; + /// Status bit masks returned by read_status() (manual command 90). + using Status = detail::BasicmicroStatus; + + /// Function used to transmit a complete packet to the controller. + /// Should return true when all bytes were written. + typedef std::function data)> write_fn; + + /// Function used to receive reply bytes from the controller. Should block + /// until at least one byte is available or the timeout expires, and return + /// the number of bytes actually read into the span (0 on timeout). + typedef std::function data, std::chrono::milliseconds timeout)> read_fn; + + /// Configuration for the Basicmicro driver. + struct Config { + uint8_t address{0x80}; /**< Packet serial address of the controller (0x80 - 0x87). */ + write_fn write; /**< Function to write bytes to the controller. */ + read_fn read; /**< Function to read bytes from the controller. */ + std::chrono::milliseconds timeout{ + 20}; /**< Total receive timeout per transaction. Must be >= 10 ms: the controller + discards a partial packet after a 10 ms inter-byte gap (manual section + 2.2.4), so waiting at least that long guarantees its packet buffer has + cleared before the next transaction. */ + espp::Logger::Verbosity log_level{espp::Logger::Verbosity::WARN}; /**< Logger verbosity. */ + }; + + /** + * @brief Create a Basicmicro driver. + * @param config Configuration parameters. + * @note The documented contracts are enforced here: a timeout below the + * protocol's 10 ms packet-clear window is clamped up to 10 ms, and an + * address outside 0x80-0x87 is clamped into the valid range (both with + * a warning) rather than silently violating the wire protocol. + */ + explicit Basicmicro(const Config &config) + : BaseComponent("Basicmicro", config.log_level) + , config_(config) { + if (config_.timeout < std::chrono::milliseconds(detail::kBasicmicroPacketTimeoutMs)) { + logger_.warn("timeout {} ms is below the protocol's {} ms packet-clear window; clamping", + config_.timeout.count(), detail::kBasicmicroPacketTimeoutMs); + config_.timeout = std::chrono::milliseconds(detail::kBasicmicroPacketTimeoutMs); + } + if (config_.address < detail::kBasicmicroMinAddress || + config_.address > detail::kBasicmicroMaxAddress) { + const uint8_t clamped = + std::clamp(config_.address, detail::kBasicmicroMinAddress, detail::kBasicmicroMaxAddress); + logger_.warn("address {:#04x} is outside the valid packet-serial range [{:#04x}, {:#04x}]; " + "clamping to {:#04x}", + config_.address, detail::kBasicmicroMinAddress, detail::kBasicmicroMaxAddress, + clamped); + config_.address = clamped; + } + } + + // ------------------------- duty-cycle drive ------------------------------ + + /** + * @brief Drive motor 1 with a signed duty cycle (command 32). + * @param duty Signed duty, -32767 to +32767 (= -100% to +100%). + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m1_duty(int16_t duty, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i16_be(payload, duty); + return write_command(Command::DriveM1SignedDuty, payload, ec); + } + + /** + * @brief Drive motor 2 with a signed duty cycle (command 33). + * @param duty Signed duty, -32767 to +32767 (= -100% to +100%). + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m2_duty(int16_t duty, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i16_be(payload, duty); + return write_command(Command::DriveM2SignedDuty, payload, ec); + } + + /** + * @brief Drive both motors with signed duty cycles (command 34). + * @param duty_m1 Signed duty for motor 1, -32767 to +32767. + * @param duty_m2 Signed duty for motor 2, -32767 to +32767. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_duty(int16_t duty_m1, int16_t duty_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i16_be(payload, duty_m1); + detail::append_i16_be(payload, duty_m2); + return write_command(Command::DriveM1M2SignedDuty, payload, ec); + } + + // ------------------------ closed-loop speed drive ------------------------ + + /** + * @brief Drive motor 1 at a signed speed in quadrature pulses per second + * (command 35). Requires an encoder and tuned velocity PID. + * @param qpps Signed speed in quad pulses per second. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m1_speed(int32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps); + return write_command(Command::DriveM1SignedSpeed, payload, ec); + } + + /** + * @brief Drive motor 2 at a signed speed in quadrature pulses per second + * (command 36). Requires an encoder and tuned velocity PID. + * @param qpps Signed speed in quad pulses per second. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m2_speed(int32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps); + return write_command(Command::DriveM2SignedSpeed, payload, ec); + } + + /** + * @brief Drive both motors at signed speeds in quadrature pulses per second + * (command 37). + * @param qpps_m1 Signed speed for motor 1 in quad pulses per second. + * @param qpps_m2 Signed speed for motor 2 in quad pulses per second. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_speed(int32_t qpps_m1, int32_t qpps_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps_m1); + detail::append_i32_be(payload, qpps_m2); + return write_command(Command::DriveM1M2SignedSpeed, payload, ec); + } + + /** + * @brief Drive motor 1 at a signed speed with an acceleration ramp + * (command 38). + * @param accel Acceleration in qpps per second (unsigned). + * @param qpps Signed target speed in quad pulses per second. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m1_speed_accel(uint32_t accel, int32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps); + return write_command(Command::DriveM1SignedSpeedAccel, payload, ec); + } + + /** + * @brief Drive motor 2 at a signed speed with an acceleration ramp + * (command 39). + * @param accel Acceleration in qpps per second (unsigned). + * @param qpps Signed target speed in quad pulses per second. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_m2_speed_accel(uint32_t accel, int32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps); + return write_command(Command::DriveM2SignedSpeedAccel, payload, ec); + } + + /** + * @brief Drive both motors at signed speeds with a shared acceleration ramp + * (command 40). + * @param accel Acceleration in qpps per second (unsigned, applies to both). + * @param qpps_m1 Signed target speed for motor 1. + * @param qpps_m2 Signed target speed for motor 2. + * @param ec Set on failure. + * @return True on success. + */ + bool drive_speed_accel(uint32_t accel, int32_t qpps_m1, int32_t qpps_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps_m1); + detail::append_i32_be(payload, qpps_m2); + return write_command(Command::DriveM1M2SignedSpeedAccel, payload, ec); + } + + // -------------------------- buffered motion ------------------------------ + + /** + * @brief Buffered drive of motor 1 with signed speed and distance + * (command 41). + * @param qpps Signed speed in quad pulses per second. + * @param distance Distance in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it (up to 64 + * commands per motor buffer). + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_m1_speed_distance(int32_t qpps, uint32_t distance, bool immediate, + std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps); + detail::append_u32_be(payload, distance); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM1SpeedDistance, payload, ec); + } + + /** + * @brief Buffered drive of motor 2 with signed speed and distance + * (command 42). + * @param qpps Signed speed in quad pulses per second. + * @param distance Distance in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it. + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_m2_speed_distance(int32_t qpps, uint32_t distance, bool immediate, + std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps); + detail::append_u32_be(payload, distance); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM2SpeedDistance, payload, ec); + } + + /** + * @brief Buffered drive of both motors with signed speeds and distances + * (command 43). + * @param qpps_m1 Signed speed for motor 1 in quad pulses per second. + * @param distance_m1 Distance for motor 1 in quad pulses (unsigned). + * @param qpps_m2 Signed speed for motor 2 in quad pulses per second. + * @param distance_m2 Distance for motor 2 in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it. + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_speed_distance(int32_t qpps_m1, uint32_t distance_m1, int32_t qpps_m2, + uint32_t distance_m2, bool immediate, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_i32_be(payload, qpps_m1); + detail::append_u32_be(payload, distance_m1); + detail::append_i32_be(payload, qpps_m2); + detail::append_u32_be(payload, distance_m2); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM1M2SpeedDistance, payload, ec); + } + + /** + * @brief Buffered drive of motor 1 with acceleration, signed speed and + * distance (command 44). + * @param accel Acceleration in qpps per second (unsigned). + * @param qpps Signed speed in quad pulses per second. + * @param distance Distance in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it. + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_m1_speed_accel_distance(uint32_t accel, int32_t qpps, uint32_t distance, + bool immediate, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps); + detail::append_u32_be(payload, distance); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM1SpeedAccelDistance, payload, ec); + } + + /** + * @brief Buffered drive of motor 2 with acceleration, signed speed and + * distance (command 45). + * @param accel Acceleration in qpps per second (unsigned). + * @param qpps Signed speed in quad pulses per second. + * @param distance Distance in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it. + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_m2_speed_accel_distance(uint32_t accel, int32_t qpps, uint32_t distance, + bool immediate, std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps); + detail::append_u32_be(payload, distance); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM2SpeedAccelDistance, payload, ec); + } + + /** + * @brief Buffered drive of both motors with a shared acceleration, signed + * speeds and distances (command 46). + * @param accel Acceleration in qpps per second (unsigned, applies to both). + * @param qpps_m1 Signed speed for motor 1 in quad pulses per second. + * @param distance_m1 Distance for motor 1 in quad pulses (unsigned). + * @param qpps_m2 Signed speed for motor 2 in quad pulses per second. + * @param distance_m2 Distance for motor 2 in quad pulses (unsigned). + * @param immediate If true, stop the currently-executing command, flush the + * buffer and run this command now; if false, queue it. + * @param ec Set on failure. + * @return True on success. + */ + bool buffered_drive_speed_accel_distance(uint32_t accel, int32_t qpps_m1, uint32_t distance_m1, + int32_t qpps_m2, uint32_t distance_m2, bool immediate, + std::error_code &ec) { + std::scoped_lock lk(mutex_); + std::vector payload; + detail::append_u32_be(payload, accel); + detail::append_i32_be(payload, qpps_m1); + detail::append_u32_be(payload, distance_m1); + detail::append_i32_be(payload, qpps_m2); + detail::append_u32_be(payload, distance_m2); + detail::append_u8(payload, immediate ? 1 : 0); + return write_command(Command::BufferedM1M2SpeedAccelDistance, payload, ec); + } + + /** + * @brief Read how many buffered commands are waiting per motor (command 47). + * @param buffer_m1 Motor 1 buffer state: 0x80 = buffer empty / last command + * finished, 0 = last command is executing, 1-0x3F = commands waiting. + * @param buffer_m2 Motor 2 buffer state (same encoding). + * @param ec Set on failure. + * @return True on success. + */ + bool read_buffer_lengths(uint8_t &buffer_m1, uint8_t &buffer_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[2] = {}; + if (!read_command(Command::ReadBufferLengths, data, ec)) + return false; + buffer_m1 = data[0]; + buffer_m2 = data[1]; + return true; + } + + // ------------------------------ encoders --------------------------------- + + /** + * @brief Read the motor 1 encoder count / position (command 16). + * @param count Encoder count (quadrature: full 32-bit range; absolute: + * 0-4095). + * @param status Status bits: bit0 = underflow occurred (cleared on read), + * bit1 = direction (0 forward, 1 backward), bit2 = overflow occurred + * (cleared on read). + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoder_m1(uint32_t &count, uint8_t &status, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_encoder(Command::ReadEncoderM1, count, status, ec); + } + + /** + * @brief Read the motor 2 encoder count / position (command 17). + * @param count Encoder count (quadrature: full 32-bit range; absolute: + * 0-4095). + * @param status Status bits: bit0 = underflow occurred (cleared on read), + * bit1 = direction (0 forward, 1 backward), bit2 = overflow occurred + * (cleared on read). + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoder_m2(uint32_t &count, uint8_t &status, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_encoder(Command::ReadEncoderM2, count, status, ec); + } + + /** + * @brief Read both encoder counters in one transaction (command 78). + * @param count_m1 Motor 1 encoder count. + * @param count_m2 Motor 2 encoder count. + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoders(uint32_t &count_m1, uint32_t &count_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[8] = {}; + if (!read_command(Command::ReadEncoderCounters, data, ec)) + return false; + count_m1 = detail::read_u32_be(data, 0); + count_m2 = detail::read_u32_be(data, 4); + return true; + } + + /** + * @brief Reset both quadrature encoder counters to zero (command 20). + * @param ec Set on failure. + * @return True on success. + */ + bool reset_encoders(std::error_code &ec) { + std::scoped_lock lk(mutex_); + return write_command(Command::ResetEncoders, {}, ec); + } + + /** + * @brief Read the motor 1 encoder speed in pulses per second (command 18). + * @param qpps Speed in pulses per second (as reported by the controller). + * @param direction 0 = forward, 1 = backward. + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoder_speed_m1(int32_t &qpps, uint8_t &direction, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_speed(Command::ReadEncoderSpeedM1, qpps, direction, ec); + } + + /** + * @brief Read the motor 2 encoder speed in pulses per second (command 19). + * @param qpps Speed in pulses per second (as reported by the controller). + * @param direction 0 = forward, 1 = backward. + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoder_speed_m2(int32_t &qpps, uint8_t &direction, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_speed(Command::ReadEncoderSpeedM2, qpps, direction, ec); + } + + /** + * @brief Read both instantaneous speeds (counts per second over the last + * 1/300th of a second) in one transaction (command 79). + * @param qpps_m1 Motor 1 instantaneous speed. + * @param qpps_m2 Motor 2 instantaneous speed. + * @param ec Set on failure. + * @return True on success. + */ + bool read_ispeeds(int32_t &qpps_m1, int32_t &qpps_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[8] = {}; + if (!read_command(Command::ReadISpeedCounters, data, ec)) + return false; + qpps_m1 = detail::read_i32_be(data, 0); + qpps_m2 = detail::read_i32_be(data, 4); + return true; + } + + /** + * @brief Read the encoder modes / pin assignments for both motors + * (command 91). + * @param mode_m1 Motor 1 encoder mode. + * @param mode_m2 Motor 2 encoder mode. + * @param ec Set on failure. + * @return True on success. + */ + bool read_encoder_modes(uint8_t &mode_m1, uint8_t &mode_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[2] = {}; + if (!read_command(Command::ReadEncoderModes, data, ec)) + return false; + mode_m1 = data[0]; + mode_m2 = data[1]; + return true; + } + + // ----------------------------- velocity PID ------------------------------ + + /** + * @brief Set the motor 1 velocity PID constants and QPPS (command 28). + * + * Gains are converted to the controller's 16.16 fixed-point representation + * (value * 65536); the controller defaults correspond to P=1.0, I=0.5, + * D=0.25, QPPS=44000. + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param qpps Encoder speed (quad pulses per second) at 100% motor power. + * @param ec Set on failure. + * @return True on success. + */ + bool set_velocity_pid_m1(float p, float i, float d, uint32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return set_velocity_pid(Command::SetVelocityPidM1, p, i, d, qpps, ec); + } + + /** + * @brief Set the motor 2 velocity PID constants and QPPS (command 29). + * See set_velocity_pid_m1() for the fixed-point conversion. + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param qpps Encoder speed (quad pulses per second) at 100% motor power. + * @param ec Set on failure. + * @return True on success. + */ + bool set_velocity_pid_m2(float p, float i, float d, uint32_t qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return set_velocity_pid(Command::SetVelocityPidM2, p, i, d, qpps, ec); + } + + /** + * @brief Read the motor 1 velocity PID constants and QPPS (command 55). + * Fixed-point values are converted back to floats (divide by 65536). + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param qpps Encoder speed (quad pulses per second) at 100% motor power. + * @param ec Set on failure. + * @return True on success. + */ + bool read_velocity_pid_m1(float &p, float &i, float &d, uint32_t &qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_velocity_pid(Command::ReadVelocityPidM1, p, i, d, qpps, ec); + } + + /** + * @brief Read the motor 2 velocity PID constants and QPPS (command 56). + * Fixed-point values are converted back to floats (divide by 65536). + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param qpps Encoder speed (quad pulses per second) at 100% motor power. + * @param ec Set on failure. + * @return True on success. + */ + bool read_velocity_pid_m2(float &p, float &i, float &d, uint32_t &qpps, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_velocity_pid(Command::ReadVelocityPidM2, p, i, d, qpps, ec); + } + + // -------------------------------- telemetry ------------------------------ + + /** + * @brief Read the firmware version string (command 21). + * + * The controller returns up to 48 bytes terminated by a line feed and a NUL + * character (e.g. "MCP266 2x60A v1.0.0"); the returned string has the + * terminators stripped. + * @param version The firmware / product version string. + * @param ec Set on failure. + * @return True on success. + */ + bool read_firmware_version(std::string &version, std::error_code &ec) { + std::scoped_lock lk(mutex_); + if (!transport_ok(ec)) + return false; + const auto request = detail::build_read_request( + config_.address, static_cast(Command::ReadFirmwareVersion)); + if (!config_.write(request)) { + ec = std::make_error_code(std::errc::io_error); + return false; + } + // variable-length reply: string bytes ... LF, NUL, then CRC16 (2 bytes) + static constexpr size_t max_version_len = 48; + std::vector reply; + reply.reserve(max_version_len + 2); + bool terminated = false; + while (reply.size() < max_version_len) { + uint8_t b; + if (!read_exact({&b, 1}, ec)) + return false; + reply.push_back(b); + if (reply.size() >= 2 && reply[reply.size() - 2] == '\n' && reply[reply.size() - 1] == '\0') { + terminated = true; + break; + } + } + if (!terminated) { + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + uint8_t crc[2] = {}; + if (!read_exact(crc, ec)) + return false; + reply.insert(reply.end(), std::begin(crc), std::end(crc)); + if (!detail::validate_reply(config_.address, static_cast(Command::ReadFirmwareVersion), + reply)) { + logger_.error("read_firmware_version: bad reply CRC"); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + // strip the LF + NUL terminators and the CRC + version.assign(reply.begin(), reply.end() - 4); + ec.clear(); + return true; + } + + /** + * @brief Read the main battery (B+/B-) voltage (command 24). + * @param volts Voltage in volts (the controller reports tenths of a volt). + * @param ec Set on failure. + * @return True on success. + */ + bool read_main_battery_voltage(float &volts, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_tenths(Command::ReadMainBatteryVoltage, volts, ec); + } + + /** + * @brief Read the logic battery (LB+/LB-) voltage (command 25). + * @param volts Voltage in volts (the controller reports tenths of a volt). + * @param ec Set on failure. + * @return True on success. + */ + bool read_logic_battery_voltage(float &volts, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_tenths(Command::ReadLogicBatteryVoltage, volts, ec); + } + + /** + * @brief Read the motor currents (command 49). + * @param amps_m1 Motor 1 current in amps (the controller reports 10 mA + * units, i.e. value / 100). + * @param amps_m2 Motor 2 current in amps. + * @param ec Set on failure. + * @return True on success. + */ + bool read_currents(float &s_m1, float &s_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[4] = {}; + if (!read_command(Command::ReadMotorCurrents, data, ec)) + return false; + amps_m1 = static_cast(detail::read_i16_be(data, 0)) / 100.0f; + amps_m2 = static_cast(detail::read_i16_be(data, 2)) / 100.0f; + return true; + } + + /** + * @brief Read the motor PWM output values (command 48). + * @param percent_m1 Motor 1 duty cycle in percent (-100 to +100; the + * controller reports +/-32767, i.e. value / 327.67). + * @param percent_m2 Motor 2 duty cycle in percent. + * @param ec Set on failure. + * @return True on success. + */ + bool read_motor_pwms(float &percent_m1, float &percent_m2, std::error_code &ec) { + std::scoped_lock lk(mutex_); + uint8_t data[4] = {}; + if (!read_command(Command::ReadMotorPWMs, data, ec)) + return false; + percent_m1 = static_cast(detail::read_i16_be(data, 0)) / 327.67f; + percent_m2 = static_cast(detail::read_i16_be(data, 2)) / 327.67f; + return true; + } + + /** + * @brief Read the board temperature (command 82). + * @param degrees Temperature in degrees (the controller reports tenths of a + * degree). + * @param ec Set on failure. + * @return True on success. + */ + bool read_temperature(float °rees, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_tenths(Command::ReadTemperature, degrees, ec); + } + + /** + * @brief Read the second board temperature (command 83, only on supported + * units). + * @param degrees Temperature in degrees (the controller reports tenths of a + * degree). + * @param ec Set on failure. + * @return True on success. + */ + bool read_temperature2(float °rees, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_tenths(Command::ReadTemperature2, degrees, ec); + } + + /** + * @brief Read the unit status bit mask (command 90). See Basicmicro::Status + * for the bit definitions (the manual documents the low 16 bits). + * @param status The status bit mask (0 = normal). + * @param ec Set on failure. + * @note The manual leaves the field width unstated, but current MCP firmware + * returns a 32-bit status (Basicmicro's official Arduino library reads + * it with Read4). This reads 4 bytes first; if that transaction fails + * (older firmware replying 16-bit makes the reply end mid-read), the + * device's 10 ms packet-clear gap has already elapsed via the timeout, + * so a single 16-bit retry is performed for legacy-firmware units. + * @return True on success. + */ + bool read_status(uint32_t &status, std::error_code &ec) { + std::scoped_lock lk(mutex_); + { + uint8_t data[4] = {}; + std::error_code ec32; + if (read_command(Command::ReadStatus, data, ec32)) { + status = detail::read_u32_be(data, 0); + return true; + } + } + // Legacy retry: the failed 32-bit attempt consumed the full receive + // timeout (>= the 10 ms packet-clear window), so the controller's packet + // buffer is clean and any short reply was fully drained from the host. + logger_.warn("32-bit status read failed; retrying as 16-bit (legacy firmware)"); + uint8_t data[2] = {}; + if (!read_command(Command::ReadStatus, data, ec)) + return false; + status = detail::read_u16_be(data, 0); + return true; + } + + // ------------------------------ management ------------------------------- + + /** + * @brief Write all settings to non-volatile memory (command 94) so they are + * reloaded on power-up. + * @note Per the manual this request is sent WITHOUT a CRC ([Address, 94]) + * but is still acknowledged with 0xFF. + * @param ec Set on failure. + * @return True on success. + */ + bool write_settings_to_eeprom(std::error_code &ec) { + if (!transport_ok(ec)) + return false; + std::scoped_lock lk(mutex_); + const auto request = detail::build_read_request( + config_.address, static_cast(Command::WriteSettingsToEeprom)); + if (!config_.write(request)) { + ec = std::make_error_code(std::errc::io_error); + return false; + } + return read_ack(ec); + } + + /** + * @brief Reset an E-Stop condition (command 200). Does nothing unless the + * E-Stop reset has been unlocked (manual command 201). + * @param ec Set on failure. + * @return True on success. + */ + bool e_stop_reset(std::error_code &ec) { + std::scoped_lock lk(mutex_); + return write_command(Command::EStopReset, {}, ec); + } + +protected: + /// Validate that both transport functions were configured. Calling an empty + /// std::function throws std::bad_function_call, which would violate this + /// component's no-exceptions contract -- so every transaction entry point + /// checks here first and fails with invalid_argument instead. + bool transport_ok(std::error_code &ec) { + if (!config_.write || !config_.read) { + logger_.error("Config::write and Config::read must both be set"); + ec = std::make_error_code(std::errc::invalid_argument); + return false; + } + return true; + } + + /// Read exactly buf.size() bytes, looping on the user read function until + /// the configured timeout elapses. Sets ec and returns false on timeout. + bool read_exact(std::span buf, std::error_code &ec) { + const auto deadline = std::chrono::steady_clock::now() + config_.timeout; + size_t got = 0; + while (got < buf.size()) { + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) + break; + const auto remaining = std::chrono::duration_cast(deadline - now); + const size_t n = + config_.read(buf.subspan(got), std::max(remaining, std::chrono::milliseconds(1))); + if (n == 0) + break; // the read function timed out + got += n; + } + if (got < buf.size()) { + logger_.debug("timed out reading reply ({}/{} bytes)", got, buf.size()); + ec = std::make_error_code(std::errc::timed_out); + return false; + } + ec.clear(); + return true; + } + + /// Read and check the single 0xFF ACK byte of a write command. + bool read_ack(std::error_code &ec) { + uint8_t ack = 0; + if (!read_exact({&ack, 1}, ec)) + return false; + if (ack != detail::kBasicmicroAck) { + logger_.error("bad ACK byte: 0x{:02X}", ack); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + ec.clear(); + return true; + } + + /// Run one write-command transaction: [addr, cmd, payload, CRC] -> 0xFF. + /// The caller must hold mutex_. + bool write_command(Command cmd, std::span payload, std::error_code &ec) { + if (!transport_ok(ec)) + return false; + const auto packet = + detail::build_write_packet(config_.address, static_cast(cmd), payload); + logger_.debug("write command {} ({} byte packet)", static_cast(cmd), packet.size()); + if (!config_.write(packet)) { + ec = std::make_error_code(std::errc::io_error); + return false; + } + return read_ack(ec); + } + + /// Run one read-command transaction: [addr, cmd] -> data + CRC. The reply + /// data (excluding CRC) is written into @p data, whose size determines how + /// many data bytes are expected. The caller must hold mutex_. + bool read_command(Command cmd, std::span data, std::error_code &ec) { + if (!transport_ok(ec)) + return false; + const auto request = detail::build_read_request(config_.address, static_cast(cmd)); + if (!config_.write(request)) { + ec = std::make_error_code(std::errc::io_error); + return false; + } + // reply = data bytes + 2 CRC bytes + std::vector reply(data.size() + 2); + if (!read_exact(reply, ec)) + return false; + if (!detail::validate_reply(config_.address, static_cast(cmd), reply)) { + logger_.error("read command {}: bad reply CRC", static_cast(cmd)); + ec = std::make_error_code(std::errc::protocol_error); + return false; + } + std::copy(reply.begin(), reply.end() - 2, data.begin()); + ec.clear(); + return true; + } + + /// Shared implementation for commands 16/17 (count + status byte). + bool read_encoder(Command cmd, uint32_t &count, uint8_t &status, std::error_code &ec) { + uint8_t data[5] = {}; + if (!read_command(cmd, data, ec)) + return false; + count = detail::read_u32_be(data, 0); + status = data[4]; + return true; + } + + /// Shared implementation for commands 18/19/30/31 (speed + direction byte). + bool read_speed(Command cmd, int32_t &qpps, uint8_t &direction, std::error_code &ec) { + uint8_t data[5] = {}; + if (!read_command(cmd, data, ec)) + return false; + qpps = detail::read_i32_be(data, 0); + direction = data[4]; + return true; + } + + /// Shared implementation for the 2-byte tenths-of-a-unit reads (24/25/82/83). + bool read_tenths(Command cmd, float &value, std::error_code &ec) { + uint8_t data[2] = {}; + if (!read_command(cmd, data, ec)) + return false; + value = static_cast(detail::read_u16_be(data, 0)) / 10.0f; + return true; + } + + /// Shared implementation for commands 28/29. Wire order is D, P, I, QPPS. + bool set_velocity_pid(Command cmd, float p, float i, float d, uint32_t qpps, + std::error_code &ec) { + std::vector payload; + detail::append_u32_be(payload, static_cast(d * detail::kBasicmicroPidScale)); + detail::append_u32_be(payload, static_cast(p * detail::kBasicmicroPidScale)); + detail::append_u32_be(payload, static_cast(i * detail::kBasicmicroPidScale)); + detail::append_u32_be(payload, qpps); + return write_command(cmd, payload, ec); + } + + /// Shared implementation for commands 55/56. Wire order is P, I, D, QPPS. + bool read_velocity_pid(Command cmd, float &p, float &i, float &d, uint32_t &qpps, + std::error_code &ec) { + uint8_t data[16] = {}; + if (!read_command(cmd, data, ec)) + return false; + p = static_cast(detail::read_u32_be(data, 0)) / detail::kBasicmicroPidScale; + i = static_cast(detail::read_u32_be(data, 4)) / detail::kBasicmicroPidScale; + d = static_cast(detail::read_u32_be(data, 8)) / detail::kBasicmicroPidScale; + qpps = detail::read_u32_be(data, 12); + return true; + } + + Config config_; + + /// Serializes complete transactions (request write + ACK/reply read) so + /// concurrent callers cannot interleave packets on the shared serial line. + std::mutex mutex_; +}; + +} // namespace espp diff --git a/components/basicmicro/include/detail/basicmicro_core.hpp b/components/basicmicro/include/detail/basicmicro_core.hpp new file mode 100644 index 0000000000..9008195de4 --- /dev/null +++ b/components/basicmicro/include/detail/basicmicro_core.hpp @@ -0,0 +1,266 @@ +#pragma once + +// Basicmicro (MCP236 / MCP266 / RoboClaw-family) PACKET SERIAL protocol — wire +// core. +// +// This header is intentionally free of any ESP-IDF / FreeRTOS dependency so +// that the wire logic (CRC16, packet building, reply validation, big-endian +// type codecs) can be built and unit-tested on a host with nothing more than a +// C++20 standard library. The `espp::Basicmicro` component composes this core +// together with `espp::BaseComponent` for logging and adds the transaction +// (write / read+timeout) layer on top of user-provided I/O functions. +// +// Wire format (MCP Series User Manual, section 2.2 "Packet Serial Mode"): +// - Write commands: [Address, Command, Data..., CRC16(2 bytes)] +// and the controller replies with a single 0xFF ACK byte +// (nothing at all is sent back if the packet was invalid). +// - Read commands: [Address, Command] (no CRC appended to the request); +// the controller replies with the data bytes followed by a +// CRC16 computed over the SENT Address and Command bytes +// plus all of the reply data bytes (section 2.2.7). +// - All multi-byte values are big-endian ("high byte first", section 2.2.9), +// including the CRC16 itself. +// - Addresses range from 0x80 to 0x87 (section 2.2.2). +// - CRC16 is the CCITT/XModem variant: polynomial 0x1021, initial value 0, +// not reflected (section 2.2.6 prints the reference C implementation which +// is replicated byte-for-byte below). +// - Packet timeout: a >=10 ms gap between bytes makes the controller discard +// the partial packet (section 2.2.4), so a >=10 ms receive timeout doubles +// as the error-recovery mechanism — by the time a reply times out, the +// controller's packet buffer has already been cleared automatically. + +#include +#include +#include +#include +#include + +namespace espp { +namespace detail { + +/// The ACK byte returned by the controller for every valid write command. +static constexpr uint8_t kBasicmicroAck = 0xFF; + +/// Valid packet-serial address range (manual section 2.2.2). +static constexpr uint8_t kBasicmicroMinAddress = 0x80; +/// Valid packet-serial address range (manual section 2.2.2). +static constexpr uint8_t kBasicmicroMaxAddress = 0x87; + +/// A >=10 ms inter-byte gap clears the controller's packet buffer (manual +/// section 2.2.4). Receive timeouts should therefore be at least this long so +/// that a timed-out transaction leaves the controller ready for a new packet. +static constexpr int kBasicmicroPacketTimeoutMs = 10; + +/// Velocity PID gains are transferred as 16.16 fixed point (scaled by 65536). +/// The manual (commands 28/29) lists the defaults as P=0x00010000, I=0x00008000 +/// and D=0x00004000, i.e. P=1.0, I=0.5, D=0.25. +static constexpr float kBasicmicroPidScale = 65536.0f; + +/// @brief Packet-serial command bytes. +/// +/// Every value below was verified against the MCP Series User Manual (sections +/// 2.2.12, 2.2.13, 2.3.1, 2.4.8 and 2.4.9). Commands whose payload layout is +/// not clearly documented in the manual are intentionally omitted. +enum class BasicmicroCommand : uint8_t { + // -- Compatibility commands (section 2.2.12), payload: one byte 0-127 -- + DriveForwardM1 = 0, ///< 0 = stop, 127 = full forward + DriveBackwardsM1 = 1, ///< 0 = stop, 127 = full reverse + DriveForwardM2 = 4, ///< 0 = stop, 127 = full forward + DriveBackwardsM2 = 5, ///< 0 = stop, 127 = full reverse + DriveM1_7Bit = 6, ///< 0 = full reverse, 64 = stop, 127 = full forward + DriveM2_7Bit = 7, ///< 0 = full reverse, 64 = stop, 127 = full forward + // -- Encoder commands (section 2.4.8) -- + ReadEncoderM1 = 16, ///< reply: count (4 bytes), status (1 byte) + ReadEncoderM2 = 17, ///< reply: count (4 bytes), status (1 byte) + ReadEncoderSpeedM1 = 18, ///< reply: pulses/s (4 bytes), direction (1 byte) + ReadEncoderSpeedM2 = 19, ///< reply: pulses/s (4 bytes), direction (1 byte) + ResetEncoders = 20, ///< payload: none (write command, CRC appended) + ReadFirmwareVersion = 21, ///< reply: string terminated by LF + NUL (<=48 bytes) + SetEncoderM1 = 22, ///< payload: value (4 bytes) + SetEncoderM2 = 23, ///< payload: value (4 bytes) + ReadMainBatteryVoltage = 24, ///< reply: tenths of a volt (2 bytes) + ReadLogicBatteryVoltage = 25, ///< reply: tenths of a volt (2 bytes) + SetVelocityPidM1 = 28, ///< payload: D, P, I (16.16 fixed), QPPS (4 bytes each) + SetVelocityPidM2 = 29, ///< payload: D, P, I (16.16 fixed), QPPS (4 bytes each) + ReadRawSpeedM1 = 30, ///< reply: counts/s (4 bytes), direction (1 byte) + ReadRawSpeedM2 = 31, ///< reply: counts/s (4 bytes), direction (1 byte) + // -- Advanced motor control (section 2.4.9) -- + DriveM1SignedDuty = 32, ///< payload: duty (2 bytes, +/-32767) + DriveM2SignedDuty = 33, ///< payload: duty (2 bytes, +/-32767) + DriveM1M2SignedDuty = 34, ///< payload: dutyM1 (2 bytes), dutyM2 (2 bytes) + DriveM1SignedSpeed = 35, ///< payload: speed (4 bytes, qpps) + DriveM2SignedSpeed = 36, ///< payload: speed (4 bytes, qpps) + DriveM1M2SignedSpeed = 37, ///< payload: speedM1 (4 bytes), speedM2 (4 bytes) + DriveM1SignedSpeedAccel = 38, ///< payload: accel (4 bytes), speed (4 bytes) + DriveM2SignedSpeedAccel = 39, ///< payload: accel (4 bytes), speed (4 bytes) + DriveM1M2SignedSpeedAccel = 40, ///< payload: accel, speedM1, speedM2 (4 bytes each) + BufferedM1SpeedDistance = 41, ///< payload: speed, distance (4 bytes each), buffer (1 byte) + BufferedM2SpeedDistance = 42, ///< payload: speed, distance (4 bytes each), buffer (1 byte) + BufferedM1M2SpeedDistance = 43, ///< payload: speedM1, distM1, speedM2, distM2, buffer + BufferedM1SpeedAccelDistance = 44, ///< payload: accel, speed, distance, buffer + BufferedM2SpeedAccelDistance = 45, ///< payload: accel, speed, distance, buffer + BufferedM1M2SpeedAccelDistance = 46, ///< payload: accel, speedM1, distM1, speedM2, distM2, buffer + ReadBufferLengths = 47, ///< reply: bufferM1 (1 byte), bufferM2 (1 byte) + ReadMotorPWMs = 48, ///< reply: pwmM1 (2 bytes), pwmM2 (2 bytes), +/-32767 + ReadMotorCurrents = 49, ///< reply: currentM1 (2 bytes), currentM2 (2 bytes), 10 mA units + ReadVelocityPidM1 = 55, ///< reply: P, I, D (16.16 fixed), QPPS (4 bytes each) + ReadVelocityPidM2 = 56, ///< reply: P, I, D (16.16 fixed), QPPS (4 bytes each) + SetMainBatteryVoltages = 57, ///< payload: min (2 bytes), max (2 bytes), tenths of a volt + SetLogicBatteryVoltages = 58, ///< payload: min (2 bytes), max (2 bytes), tenths of a volt + ReadMainBatteryVoltageSettings = 59, ///< reply: min (2 bytes), max (2 bytes) + ReadLogicBatteryVoltageSettings = 60, ///< reply: min (2 bytes), max (2 bytes) + SetM1DefaultDutyAccel = 68, ///< payload: accel (4 bytes) + SetM2DefaultDutyAccel = 69, ///< payload: accel (4 bytes) + ReadEncoderCounters = 78, ///< reply: encM1 (4 bytes), encM2 (4 bytes) + ReadISpeedCounters = 79, ///< reply: ispeedM1 (4 bytes), ispeedM2 (4 bytes) + RestoreDefaults = 80, ///< payload: none (write command, CRC appended) + ReadDefaultDutyAccels = 81, ///< reply: accelM1 (4 bytes), accelM2 (4 bytes) + ReadTemperature = 82, ///< reply: tenths of a degree (2 bytes) + ReadTemperature2 = 83, ///< reply: tenths of a degree (2 bytes), supported units only + // -- Status / configuration (section 2.3.1) -- + ReadStatus = 90, ///< reply: status bit mask (see BasicmicroStatus) + ReadEncoderModes = 91, ///< reply: encM1 mode (1 byte), encM2 mode (1 byte) + SetEncoderModeM1 = 92, ///< payload: pin/mode (1 byte) + SetEncoderModeM2 = 93, ///< payload: pin/mode (1 byte) + WriteSettingsToEeprom = 94, ///< no payload and no CRC on the request (per manual), ACK reply + EStopReset = 200, ///< payload: none (write command, CRC appended) +}; + +/// @brief Unit status bit masks returned by BasicmicroCommand::ReadStatus +/// (manual command 90). Current MCP firmware returns a 32-bit status +/// word (read via Read4 in the official library); the manual documents +/// the masks below, which occupy the low 16 bits. +enum class BasicmicroStatus : uint32_t { + Normal = 0x0000, + M1OverCurrentWarning = 0x0001, + M2OverCurrentWarning = 0x0002, + EStop = 0x0004, + TemperatureError = 0x0008, + Temperature2Error = 0x0010, + MainBatteryHighError = 0x0020, + LogicBatteryHighError = 0x0040, + LogicBatteryLowError = 0x0080, + MainBatteryHighWarning = 0x0400, + MainBatteryLowWarning = 0x0800, + TemperatureWarning = 0x1000, + Temperature2Warning = 0x2000, +}; + +/// Fold a single byte through the running CRC16 remainder. This replicates the +/// reference implementation printed in manual section 2.2.6 (CRC-16/XMODEM: +/// polynomial 0x1021, initial value 0, non-reflected, MSB first). +inline uint16_t basicmicro_crc16_byte(uint16_t crc, uint8_t val) { + crc ^= static_cast(static_cast(val) << 8); + for (int i = 0; i < 8; i++) + crc = (crc & 0x8000) ? static_cast((crc << 1) ^ 0x1021) + : static_cast(crc << 1); + return crc; +} + +/// CRC16 over a buffer. The default initial value (0) matches the manual's +/// reference implementation; pass a previous remainder to continue a running +/// CRC (used to seed reply validation with the sent address + command bytes). +inline uint16_t basicmicro_crc16(std::span data, uint16_t init = 0) { + return std::accumulate(data.begin(), data.end(), init, basicmicro_crc16_byte); +} + +// --- big-endian codec helpers ("high byte first", manual section 2.2.9) --- + +/// Append a single byte. +inline void append_u8(std::vector &v, uint8_t val) { v.push_back(val); } + +/// Append a 16-bit value, high byte first. +inline void append_u16_be(std::vector &v, uint16_t val) { + v.push_back(static_cast(val >> 8)); + v.push_back(static_cast(val & 0xFF)); +} + +/// Append a 32-bit value, high byte first. +inline void append_u32_be(std::vector &v, uint32_t val) { + v.push_back(static_cast(val >> 24)); + v.push_back(static_cast((val >> 16) & 0xFF)); + v.push_back(static_cast((val >> 8) & 0xFF)); + v.push_back(static_cast(val & 0xFF)); +} + +/// Append a signed 16-bit value (two's complement), high byte first. +inline void append_i16_be(std::vector &v, int16_t val) { + append_u16_be(v, static_cast(val)); +} + +/// Append a signed 32-bit value (two's complement), high byte first. +inline void append_i32_be(std::vector &v, int32_t val) { + append_u32_be(v, static_cast(val)); +} + +/// Read a 16-bit big-endian value at byte offset @p off. The caller must +/// ensure the span holds at least off+2 bytes. +inline uint16_t read_u16_be(std::span s, size_t off) { + return static_cast((static_cast(s[off]) << 8) | + static_cast(s[off + 1])); +} + +/// Read a 32-bit big-endian value at byte offset @p off. The caller must +/// ensure the span holds at least off+4 bytes. +inline uint32_t read_u32_be(std::span s, size_t off) { + return (static_cast(s[off]) << 24) | (static_cast(s[off + 1]) << 16) | + (static_cast(s[off + 2]) << 8) | static_cast(s[off + 3]); +} + +/// Read a signed 16-bit big-endian value (two's complement) at byte offset @p off. +inline int16_t read_i16_be(std::span s, size_t off) { + return static_cast(read_u16_be(s, off)); +} + +/// Read a signed 32-bit big-endian value (two's complement) at byte offset @p off. +inline int32_t read_i32_be(std::span s, size_t off) { + return static_cast(read_u32_be(s, off)); +} + +// --- packet building / reply validation ----------------------------------- + +/// @brief Build a write-command packet: [Address, Command, payload..., CRC16]. +/// The CRC16 covers the address, command and payload bytes and is +/// appended high byte first. +/// @param address Controller address (0x80 - 0x87). +/// @param command Command byte. +/// @param payload Command data bytes (may be empty, e.g. ResetEncoders). +/// @return The complete packet, ready to transmit. +inline std::vector build_write_packet(uint8_t address, uint8_t command, + std::span payload = {}) { + std::vector pkt; + pkt.reserve(payload.size() + 4); + pkt.push_back(address); + pkt.push_back(command); + pkt.insert(pkt.end(), payload.begin(), payload.end()); + append_u16_be(pkt, basicmicro_crc16(pkt)); + return pkt; +} + +/// @brief Build a read-command request: [Address, Command]. Read requests +/// carry no CRC (the CRC of the reply is instead seeded with these two +/// bytes, see validate_reply()). +inline std::vector build_read_request(uint8_t address, uint8_t command) { + return {address, command}; +} + +/// @brief Validate a read-command reply. +/// +/// Per manual section 2.2.7 the reply CRC16 is computed over the SENT address +/// and command bytes followed by all reply data bytes, and is transmitted high +/// byte first as the last two bytes of the reply. +/// @param address The address byte that was sent. +/// @param command The command byte that was sent. +/// @param reply The full reply: data bytes followed by the 2 CRC bytes. +/// @return True if the reply is at least 2 bytes and its CRC matches. +inline bool validate_reply(uint8_t address, uint8_t command, std::span reply) { + if (reply.size() < 2) + return false; + const uint8_t header[] = {address, command}; + uint16_t crc = basicmicro_crc16(header); + crc = basicmicro_crc16(reply.first(reply.size() - 2), crc); + return crc == read_u16_be(reply, reply.size() - 2); +} + +} // namespace detail +} // namespace espp diff --git a/components/basicmicro/test/basicmicro_host_test.cpp b/components/basicmicro/test/basicmicro_host_test.cpp new file mode 100644 index 0000000000..bdc7c6e6bd --- /dev/null +++ b/components/basicmicro/test/basicmicro_host_test.cpp @@ -0,0 +1,180 @@ +// Host-buildable unit tests for the Basicmicro (MCP / RoboClaw-family) packet +// serial wire core. Build & run with: +// c++ -std=c++20 -I../include basicmicro_host_test.cpp -o test && ./test +// +// These tests exercise the helpers in detail/basicmicro_core.hpp directly so +// they need no ESP-IDF headers. Golden CRC values were computed by executing +// the reference CRC16 C implementation printed in MCP Series User Manual +// section 2.2.6 (CRC-16/XMODEM: poly 0x1021, init 0, non-reflected). + +#include +#include +#include +#include +#include + +#include "detail/basicmicro_core.hpp" + +using namespace espp::detail; + +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 uint16_t crc_of(std::string_view s) { + return basicmicro_crc16( + std::span(reinterpret_cast(s.data()), s.size())); +} + +static void test_crc_golden() { + std::printf("test_crc_golden\n"); + // empty input leaves the initial remainder (0) untouched + CHECK(crc_of("") == 0x0000); + // a zero byte folded through a zero remainder stays zero + const uint8_t zero = 0x00; + CHECK(basicmicro_crc16(std::span(&zero, 1)) == 0x0000); + // the classic CRC-16/XMODEM check value + CHECK(crc_of("123456789") == 0x31C3); + CHECK(crc_of("A") == 0x58E5); + // real packets from the manual's command set: + // [0x80, 0 (DriveForwardM1), 35] + const uint8_t drive_fwd[] = {0x80, 0x00, 0x23}; + CHECK(basicmicro_crc16(drive_fwd) == 0x2F5B); + // [0x80, 32 (DriveM1SignedDuty), duty=0x4000 (+50%)] + const uint8_t drive_duty[] = {0x80, 0x20, 0x40, 0x00}; + CHECK(basicmicro_crc16(drive_duty) == 0x5632); + // [0x80, 20 (ResetEncoders)] -- write command with an empty payload + const uint8_t reset_enc[] = {0x80, 0x14}; + CHECK(basicmicro_crc16(reset_enc) == 0x492D); + // seeded continuation equals one-shot over the concatenation + const uint8_t head[] = {0x80, 0x18}; + const uint8_t tail[] = {0x00, 0x7B}; + CHECK(basicmicro_crc16(tail, basicmicro_crc16(head)) == 0xF806); +} + +static void test_codecs() { + std::printf("test_codecs\n"); + std::vector v; + append_u8(v, 0xAB); + append_u16_be(v, 0x1234); + append_u32_be(v, 0xDEADBEEF); + append_i16_be(v, -2); // 0xFFFE + append_i32_be(v, -3000); // 0xFFFFF448 + const std::vector expected = {0xAB, 0x12, 0x34, 0xDE, 0xAD, 0xBE, 0xEF, + 0xFF, 0xFE, 0xFF, 0xFF, 0xF4, 0x48}; + CHECK(v == expected); + // decode round-trips (big-endian, "high byte first" per manual 2.2.9) + CHECK(read_u16_be(v, 1) == 0x1234); + CHECK(read_u32_be(v, 3) == 0xDEADBEEF); + CHECK(read_i16_be(v, 7) == -2); + CHECK(read_i32_be(v, 9) == -3000); +} + +static void test_packet_build() { + std::printf("test_packet_build\n"); + // write packet with payload: [addr, cmd, data..., crc_hi, crc_lo] + std::vector payload; + append_u8(payload, 0x23); + const auto pkt = build_write_packet(0x80, 0, payload); + const std::vector expected = {0x80, 0x00, 0x23, 0x2F, 0x5B}; + CHECK(pkt == expected); + + // write packet with an empty payload (ResetEncoders) + const auto reset = build_write_packet(0x80, 20); + const std::vector expected_reset = {0x80, 0x14, 0x49, 0x2D}; + CHECK(reset == expected_reset); + + // drive M1 signed speed -3000 qpps: [0x80, 35, FF FF F4 48, crc] + std::vector speed_payload; + append_i32_be(speed_payload, -3000); + const auto speed_pkt = build_write_packet(0x80, 35, speed_payload); + const std::vector expected_speed = {0x80, 0x23, 0xFF, 0xFF, 0xF4, 0x48, 0xA0, 0x4F}; + CHECK(speed_pkt == expected_speed); + + // read requests carry no CRC + const auto req = build_read_request(0x80, 24); + const std::vector expected_req = {0x80, 0x18}; + CHECK(req == expected_req); +} + +static void test_reply_validation() { + std::printf("test_reply_validation\n"); + // reply to ReadMainBatteryVoltage (cmd 24) at addr 0x80 with value 123 + // (12.3 V): data [0x00, 0x7B], CRC over [0x80, 0x18, 0x00, 0x7B] = 0xF806 + const std::vector reply = {0x00, 0x7B, 0xF8, 0x06}; + CHECK(validate_reply(0x80, 24, reply)); + + // wrong address / command seeds must fail + CHECK(!validate_reply(0x81, 24, reply)); + CHECK(!validate_reply(0x80, 25, reply)); + + // corrupt data byte must fail + std::vector bad_data = reply; + bad_data[0] ^= 0x01; + CHECK(!validate_reply(0x80, 24, bad_data)); + + // corrupt CRC byte must fail + std::vector bad_crc = reply; + bad_crc[3] ^= 0x01; + CHECK(!validate_reply(0x80, 24, bad_crc)); + + // too-short replies must fail (need at least the 2 CRC bytes) + const std::vector tiny = {0xF8}; + CHECK(!validate_reply(0x80, 24, tiny)); + + // ReadEncoderM1 reply at addr 0x81: count 0x12345678, status 0x02, CRC over + // [0x81, 0x10, 0x12, 0x34, 0x56, 0x78, 0x02] = 0xE201 + const std::vector enc_reply = {0x12, 0x34, 0x56, 0x78, 0x02, 0xE2, 0x01}; + CHECK(validate_reply(0x81, 16, enc_reply)); + CHECK(read_u32_be(enc_reply, 0) == 0x12345678u); + CHECK(enc_reply[4] == 0x02); +} + +static void test_round_trip() { + std::printf("test_round_trip\n"); + // Build a write packet, then check that the packet body validates against + // its own trailing CRC using the reply-validation seeding rules: a write + // packet [addr, cmd, data, crc] is equivalent to a "reply" of data bytes + // whose CRC is seeded with [addr, cmd]. + std::vector payload; + append_u32_be(payload, 0x00010000); // P = 1.0 in 16.16 fixed point + append_u32_be(payload, 0x00008000); // I = 0.5 + append_u32_be(payload, 0x00004000); // D = 0.25 + append_u32_be(payload, 44000); // QPPS default + const auto pkt = build_write_packet(0x80, 28, payload); + CHECK(pkt.size() == 2 + 16 + 2); + // whole packet CRCs to zero remainder... (property of appending the CRC) + // more directly: the stored CRC matches a recomputation over addr+cmd+data + const std::span body(pkt.data() + 2, pkt.size() - 4); + CHECK(validate_reply(pkt[0], pkt[1], std::span(pkt.data() + 2, pkt.size() - 2))); + CHECK(basicmicro_crc16(std::span(pkt.data(), pkt.size() - 2)) == + read_u16_be(pkt, pkt.size() - 2)); + CHECK(body.size() == 16); + + // command / status enums carry the verified wire values + CHECK(static_cast(BasicmicroCommand::ReadFirmwareVersion) == 21); + CHECK(static_cast(BasicmicroCommand::SetVelocityPidM1) == 28); + CHECK(static_cast(BasicmicroCommand::ReadVelocityPidM1) == 55); + CHECK(static_cast(BasicmicroCommand::ReadStatus) == 90); + CHECK(static_cast(BasicmicroCommand::EStopReset) == 200); + CHECK(static_cast(BasicmicroStatus::Temperature2Warning) == 0x2000); +} + +int main() { + test_crc_golden(); + test_codecs(); + test_packet_build(); + test_reply_validation(); + test_round_trip(); + 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 0046d51535..b5304e6c66 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -80,6 +80,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/as5600/example/main/as5600_example.cpp \ $(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/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 \ @@ -217,6 +218,8 @@ INPUT = \ $(PROJECT_PATH)/components/aw9523/include/aw9523.hpp \ $(PROJECT_PATH)/components/base_component/include/base_component.hpp \ $(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/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/basicmicro.rst b/doc/en/motor_control/basicmicro.rst new file mode 100644 index 0000000000..c773d5bb14 --- /dev/null +++ b/doc/en/motor_control/basicmicro.rst @@ -0,0 +1,82 @@ +Basicmicro (MCP / RoboClaw) Motor Controller Component +====================================================== + +Overview +-------- + +``espp::Basicmicro`` is a driver for Basicmicro MCP236 / MCP266 (and other +RoboClaw-family) brushed DC motor controllers speaking their packet serial +protocol, typically over UART. + +The component is transport-agnostic: it performs no I/O itself and instead +calls user-provided ``write`` / ``read`` functions for each transaction, so it +works over a UART driver, USB CDC, an RS-232 adapter, etc. All wire-format +logic (CRC16, packet building, reply validation, big-endian codecs) lives in +``espp::detail`` inside ``include/detail/basicmicro_core.hpp``, a +host-buildable core that depends only on the C++20 standard library and is +unit-tested off-target. + +Features +-------- + +- Duty-cycle drive (commands 32/33/34) and closed-loop speed drive in + quadrature pulses per second (35/36/37), with optional acceleration ramps + (38/39/40) +- Buffered speed / accel / distance motion commands (41-46) plus buffer-state + readback (47) +- Encoder counts (16/17/78), speeds (18/19/79), reset (20) and encoder-mode + readback (91) +- Velocity PID get/set with automatic 16.16 fixed-point conversion (28/29, + 55/56) +- Telemetry: firmware version (21), battery voltages (24/25), motor currents + (49), motor PWMs (48), board temperatures (82/83) and unit status (90) +- Management: write settings to EEPROM (94), E-Stop reset (200) +- No exceptions; all methods report errors via ``std::error_code`` +- Thread-safe: each transaction (request write + ACK/reply read) is serialized + by an internal mutex + +Protocol +-------- + +Per the MCP Series User Manual (section 2.2), write commands send +``[Address, Command, Data..., CRC16]`` and are acknowledged with a single +``0xFF`` byte, while read commands send ``[Address, Command]`` (no CRC) and +reply with the data followed by a CRC16 seeded with the sent address and +command bytes. All multi-byte values are big-endian and the CRC is +CRC-16/XMODEM (poly ``0x1021``, init ``0``). A 10 ms inter-byte gap clears the +controller's packet buffer, so the configured receive timeout (>= 10 ms) +doubles as the error-recovery mechanism. + +Basic Usage +----------- + +.. code-block:: cpp + + espp::Basicmicro mcp({ + .address = 0x80, + .write = [](std::span data) { /* UART write */ return true; }, + .read = [](std::span data, std::chrono::milliseconds timeout) -> size_t { + /* UART read with timeout, return bytes read */ return 0; + }, + }); + + std::error_code ec; + std::string version; + mcp.read_firmware_version(version, ec); + mcp.drive_m1_duty(4096, ec); // ~12.5% duty + uint32_t count; uint8_t status; + mcp.read_encoder_m1(count, status, ec); + mcp.drive_m1_duty(0, ec); + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + basicmicro_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/basicmicro.inc diff --git a/doc/en/motor_control/basicmicro_example.md b/doc/en/motor_control/basicmicro_example.md new file mode 100644 index 0000000000..1e7a158bd4 --- /dev/null +++ b/doc/en/motor_control/basicmicro_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/basicmicro/example/README.md +``` diff --git a/doc/en/motor_control/index.rst b/doc/en/motor_control/index.rst index 1b347e3bb8..2666951a22 100644 --- a/doc/en/motor_control/index.rst +++ b/doc/en/motor_control/index.rst @@ -10,6 +10,7 @@ Motor-control algorithms and controller interfaces. See also the pid adrc + basicmicro odrive_ascii odrive_native trajectory_planner