feat(mcp266): set/zero encoder counts and read back the position PID record

components/mcp266/README.md

@@ -34,7 +34,12 @@ object dictionary at index `0x2000 + command number` (see
 `include/detail/mcp266_core.hpp`, which is host-buildable and unit-tested).
 This component uses that to:
 
-* configure the position PID (commands 61-64),
+* configure the position PID (commands 61-64), read the record back
+  (`read_position_pid`, `read_position_limits`) and install tuned gains
+  (`set_position_pid`),
+* set an axis's encoder count (22/23, `set_encoder`) or zero both (20,
+  `reset_encoders`) — how a quadrature encoder is homed against a limit switch
+  or restored to a remembered position after power-up,
 * issue the manufacturer speed/duty commands (32/33, 35/36), and
 * read telemetry: main battery (24) and temperature (82).
 

components/mcp266/example/README.md

@@ -8,7 +8,8 @@ Basicmicro MCP266 (RoboClaw-family) motor controller over **CANopen**. It:
 2. NMT-starts the node and clears any latched CiA 402 faults,
 3. reads the main battery voltage and board temperature,
 4. configures the M1 position loop (widening the position clamp and seeding a
-   non-zero P gain — required once per boot), and
+   non-zero P gain — required once per boot) and reads the resulting PID
+   record back, and
 5. runs a small profile-position sequence on M1, reporting arrival at each
    target.
 

components/mcp266/example/main/mcp266_example.cpp

@@ -102,6 +102,15 @@ extern "C" void app_main(void) {
     logger.error("Failed to configure M1 position loop: {}", ec.message());
     return;
   }
