From 0738d3513ff3dffd0bdc9ff3fb673dc8295f89b4 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Wed, 2 Sep 2026 11:22:07 -0500 Subject: [PATCH] feat(basicmicro): position PID get/set for both channels (commands 61-64) The component implemented the velocity PID (28/29, 55/56) but not the position PID, so closed-loop position moves could not be configured. Add set_position_pid_m1/m2 (commands 61/62) and read_position_pid_m1/m2 (63/64), covering the full seven-constant position loop: P/I/D gains (scaled by 1024, the position-loop convention -- distinct from the velocity loop's 16.16), MaxI windup limit, deadzone, and the MinPos/ MaxPos clamp. The factory default for every position constant is zero, which clamps all position targets to [0, 0]; these setters are what a CANopen or packet-serial master uses to make position mode usable. Note the documented wire asymmetry: command 61/62 send D, P, I while 63/64 reply P, I, D -- handled inside the shared helpers. Host golden- frame test and README updated. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_0153z8MvyCu6YT47myGDn4rK --- components/basicmicro/README.md | 2 + components/basicmicro/include/basicmicro.hpp | 104 ++++++++++++++++++ .../include/detail/basicmicro_core.hpp | 57 ++++++++-- .../basicmicro/test/basicmicro_host_test.cpp | 64 +++++++++++ 4 files changed, 219 insertions(+), 8 deletions(-) diff --git a/components/basicmicro/README.md b/components/basicmicro/README.md index a80d0b4128..b2c06006d0 100644 --- a/components/basicmicro/README.md +++ b/components/basicmicro/README.md @@ -36,6 +36,8 @@ dependencies that is unit-tested off-target. encoder-mode readback (91) - Velocity PID get/set with automatic 16.16 fixed-point conversion (28/29, 55/56) +- Position PID get/set (61-64) for both channels, with automatic 1024x + fixed-point conversion and the MaxI / deadzone / min / max position fields - Telemetry: firmware version (21), main/logic battery voltage (24/25), motor currents (49), motor PWMs (48), board temperatures (82/83) and unit status (90) diff --git a/components/basicmicro/include/basicmicro.hpp b/components/basicmicro/include/basicmicro.hpp index a00cac984b..ebf96893e6 100644 --- a/components/basicmicro/include/basicmicro.hpp +++ b/components/basicmicro/include/basicmicro.hpp @@ -588,6 +588,76 @@ class Basicmicro : public BaseComponent { return read_velocity_pid(Command::ReadVelocityPidM2, p, i, d, qpps, ec); } + /** + * @brief Set the motor 1 position PID constants (command 61). + * + * The position loop has seven constants: P, I, D gains (transferred scaled + * by 1024), MaxI (integral windup limit), Deadzone (in encoder counts), and + * MinPos / MaxPos (the position range the loop will command; a target + * outside it is clamped). The factory default for every constant is zero, + * so position commands (65-67) produce no motion until these are set. Note + * the wire order for this command is D, P, I (unlike the P, I, D read order + * of command 63). + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param max_i Maximum integral windup. + * @param deadzone Deadzone in encoder counts. + * @param min_pos Minimum commandable position. + * @param max_pos Maximum commandable position. + * @param ec Set on failure. + * @return True on success. + */ + bool set_position_pid_m1(float p, float i, float d, uint32_t max_i, uint32_t deadzone, + int32_t min_pos, int32_t max_pos, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return set_position_pid(Command::SetPositionPidM1, p, i, d, max_i, deadzone, min_pos, max_pos, + ec); + } + + /** + * @brief Set the motor 2 position PID constants (command 62). + * See set_position_pid_m1() for the constants and scaling. + */ + bool set_position_pid_m2(float p, float i, float d, uint32_t max_i, uint32_t deadzone, + int32_t min_pos, int32_t max_pos, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return set_position_pid(Command::SetPositionPidM2, p, i, d, max_i, deadzone, min_pos, max_pos, + ec); + } + + /** + * @brief Read the motor 1 position PID constants (command 63). + * Gains are converted back to floats (divide by 1024). The reply + * order is P, I, D (unlike the D, P, I write order of command 61). + * @param p Proportional gain. + * @param i Integral gain. + * @param d Derivative gain. + * @param max_i Maximum integral windup. + * @param deadzone Deadzone in encoder counts. + * @param min_pos Minimum commandable position. + * @param max_pos Maximum commandable position. + * @param ec Set on failure. + * @return True on success. + */ + bool read_position_pid_m1(float &p, float &i, float &d, uint32_t &max_i, uint32_t &deadzone, + int32_t &min_pos, int32_t &max_pos, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_position_pid(Command::ReadPositionPidM1, p, i, d, max_i, deadzone, min_pos, max_pos, + ec); + } + + /** + * @brief Read the motor 2 position PID constants (command 64). + * See read_position_pid_m1() for the fields and scaling. + */ + bool read_position_pid_m2(float &p, float &i, float &d, uint32_t &max_i, uint32_t &deadzone, + int32_t &min_pos, int32_t &max_pos, std::error_code &ec) { + std::scoped_lock lk(mutex_); + return read_position_pid(Command::ReadPositionPidM2, p, i, d, max_i, deadzone, min_pos, max_pos, + ec); + } + // -------------------------------- telemetry ------------------------------ /** @@ -942,6 +1012,40 @@ class Basicmicro : public BaseComponent { return true; } + /// Shared implementation for commands 61/62. Wire order is + /// D, P, I, MaxI, Deadzone, MinPos, MaxPos (P/I/D scaled by 1024). + bool set_position_pid(Command cmd, float p, float i, float d, uint32_t max_i, uint32_t deadzone, + int32_t min_pos, int32_t max_pos, std::error_code &ec) { + std::vector payload; + payload.reserve(28); // fixed 7 x 4 bytes; avoid incremental reallocations + detail::append_u32_be(payload, detail::scale_pid_gain(d, detail::kBasicmicroPositionPidScale)); + detail::append_u32_be(payload, detail::scale_pid_gain(p, detail::kBasicmicroPositionPidScale)); + detail::append_u32_be(payload, detail::scale_pid_gain(i, detail::kBasicmicroPositionPidScale)); + detail::append_u32_be(payload, max_i); + detail::append_u32_be(payload, deadzone); + detail::append_i32_be(payload, min_pos); + detail::append_i32_be(payload, max_pos); + return write_command(cmd, payload, ec); + } + + /// Shared implementation for commands 63/64. Reply order is + /// P, I, D, MaxI, Deadzone, MinPos, MaxPos (P/I/D scaled by 1024). + bool read_position_pid(Command cmd, float &p, float &i, float &d, uint32_t &max_i, + uint32_t &deadzone, int32_t &min_pos, int32_t &max_pos, + std::error_code &ec) { + uint8_t data[28] = {}; + if (!read_command(cmd, data, ec)) + return false; + p = static_cast(detail::read_u32_be(data, 0)) / detail::kBasicmicroPositionPidScale; + i = static_cast(detail::read_u32_be(data, 4)) / detail::kBasicmicroPositionPidScale; + d = static_cast(detail::read_u32_be(data, 8)) / detail::kBasicmicroPositionPidScale; + max_i = detail::read_u32_be(data, 12); + deadzone = detail::read_u32_be(data, 16); + min_pos = detail::read_i32_be(data, 20); + max_pos = detail::read_i32_be(data, 24); + return true; + } + Config config_; /// Serializes complete transactions (request write + ACK/reply read) so diff --git a/components/basicmicro/include/detail/basicmicro_core.hpp b/components/basicmicro/include/detail/basicmicro_core.hpp index 9008195de4..54ce967643 100644 --- a/components/basicmicro/include/detail/basicmicro_core.hpp +++ b/components/basicmicro/include/detail/basicmicro_core.hpp @@ -29,6 +29,7 @@ // 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 @@ -56,6 +57,11 @@ static constexpr int kBasicmicroPacketTimeoutMs = 10; /// and D=0x00004000, i.e. P=1.0, I=0.5, D=0.25. static constexpr float kBasicmicroPidScale = 65536.0f; +/// Position PID gains are transferred scaled by 1024 (Basicmicro reference +/// library convention for the position loop; commands 61-64). The velocity +/// loop uses the separate kBasicmicroPidScale above. +static constexpr float kBasicmicroPositionPidScale = 1024.0f; + /// @brief Packet-serial command bytes. /// /// Every value below was verified against the MCP Series User Manual (sections @@ -109,14 +115,20 @@ enum class BasicmicroCommand : uint8_t { 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 + SetPositionPidM1 = + 61, ///< payload: D, P, I (scaled 1024), MaxI, Deadzone, MinPos, MaxPos (4 each) + SetPositionPidM2 = + 62, ///< payload: D, P, I (scaled 1024), MaxI, Deadzone, MinPos, MaxPos (4 each) + ReadPositionPidM1 = 63, ///< reply: P, I, D (scaled 1024), MaxI, Deadzone, MinPos, MaxPos (4 each) + ReadPositionPidM2 = 64, ///< reply: P, I, D (scaled 1024), MaxI, Deadzone, MinPos, MaxPos (4 each) + 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) @@ -164,6 +176,35 @@ inline uint16_t basicmicro_crc16(std::span data, uint16_t init = return std::accumulate(data.begin(), data.end(), init, basicmicro_crc16_byte); } +/// Convert a floating-point PID gain to the controller's fixed-point wire +/// representation (multiply by \p scale). Rounds to nearest rather than +/// truncating (truncation biases every gain downward by up to ~1 LSB), and +/// clamps to the non-negative uint32_t range: PID gains are non-negative on +/// these controllers, and a raw static_cast of a negative product to uint32_t +/// would silently wrap to a huge value. Non-finite or out-of-range inputs are +/// saturated rather than fed to the rounding function (std::llround of a value +/// outside long long, or of inf/NaN, is undefined). +inline uint32_t scale_pid_gain(float gain, float scale) { + if (!(gain > 0.0f)) { // false for <= 0 and for NaN + return 0; + } + const double scaled = static_cast(gain) * static_cast(scale); + // Compare before rounding: a scaled value at/above UINT32_MAX (or +inf) must + // not reach std::llround, whose result is undefined outside long long's range + // and for non-finite inputs. + if (!(scaled < 4294967296.0)) { // 2^32; also false for +inf and NaN + return UINT32_MAX; + } + const long long rounded = std::llround(scaled); + if (rounded <= 0) { + return 0; + } + if (rounded > static_cast(UINT32_MAX)) { + return UINT32_MAX; + } + return static_cast(rounded); +} + // --- big-endian codec helpers ("high byte first", manual section 2.2.9) --- /// Append a single byte. diff --git a/components/basicmicro/test/basicmicro_host_test.cpp b/components/basicmicro/test/basicmicro_host_test.cpp index bdc7c6e6bd..3b5b4d0b89 100644 --- a/components/basicmicro/test/basicmicro_host_test.cpp +++ b/components/basicmicro/test/basicmicro_host_test.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -160,9 +161,72 @@ static void test_round_trip() { CHECK(static_cast(BasicmicroCommand::ReadFirmwareVersion) == 21); CHECK(static_cast(BasicmicroCommand::SetVelocityPidM1) == 28); CHECK(static_cast(BasicmicroCommand::ReadVelocityPidM1) == 55); + CHECK(static_cast(BasicmicroCommand::SetPositionPidM1) == 61); + CHECK(static_cast(BasicmicroCommand::SetPositionPidM2) == 62); + CHECK(static_cast(BasicmicroCommand::ReadPositionPidM1) == 63); + CHECK(static_cast(BasicmicroCommand::ReadPositionPidM2) == 64); CHECK(static_cast(BasicmicroCommand::ReadStatus) == 90); CHECK(static_cast(BasicmicroCommand::EStopReset) == 200); CHECK(static_cast(BasicmicroStatus::Temperature2Warning) == 0x2000); + + // position PID payload (command 61): D, P, I scaled by 1024, then MaxI, + // Deadzone, MinPos, MaxPos as raw 32-bit -> 7 * 4 = 28 data bytes. Build the + // gains through scale_pid_gain() exactly as the production path does. + std::vector pos; + append_u32_be(pos, scale_pid_gain(4.0f, kBasicmicroPositionPidScale)); // D + append_u32_be(pos, scale_pid_gain(2.0f, kBasicmicroPositionPidScale)); // P + append_u32_be(pos, scale_pid_gain(0.0f, kBasicmicroPositionPidScale)); // I + append_u32_be(pos, 0); // MaxI + append_u32_be(pos, 10); // Deadzone + append_i32_be(pos, -20000); // MinPos + append_i32_be(pos, 20000); // MaxPos + CHECK(pos.size() == 28); + CHECK(read_u32_be(pos, 0) == 4096); // 4.0 * 1024 + CHECK(read_u32_be(pos, 4) == 2048); // 2.0 * 1024 + CHECK(read_i32_be(pos, 20) == -20000); + CHECK(read_i32_be(pos, 24) == 20000); + const auto pos_pkt = + build_write_packet(0x80, static_cast(BasicmicroCommand::SetPositionPidM1), pos); + CHECK(pos_pkt.size() == 2 + 28 + 2); + CHECK(basicmicro_crc16(std::span(pos_pkt.data(), pos_pkt.size() - 2)) == + read_u16_be(pos_pkt, pos_pkt.size() - 2)); + + // scale_pid_gain(): round-to-nearest (not truncate) and clamp non-negative. + CHECK(scale_pid_gain(4.0f, kBasicmicroPositionPidScale) == 4096); + CHECK(scale_pid_gain(1.5f, kBasicmicroPositionPidScale) == 1536); + // 0.1 * 1024 = 102.4 -> rounds to 102 (truncation would also give 102); + // 0.10009765625 (= 102.5/1024) rounds to 103, where truncation gives 102. + CHECK(scale_pid_gain(102.5f / kBasicmicroPositionPidScale, kBasicmicroPositionPidScale) == 103); + CHECK(scale_pid_gain(-1.0f, kBasicmicroPositionPidScale) == 0); // negatives clamp to 0 + CHECK(scale_pid_gain(0.0f, kBasicmicroPositionPidScale) == 0); + CHECK(scale_pid_gain(1.0f, 65536.0f) == 65536); // velocity 16.16 scale too + // non-finite and out-of-range inputs saturate rather than invoke UB + CHECK(scale_pid_gain(std::numeric_limits::infinity(), kBasicmicroPositionPidScale) == + 0xFFFFFFFFu); + CHECK(scale_pid_gain(std::numeric_limits::quiet_NaN(), kBasicmicroPositionPidScale) == 0); + CHECK(scale_pid_gain(1.0e12f, kBasicmicroPositionPidScale) == 0xFFFFFFFFu); // >> 2^32 + CHECK(scale_pid_gain(4194304.0f, 1024.0f) == 0xFFFFFFFFu); // 4194304 * 1024 == 2^32, saturates + CHECK(scale_pid_gain(4194303.0f, 1024.0f) == 4294966272u); // just below 2^32, fits exactly + + // position PID REPLY (command 63/64): the read order is P, I, D, then MaxI, + // Deadzone, MinPos, MaxPos -- distinct from the D, P, I write order above. + // Build a 28-byte reply and confirm each field decodes from its offset. + std::vector reply; + append_u32_be(reply, static_cast(2.0f * kBasicmicroPositionPidScale)); // P (offset 0) + append_u32_be(reply, static_cast(0.5f * kBasicmicroPositionPidScale)); // I (offset 4) + append_u32_be(reply, static_cast(4.0f * kBasicmicroPositionPidScale)); // D (offset 8) + append_u32_be(reply, 7); // MaxI (offset 12) + append_u32_be(reply, 10); // Deadzone (offset 16) + append_i32_be(reply, -20000); // MinPos (offset 20) + append_i32_be(reply, 20000); // MaxPos (offset 24) + CHECK(reply.size() == 28); + CHECK(static_cast(read_u32_be(reply, 0)) / kBasicmicroPositionPidScale == 2.0f); // P + CHECK(static_cast(read_u32_be(reply, 4)) / kBasicmicroPositionPidScale == 0.5f); // I + CHECK(static_cast(read_u32_be(reply, 8)) / kBasicmicroPositionPidScale == 4.0f); // D + CHECK(read_u32_be(reply, 12) == 7); + CHECK(read_u32_be(reply, 16) == 10); + CHECK(read_i32_be(reply, 20) == -20000); + CHECK(read_i32_be(reply, 24) == 20000); } int main() {