feat(basicmicro): position PID get/set for both channels (commands 61-64)

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)

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<uint8_t> 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<float>(detail::read_u32_be(data, 0)) / detail::kBasicmicroPositionPidScale;
+    i = static_cast<float>(detail::read_u32_be(data, 4)) / detail::kBasicmicroPositionPidScale;
+    d = static_cast<float>(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

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 <cmath>
 #include <cstddef>
 #include <cstdint>
 #include <numeric>
@@ -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<const uint8_t> 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<double>(gain) * static_cast<double>(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<long long>(UINT32_MAX)) {
+    return UINT32_MAX;
+  }
+  return static_cast<uint32_t>(rounded);
+}
+
 // --- big-endian codec helpers ("high byte first", manual section 2.2.9) ---
 
 /// Append a single byte.

components/basicmicro/test/basicmicro_host_test.cpp

@@ -9,6 +9,7 @@
 
 #include <cstdint>
 #include <cstdio>
+#include <limits>
 #include <span>
 #include <string_view>
 #include <vector>
@@ -160,9 +161,72 @@ static void test_round_trip() {
   CHECK(static_cast<uint8_t>(BasicmicroCommand::ReadFirmwareVersion) == 21);
   CHECK(static_cast<uint8_t>(BasicmicroCommand::SetVelocityPidM1) == 28);
   CHECK(static_cast<uint8_t>(BasicmicroCommand::ReadVelocityPidM1) == 55);
+  CHECK(static_cast<uint8_t>(BasicmicroCommand::SetPositionPidM1) == 61);
+  CHECK(static_cast<uint8_t>(BasicmicroCommand::SetPositionPidM2) == 62);
+  CHECK(static_cast<uint8_t>(BasicmicroCommand::ReadPositionPidM1) == 63);
+  CHECK(static_cast<uint8_t>(BasicmicroCommand::ReadPositionPidM2) == 64);
   CHECK(static_cast<uint8_t>(BasicmicroCommand::ReadStatus) == 90);
   CHECK(static_cast<uint8_t>(BasicmicroCommand::EStopReset) == 200);
   CHECK(static_cast<uint16_t>(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<uint8_t> 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<uint8_t>(BasicmicroCommand::SetPositionPidM1), pos);
+  CHECK(pos_pkt.size() == 2 + 28 + 2);
+  CHECK(basicmicro_crc16(std::span<const uint8_t>(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<float>::infinity(), kBasicmicroPositionPidScale) ==
+        0xFFFFFFFFu);
+  CHECK(scale_pid_gain(std::numeric_limits<float>::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<uint8_t> reply;
+  append_u32_be(reply, static_cast<uint32_t>(2.0f * kBasicmicroPositionPidScale)); // P (offset 0)
+  append_u32_be(reply, static_cast<uint32_t>(0.5f * kBasicmicroPositionPidScale)); // I (offset 4)
+  append_u32_be(reply, static_cast<uint32_t>(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<float>(read_u32_be(reply, 0)) / kBasicmicroPositionPidScale == 2.0f); // P
+  CHECK(static_cast<float>(read_u32_be(reply, 4)) / kBasicmicroPositionPidScale == 0.5f); // I
+  CHECK(static_cast<float>(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() {