+  // The record the controller now holds: gains as tuned in Motion Studio (or
+  // the seeded fallback), and the clamp just installed.
+  float p = 0, i = 0, d = 0;
+  uint32_t max_i = 0, deadzone = 0;
+  int32_t min_pos = 0, max_pos = 0;
+  if (mcp.read_position_pid(Axis::M1, p, i, d, max_i, deadzone, min_pos, max_pos, ec)) {
+    logger.info("M1 position PID: P={:.3f} I={:.3f} D={:.3f} maxI={} deadzone={} clamp=[{}, {}]", p,
+                i, d, max_i, deadzone, min_pos, max_pos);
+  }
   if (!mcp.set_software_position_limits(Axis::M1, -20'000, 20'000, ec)) {
     logger.error("Failed to set M1 software position limits: {}", ec.message());
     return;

components/mcp266/include/detail/mcp266_core.hpp

@@ -56,30 +56,76 @@ inline constexpr uint16_t kTemperatureObject =
     command_object(BasicmicroCommand::ReadTemperature); ///< tenths of a degree C (u16)
 inline constexpr uint16_t kEStopResetObject =
     command_object(BasicmicroCommand::EStopReset); ///< write-only
+inline constexpr uint16_t kResetEncodersObject =
+    command_object(BasicmicroCommand::ResetEncoders); ///< write-only, zeros both counters
 /// @}
 
+/// \brief Scale of the position PID gains in the record: the controller stores
+///        P, I and D as fixed point x1024 (same as the packet-serial driver).
+inline constexpr int32_t kPositionGainScale = 1024;
+
+/// \brief Convert a floating-point position PID gain to the record's fixed-point
+///        representation (x kPositionGainScale).
+/// \details Mirrors espp::Basicmicro's scale_pid_gain(): rounds to nearest rather
+///          than truncating (truncation biases every gain downward by up to one
+///          LSB) and clamps to the full unsigned 32-bit range of the record's
+///          gain fields. Gains are non-negative on these controllers, and a raw
+///          cast of a negative or non-finite product to an integer is either
+///          silently wrong or undefined. The record's P, I, D, MaxI and Deadzone
+///          fields are unsigned; only MinPos / MaxPos are signed. The SDO
+///          transfer is a 4-byte bit pattern either way, so the value is carried
+///          through the i32 helpers unchanged (see position_gain_bits()).
+/// \param gain The gain as a float.
+/// \return The fixed-point gain, clamped to [0, UINT32_MAX].
+inline constexpr uint32_t scale_position_gain(float gain) {
+  if (!(gain > 0.0f)) { // false for <= 0 and for NaN
+    return 0;
+  }
+  const double scaled = static_cast<double>(gain) * static_cast<double>(kPositionGainScale);
+  if (!(scaled < 4294967295.5)) { // also false for +inf; saturate instead of overflowing
+    return UINT32_MAX;
+  }
+  return static_cast<uint32_t>(scaled + 0.5); // scaled > 0, so this rounds to nearest
+}
+
+/// \brief Reinterpret an unsigned record field (gain, MaxI, Deadzone) as the
+///        int32_t slot of the seven-field record array, preserving the bit
+///        pattern for the 4-byte SDO write.
+inline constexpr int32_t position_gain_bits(uint32_t raw) { return static_cast<int32_t>(raw); }
+
+/// \brief Convert a fixed-point gain field read back from the record (an
+///        unsigned 32-bit bit pattern carried in an int32_t slot) to a float.
+inline constexpr float position_gain_from_bits(int32_t raw) {
+  return static_cast<float>(static_cast<uint32_t>(raw)) / static_cast<float>(kPositionGainScale);
+}
+
 /// \brief The manufacturer command objects and CiA 402 offset for one axis.
 struct AxisObjects {
   uint16_t object_offset;    ///< 0 for M1, 0x800 for M2 (added to 0x60xx objects).
   uint16_t position_pid_set; ///< Position PID setter (command 61/62), write-only.
   uint16_t position_pid_get; ///< Position PID readback (command 63/64).
   uint16_t drive_duty;       ///< Signed-duty command (32/33).
   uint16_t drive_speed;      ///< Signed-speed command (35/36).
+  uint16_t encoder_set;      ///< Encoder count setter (command 22/23), write-only.
 };
 
 /// \brief Objects for motor 1 (the standard axis).
 inline constexpr AxisObjects axis_m1() {
-  return {kAxisOffsetM1, command_object(BasicmicroCommand::SetPositionPidM1),
+  return {kAxisOffsetM1,
+          command_object(BasicmicroCommand::SetPositionPidM1),
           command_object(BasicmicroCommand::ReadPositionPidM1),
           command_object(BasicmicroCommand::DriveM1SignedDuty),
-          command_object(BasicmicroCommand::DriveM1SignedSpeed)};
+          command_object(BasicmicroCommand::DriveM1SignedSpeed),
+          command_object(BasicmicroCommand::SetEncoderM1)};
 }
 /// \brief Objects for motor 2 (mirrored at +0x800 / command n+1).
 inline constexpr AxisObjects axis_m2() {
-  return {kAxisOffsetM2, command_object(BasicmicroCommand::SetPositionPidM2),
+  return {kAxisOffsetM2,
+          command_object(BasicmicroCommand::SetPositionPidM2),
           command_object(BasicmicroCommand::ReadPositionPidM2),
           command_object(BasicmicroCommand::DriveM2SignedDuty),
-          command_object(BasicmicroCommand::DriveM2SignedSpeed)};
+          command_object(BasicmicroCommand::DriveM2SignedSpeed),
+          command_object(BasicmicroCommand::SetEncoderM2)};
 }
 
 /// \brief Remap a position-PID record from the readback order to the setter

components/mcp266/include/mcp266.hpp

@@ -30,9 +30,10 @@ namespace espp {
 ///          control-loop parameters are NOT standard CiA 402 objects: the MCP
 ///          mirrors its packet-serial command set into the manufacturer region
 ///          at object index 0x2000 + command number, which this class uses to
-///          configure the position PID (commands 61-64), issue the
-///          manufacturer speed/duty commands (32/33, 35/36), and read
-///          telemetry (24, 82). See detail/mcp266_core.hpp.
+///          configure and read back the position PID (commands 61-64), set or
+///          zero the encoder counts (22/23, 20), issue the manufacturer
+///          speed/duty commands (32/33, 35/36), and read telemetry (24, 82).
+///          See detail/mcp266_core.hpp.
 ///
 ///          \b Important: two device quirks must be handled, both done by
 ///          configure_position_loop():
@@ -157,13 +158,8 @@ class Mcp266 : public BaseComponent {
       return false;
     AxisState &a = axis_state(axis);
     std::array<int32_t, 7> readback{};
-    for (uint8_t sub = 1; sub <= 7; ++sub) {
-      readback[sub - 1] = client_.read_i32(a.objects.position_pid_get, sub, ec);
-      if (ec) {
-        logger_.error("{}: position PID read 0x{:04X}:{} failed: {}", a.name,
-                      a.objects.position_pid_get, sub, ec.message());
-        return false;
-      }
+    if (!read_position_pid_raw(a, readback, ec)) {
+      return false;
     }
     // readback order is [P, I, D, MaxI, Deadzone, MinPos, MaxPos]
     if (readback[0] == 0) {
@@ -174,18 +170,14 @@ class Mcp266 : public BaseComponent {
     }
     readback[5] = min_pos;
     readback[6] = max_pos;
-    const auto setter = detail::mcp266::position_pid_readback_to_setter(readback);
-    for (uint8_t sub = 1; sub <= 7; ++sub) {
-      if (!client_.write_i32(a.objects.position_pid_set, sub, setter[sub - 1], ec)) {
-        logger_.error("{}: position PID write 0x{:04X}:{} rejected: {}", a.name,
-                      a.objects.position_pid_set, sub, ec.message());
-        return false;
-      }
+    if (!write_position_pid_raw(a, detail::mcp266::position_pid_readback_to_setter(readback), ec)) {
+      return false;
     }
     // verify via the readback's field order (min/max are subs 6/7 there too)
-    const int32_t got_min = client_.read_i32(a.objects.position_pid_get, 6, ec);
-    const int32_t got_max = client_.read_i32(a.objects.position_pid_get, 7, ec);
-    if (ec || got_min != min_pos || got_max != max_pos) {
+    int32_t got_min = 0;
+    int32_t got_max = 0;
+    if (!read_position_limits(axis, got_min, got_max, ec) || got_min != min_pos ||
+        got_max != max_pos) {
       logger_.error("{}: position clamp did not take (read [{}, {}], wanted [{}, {}])", a.name,
                     got_min, got_max, min_pos, max_pos);
       ec = std::make_error_code(std::errc::protocol_error);
@@ -202,6 +194,114 @@ class Mcp266 : public BaseComponent {
     return configure_position_loop(axis, min_pos, max_pos, kDefaultPositionP, ec);
   }
 
+  /// \brief Read an axis's position PID record (mirrored command 63/64). Gains
+  ///        are converted to floats (the record stores them x1024), matching
+  ///        espp::Basicmicro::read_position_pid().
+  /// \param axis The motor channel.
+  /// \param p Out: proportional gain.
+  /// \param i Out: integral gain.
+  /// \param d Out: derivative gain.
+  /// \param max_i Out: maximum integral windup.
+  /// \param deadzone Out: deadzone in encoder counts.
+  /// \param min_pos Out: minimum commandable position (the clamp).
+  /// \param max_pos Out: maximum commandable position (the clamp).
+  /// \param ec Set on failure.
+  /// \return True on success.
+  bool read_position_pid(Axis axis, 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) {
+    ec.clear();
+    if (!check_axis(axis, ec))
+      return false;
+    std::array<int32_t, 7> readback{};
+    if (!read_position_pid_raw(axis_state(axis), readback, ec)) {
+      return false;
+    }
+    // gain / MaxI / Deadzone fields are unsigned on the record; the i32 read
+    // only carried their bit pattern
+    p = detail::mcp266::position_gain_from_bits(readback[0]);
+    i = detail::mcp266::position_gain_from_bits(readback[1]);
+    d = detail::mcp266::position_gain_from_bits(readback[2]);
+    max_i = static_cast<uint32_t>(readback[3]);
+    deadzone = static_cast<uint32_t>(readback[4]);
+    min_pos = readback[5];
+    max_pos = readback[6];
+    return true;
+  }
+
+  /// \brief Write an axis's whole position PID record (mirrored command
+  ///        61/62), taking care of the setter's D, P, I field order. Gains are
+  ///        floats, stored x1024 (rounded to nearest and clamped non-negative,
+  ///        as espp::Basicmicro::set_position_pid() does). Prefer
+  ///        configure_position_loop() when only the clamp needs setting; this
+  ///        is for installing tuned gains.
+  /// \param axis The motor channel.
+  /// \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 (the clamp).
+  /// \param max_pos Maximum commandable position (the clamp).
+  /// \param ec Set on failure.
+  /// \return True on success.
+  bool set_position_pid(Axis axis, 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) {
+    ec.clear();
+    if (!check_axis(axis, ec))
+      return false;
+    if (min_pos > max_pos) {
+      ec = std::make_error_code(std::errc::invalid_argument);
+      return false;
+    }
+    const AxisState &a = axis_state(axis);
+    using detail::mcp266::position_gain_bits;
+    using detail::mcp266::scale_position_gain;
+    // the unsigned fields travel as 4-byte bit patterns in the i32 slots
+    const std::array<int32_t, 7> readback_order{position_gain_bits(scale_position_gain(p)),
+                                                position_gain_bits(scale_position_gain(i)),
+                                                position_gain_bits(scale_position_gain(d)),
+                                                position_gain_bits(max_i),
+                                                position_gain_bits(deadzone),
+                                                min_pos,
+                                                max_pos};
+    if (!write_position_pid_raw(a, detail::mcp266::position_pid_readback_to_setter(readback_order),
+                                ec)) {
+      return false;
+    }
+    logger_.info("{}: position PID written (P={}, I={}, D={}, clamp=[{}, {}])", a.name,
+                 static_cast<uint32_t>(readback_order[0]), static_cast<uint32_t>(readback_order[1]),
+                 static_cast<uint32_t>(readback_order[2]), min_pos, max_pos);
+    return true;
+  }
+
+  /// \brief Read just the MinPos/MaxPos clamp of an axis's position PID record
+  ///        (two SDOs instead of the seven of read_position_pid()). Useful as a
+  ///        cheap check that the controller still holds the configuration it
+  ///        was given: it reverts to its EEPROM (factory clamp [0, 0]) on a
+  ///        power cycle.
+  /// \param axis The motor channel.
+  /// \param min_pos Out: minimum commandable position.
+  /// \param max_pos Out: maximum commandable position.
+  /// \param ec Set on failure.
+  /// \return True on success.
+  bool read_position_limits(Axis axis, int32_t &min_pos, int32_t &max_pos, std::error_code &ec) {
+    ec.clear();
+    if (!check_axis(axis, ec))
+      return false;
+    const AxisState &a = axis_state(axis);
+    min_pos = client_.read_i32(a.objects.position_pid_get, 6, ec);
+    if (!ec) {
+      max_pos = client_.read_i32(a.objects.position_pid_get, 7, ec);
+    }
+    if (ec) {
+      logger_.error("{}: position limits read 0x{:04X} failed: {}", a.name,
+                    a.objects.position_pid_get, ec.message());
+      return false;
+    }
+    return true;
+  }
+
   /// \brief Set the CiA 402 software position limits (object 0x607D:1/:2) for an
   ///        axis — a per-move envelope enforced by the drive's trajectory
   ///        generator. Distinct from configure_position_loop()'s min/max, which
@@ -300,6 +400,44 @@ class Mcp266 : public BaseComponent {
     count = axis_state(axis).drive.get_position_actual(ec);
     return !ec;
   }
+  /// \brief Set an axis's encoder count (mirrored packet-serial command 22/23).
+  ///        For a quadrature encoder this defines the count at the current
+  ///        position, e.g. to home against a limit switch or to restore a
+  ///        remembered position after power-up. Takes effect at once; the
+  ///        position loop then sees the new count.
+  /// \param axis Channel.
+  /// \param count The count to install.
+  /// \param ec Set on failure.
+  /// \return True on success.
+  bool set_encoder(Axis axis, int32_t count, std::error_code &ec) {
+    ec.clear();
+    if (!check_axis(axis, ec))
+      return false;
+    const AxisState &a = axis_state(axis);
+    if (!client_.write_i32(a.objects.encoder_set, 0, count, ec)) {
+      logger_.error("{}: set encoder 0x{:04X} rejected: {}", a.name, a.objects.encoder_set,
+                    ec.message());
+      return false;
+    }
+    logger_.info("{}: encoder count set to {}", a.name, count);
+    return true;
+  }
+  /// \brief Zero both encoder counters (mirrored packet-serial command 20),
+  ///        matching espp::Basicmicro::reset_encoders().
+  /// \param ec Set on failure.
+  /// \return True on success.
+  bool reset_encoders(std::error_code &ec) {
+    ec.clear();
+    // Command 20 has no payload, so the SDO scalar width is unknown; try the
+    // common ones (as reset_estop() does).
+    bool ok = client_.write_u8(detail::mcp266::kResetEncodersObject, 0, 1, ec);
+    if (!ok) {
+      ec.clear();
+      ok = client_.write_u32(detail::mcp266::kResetEncodersObject, 0, 1, ec);
+    }
+    logger_.info("encoder reset {}", ok ? "accepted" : "rejected");
+    return ok;
+  }
   /// \brief Read the actual velocity (0x606C / 0x686C).
   /// \param axis Channel.
   /// \param qpps Out: counts/s.
@@ -441,6 +579,37 @@ class Mcp266 : public BaseComponent {
 
   AxisState &axis_state(Axis axis) { return axis == Axis::M1 ? m1_ : m2_; }
 
+  /// Read the seven-field position PID record in the readback's order
+  /// [P, I, D, MaxI, Deadzone, MinPos, MaxPos] (subindices 1..7 of command 63/64).
+  /// The first five fields are unsigned on the device; the i32 read carries
+  /// their 4-byte bit pattern, which the callers reinterpret.
+  bool read_position_pid_raw(const AxisState &a, std::array<int32_t, 7> &readback,
+                             std::error_code &ec) {
+    for (uint8_t sub = 1; sub <= 7; ++sub) {
+      readback[sub - 1] = client_.read_i32(a.objects.position_pid_get, sub, ec);
+      if (ec) {
+        logger_.error("{}: position PID read 0x{:04X}:{} failed: {}", a.name,
+                      a.objects.position_pid_get, sub, ec.message());
+        return false;
+      }
+    }
+    return true;
+  }
+
+  /// Write the seven-field record already in the setter's order
+  /// [D, P, I, MaxI, Deadzone, MinPos, MaxPos] (subindices 1..7 of command 61/62).
+  bool write_position_pid_raw(const AxisState &a, const std::array<int32_t, 7> &setter,
+                              std::error_code &ec) {
+    for (uint8_t sub = 1; sub <= 7; ++sub) {
+      if (!client_.write_i32(a.objects.position_pid_set, sub, setter[sub - 1], ec)) {
+        logger_.error("{}: position PID write 0x{:04X}:{} rejected: {}", a.name,
+                      a.objects.position_pid_set, sub, ec.message());
+        return false;
+      }
+    }
+    return true;
+  }
+
   /// Write the axis mode of operation directly (the MCP does not echo the
   /// requested mode in 0x6061, so Ds402Drive::set_mode() -- which verifies the
   /// display -- would time out), clear any fault, and walk to Operation