MCP266 CANopen Motor Controller Component

Overview

espp::Mcp266 is a dual-channel controller for a Basicmicro MCP266 (RoboClaw family) brushed DC motor driver over CANopen. It is layered on espp::CanopenClient (like espp::Ds402Drive), so it is transport-agnostic: the application owns the CAN transport, feeds received frames to the client’s process_frame(), and the client’s node id selects the MCP266.

Both motor channels (M1, M2) are driven symmetrically. M2’s CiA 402 objects mirror M1’s at +0x800, handled through Ds402Drive’s object offset. The reverse-engineered object mapping lives in espp::detail inside include/detail/mcp266_core.hpp, a host-buildable core that depends only on the C++20 standard library and is unit-tested off-target.

What works, and what does not

Position control uses the standard CiA 402 profile position mode (move_to_position) and is the supported, validated capability. It needs the position loop configured first (configure_position_loop).

Velocity / duty control is not functional on the MCP266 firmware tested. Both the standard target objects and the manufacturer speed/duty command mirror are accepted by the drive but leave the velocity generator idle even with the drive in Operation Enabled. Supported-drive-modes (0x6502) advertises only the cyclic-sync modes, so velocity likely requires csv mode with cyclic SYNC/PDO updates, which is undocumented for this device. drive_speed / drive_duty are implemented but are currently a no-op for motion.

Device specifics

The MCP266’s control-loop parameters are not standard CiA 402 objects. The MCP mirrors its packet-serial command set into the manufacturer region of the object dictionary at index 0x2000 + command number. This component uses that to configure the position PID (commands 61-64), issue the manufacturer speed/duty commands (32/33, 35/36), and read telemetry (main battery 24, temperature 82).

Two device quirks are handled by configure_position_loop(), which must be called once per boot (the MCP reverts to its EEPROM configuration on power-up):

• The position PID’s MinPos/MaxPos clamp defaults to [0, 0], which forces every position target to zero — it is widened here.

• The setter (commands 61/62) uses field order D, P, I while the readback (63/64) uses P, I, D, so a naive read-modify-write of the record would move P into the D slot and zero P. The correct field shuffle (and seeding a non-zero P when the record has none) is done here.

• MCP266 CANopen Example
  • Requirements
  • Hardware
  • Build

API Reference

Header File

• components/mcp266/include/mcp266.hpp

Classes

class Mcp266 : public espp::BaseComponent

Dual-channel controller for a Basicmicro MCP266 (RoboClaw family) brushed-DC motor driver over CANopen.

Layered on a CanopenClient (like Ds402Drive), so it is transport-agnostic: the application owns the CAN transport, feeds received frames to the client’s process_frame(), and the client’s node id selects the MCP266. Both motor channels (M1, M2) are driven symmetrically; M2’s CiA 402 objects mirror M1’s at +0x800, handled through Ds402Drive’s object offset.

Position control uses the standard CiA 402 profile position mode and is the supported, validated capability. The MCP266’s control-loop parameters are NOT standard CiA 402 objects: the MCP mirrors its packet-serial command set into the manufacturer region at object index 0x2000 + command number, which this class uses to configure the position PID (commands 61-64), issue the manufacturer speed/duty commands (32/33, 35/36), and read telemetry (24, 82). See detail/mcp266_core.hpp.

Important: two device quirks must be handled, both done by configure_position_loop():

• The position PID’s MinPos/MaxPos clamp defaults to [0, 0], which forces every position target to zero.

• The setter (commands 61/62) uses field order D, P, I while the readback (63/64) uses P, I, D, so a naive read-modify-write of the record moves P into the D slot and zeros P. The MCP reverts to its EEPROM configuration on power-up, so call configure_position_loop() once per boot before commanding moves.

Note: the manufacturer speed/duty command mirror (drive_speed / drive_duty) is implemented but does NOT produce motion on the MCP266 firmware tested (the command is accepted but the velocity generator stays idle). Use position mode for motion.

MCP266 Example

  // The CANopen node id configured on the MCP266 (Motion Studio -> CAN
  // settings). Change to match your device.
  static constexpr uint8_t node_id = 10;

  // Forward-declared handle so the Twai on_receive callback (registered at
  // Twai construction) can feed frames to the client constructed below.
  static espp::CanopenClient *client_ptr = nullptr;

  // Bring up the TWAI (CAN 2.0) peripheral. Talking to a real MCP266 requires
  // Mode::NORMAL with a 3.3 V CAN transceiver on the tx/rx GPIOs, a properly
  // terminated bus, and a matching baudrate (set in Motion Studio).
  // NOTE: twai / client / mcp are function-local statics: the Twai receive
  // task and the client's send lambda reference them, and app_main() has
  // early-return paths, so static storage keeps them alive for every callback.
  static espp::Twai twai({
      .tx_gpio = 17, // change to match your board / transceiver
      .rx_gpio = 16,
      .baudrate = 1000000,
      .mode = espp::Twai::Mode::NORMAL,
      .tx_queue_depth = 10,
      .on_receive =
          [](const espp::Twai::Message &msg) {
            if (client_ptr) {
              client_ptr->process_frame(espp::CanopenClient::CanFrame{
                  .id = msg.id,
                  .extended = msg.extended,
                  .rtr = msg.rtr,
                  .dlc = msg.dlc,
                  .data = msg.data,
              });
            }
          },
      .log_level = espp::Logger::Verbosity::WARN,
  });

  static espp::CanopenClient client({
      .node_id = node_id,
      // captureless: twai has static storage duration and is referenced
      // directly (capturing a static is ill-formed under -Werror)
      .send =
          [](const espp::CanopenClient::CanFrame &frame) {
            espp::Twai::Message msg{
                .id = frame.id,
                .extended = frame.extended,
                .rtr = frame.rtr,
                .dlc = frame.dlc,
                .data = frame.data,
            };
            std::error_code tx_ec;
            return twai.transmit(msg, tx_ec);
          },
      .sdo_timeout = 500ms,
      .log_level = espp::Logger::Verbosity::WARN,
  });
  client_ptr = &client;

  static espp::Mcp266 mcp(client, {.log_level = espp::Logger::Verbosity::INFO});

  std::error_code ec;
  if (!twai.initialize(ec)) {
    logger.error("Failed to initialize TWAI: {}", ec.message());
    return;
  }
  // NMT-start the node and clear any latched faults on both axes.
  if (!mcp.start(ec)) {
    logger.error("Failed to start MCP266: {} -- is the node on the bus?", ec.message());
    return;
  }

  // Telemetry sanity check.
  float volts = 0.0f, temp_c = 0.0f;
  if (mcp.read_main_battery_voltage(volts, ec)) {
    logger.info("Main battery: {:.1f} V", volts);
  }
  if (mcp.read_temperature(temp_c, ec)) {
    logger.info("Board temperature: {:.1f} C", temp_c);
  }

  using Axis = espp::Mcp266::Axis;

  // One-time per-boot position-loop setup on M1: widen the MinPos/MaxPos clamp
  // (factory [0, 0] forces every target to zero) and ensure a non-zero
  // position P gain. The MCP reverts to EEPROM on power-up, so this must run
  // every boot before commanding moves. Clear any latched e-stop first.
  mcp.reset_estop(ec);
  if (!mcp.configure_position_loop(Axis::M1, -2'000'000'000, 2'000'000'000, ec)) {
    logger.error("Failed to configure M1 position loop: {}", ec.message());
    return;
  }
  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;
  }

  // Run a small profile-position sequence and report arrival.
  static constexpr int32_t targets[] = {10'000, -10'000, 0};
  static constexpr uint32_t profile_velocity = 500; // counts/s
  static constexpr uint32_t profile_accel = 500;    // counts/s^2
  static constexpr uint32_t profile_decel = 500;    // counts/s^2
  static constexpr int32_t tolerance = 100;         // counts
  for (int32_t target : targets) {
    logger.info("Moving M1 to {}", target);
    if (!mcp.move_to_position(Axis::M1, target, profile_velocity, profile_accel, profile_decel,
                              ec)) {
      logger.error("Move command rejected: {}", ec.message());
      continue;
    }
    const auto deadline = std::chrono::steady_clock::now() + 30s;
    while (std::chrono::steady_clock::now() < deadline) {
      std::this_thread::sleep_for(250ms);
      int32_t position = 0;
      if (mcp.read_encoder(Axis::M1, position, ec) && std::abs(position - target) <= tolerance) {
        logger.info("  reached {} (position={})", target, position);
        break;
      }
    }
  }

Position control (CiA 402 profile position mode)

inline bool configure_position_loop(Axis axis, int32_t min_pos, int32_t max_pos, int32_t fallback_p, std::error_code &ec)

Configure an axis’s position loop for use: widen the MinPos/MaxPos clamp (factory [0, 0] forces every target to zero) and, only if the drive’s stored position P gain reads back as zero, seed a non-zero P so the loop produces output. The record is written through the setter’s D, P, I field order and the clamp verified via the readback. The MCP reverts to EEPROM on power-up, so call once per boot.

Note

fallback_p is a coarse starting value used ONLY when the drive has no stored P gain; it is not motor-tuned. For good motion, tune the position PID in Basicmicro Motion Studio (or pass a value appropriate for your motor / encoder) — an unsuitable P gain can leave the axis sluggish or make it oscillate. A drive whose P gain is already non-zero keeps its stored gains untouched.

Parameters:

• axis – The motor channel.

• min_pos – Minimum commandable position.

• max_pos – Maximum commandable position.

• fallback_p – Position P gain to seed when the stored gain is zero.

• ec – Set on failure.

Returns:

True on success.

inline bool configure_position_loop(Axis axis, int32_t min_pos, int32_t max_pos, std::error_code &ec)

Configure an axis’s position loop using the default coarse fallback P gain (see the fallback_p overload). Convenience for the common case.

inline bool set_software_position_limits(Axis axis, int32_t min_pos, int32_t max_pos, std::error_code &ec)

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 writes the manufacturer position-PID MinPos/MaxPos clamp.

Parameters:

• axis – The motor channel.

• min_pos – Lower limit.

• max_pos – Upper limit.

• ec – Set on failure.

Returns:

True on success.

inline bool move_to_position(Axis axis, int32_t target_position, uint32_t profile_velocity, uint32_t profile_acceleration, uint32_t profile_deceleration, std::error_code &ec)

Command a profile-position move: enable the axis, set the motion profile, and issue the target with the new-set-point handshake.

Parameters:

• axis – The motor channel.

• target_position – Absolute target position (encoder counts).

• profile_velocity – Cruise velocity.

• profile_acceleration – Acceleration.

• profile_deceleration – Deceleration.

• ec – Set on failure.

Returns:

True once the set-point is accepted.

Manufacturer speed / duty command mirror

Note

Accepted by the drive but inert on the MCP266 firmware tested; kept for completeness and in case a firmware update activates them.

inline bool drive_speed(Axis axis, int32_t qpps, std::error_code &ec)

Closed-loop speed via the mirrored packet-serial command (35/36).

inline bool drive_duty(Axis axis, int16_t duty, std::error_code &ec)

Open-loop duty via the mirrored packet-serial command (32/33).

Feedback

inline bool read_encoder(Axis axis, int32_t &count, std::error_code &ec)

Read the actual position (0x6064 / 0x6864).

Parameters:

• axis – Channel.

• count – Out: encoder counts.

• ec – Set on failure.

Returns:

True on success.

inline bool read_speed(Axis axis, int32_t &qpps, std::error_code &ec)

Read the actual velocity (0x606C / 0x686C).

Parameters:

• axis – Channel.

• qpps – Out: counts/s.

• ec – Set on failure.

Returns:

True on success.

inline bool read_statusword(Axis axis, uint16_t &statusword, std::error_code &ec)

Read the CiA 402 statusword (0x6041 / 0x6841).

Parameters:

• axis – Channel.

• statusword – Out.

• ec – Set on failure.

Returns:

True on success.

inline bool get_state(Axis axis, Ds402Drive::State &state, std::error_code &ec)

Read the decoded CiA 402 drive state of an axis (from its statusword).

Parameters:

• axis – Channel.

• state – Out: the power-drive-system state.

• ec – Set on failure.

Returns:

True on success.

inline bool is_target_reached(Axis axis, bool &reached, std::error_code &ec)

Whether an axis reports “target reached” (statusword bit 10) — the authoritative arrival signal for profile moves.

Parameters:

• axis – Channel.

• reached – Out.

• ec – Set on failure.

Returns:

True on success.

Device telemetry

inline bool read_main_battery_voltage(float &volts, std::error_code &ec)

Read the main battery voltage (mirrored command 24).

Parameters:

• volts – Out: volts.

• ec – Set on failure.

Returns:

True on success.

inline bool read_temperature(float &temp_c, std::error_code &ec)

Read the board temperature (mirrored command 82).

Parameters:

• temp_c – Out: degrees C.

• ec – Set on failure.

Returns:

True on success.

inline bool read_device_info(std::string &device_name, uint32_t &device_type, std::error_code &ec)

Read the standard device type (0x1000) and name (0x1008).

Public Types

using Axis = MotorAxis

Motor channel selector (shared across the espp motor drivers).

Public Functions

inline explicit Mcp266(CanopenClient &client, const Config &config)

Create an MCP266 controller.

Parameters:

• client – The CANopen client for the MCP266’s node. Must outlive this object, and its process_frame() must be driven from another task (e.g. the transport’s receive task) per CanopenClient’s contract.

• config – The configuration.

inline explicit Mcp266(CanopenClient &client)

Create an MCP266 controller with the default configuration.

inline bool start(std::error_code &ec)

NMT-start the node and clear any latched CiA 402 faults on both axes. Call once after the transport and client are up.

Parameters:

ec – Set on failure.

Returns:

True on success.

inline bool reset_faults(std::error_code &ec)

Clear any latched CiA 402 fault on both axes.

Parameters:

ec – Set on failure.

Returns:

True on success.

inline bool reset_estop(std::error_code &ec)

Attempt an E-stop reset (mirrored packet-serial command 200 at 0x20C8). Harmless when nothing is latched.

Parameters:

ec – Set on failure.

Returns:

True if accepted.

inline Ds402Drive *drive(Axis axis)

Access an axis’s underlying Ds402Drive for advanced CiA 402 use.

Parameters:

axis – The motor channel.

Returns:

Pointer to the axis drive helper, or nullptr if axis is not a valid channel (M1 / M2) — so an axis decoded from an untrusted byte cannot silently return the M2 drive.

inline const std::string &get_name() const

Get the name of the component

Note

This is the tag of the logger

Returns:

A const reference to the name of the component

inline void set_log_tag(const std::string_view &tag)

Set the tag for the logger

Parameters:

tag – The tag to use for the logger

inline espp::Logger::Verbosity get_log_level() const

Get the log level for the logger

See also

Logger::Verbosity

See also

Logger::set_verbosity

Returns:

The verbosity level of the logger

inline void set_log_level(espp::Logger::Verbosity level)

Set the log level for the logger

See also

Logger::Verbosity

See also

Logger::set_verbosity

Parameters:

level – The verbosity level to use for the logger

inline void set_log_verbosity(espp::Logger::Verbosity level)

Set the log verbosity for the logger

See also

set_log_level

See also

Logger::Verbosity

See also

Logger::set_verbosity

Note

This is a convenience method that calls set_log_level

Parameters:

level – The verbosity level to use for the logger

inline espp::Logger::Verbosity get_log_verbosity() const

Get the log verbosity for the logger

See also

get_log_level

See also

Logger::Verbosity

See also

Logger::get_verbosity

Note

This is a convenience method that calls get_log_level

Returns:

The verbosity level of the logger

inline void set_log_rate_limit(std::chrono::duration<float> rate_limit)

Set the rate limit for the logger

See also

Logger::set_rate_limit

Note

Only calls to the logger that have _rate_limit suffix will be rate limited

Parameters:

rate_limit – The rate limit to use for the logger

struct Config

Configuration for the Mcp266 controller.

Public Members

std::chrono::milliseconds state_timeout = {1000}

Per-axis CiA 402 transition timeout.

std::chrono::milliseconds poll_period = {25}

Statusword polling period.

Logger::Verbosity log_level = {Logger::Verbosity::WARN}

Logger verbosity.

Header File

• components/mcp266/include/mcp266_protocol.hpp

Header File

• components/mcp266/include/mcp266_service.hpp

Classes

class Mcp266Service : public espp::BaseComponent

Dispatcher service exposing an espp::Mcp266 over the MCP266 console protocol (module 6 by default).

Requests are executed against the driver under the MCP mutex (SDO transactions block, so feed this service from a worker task, e.g. an espp::DispatcherWorker) and the reply is sent after the mutex is released. A STATUS snapshot is eight SDO reads; the host-requested stream period is clamped to [Config::min_stream_period_ms, Config::max_stream_period_ms] so the stream can neither starve command handling nor flood the CAN bus.

Mcp266Service Example

  // Vendor (WebUSB) and CDC (Web Serial) are independent byte streams, so each
  // gets its own Mcp266Service (replies and the STATUS stream go back on the
  // stream the host is using) and its own DispatcherWorker (one bounded RX
  // queue + worker task feeding one parser, so the blocking SDO command
  // handler never runs in the TinyUSB callback context). Both services share
  // the one driver and its mutex.
  //
  // Each transport's `send` is called from TWO tasks -- the dispatcher worker
  // (request + discovery replies) and the service's status-stream task -- and
  // UsbDevice's write_*() do not serialize writers, so guard each with a mutex
  // to keep frames atomic on the wire.
  std::mutex tx_mutex;
  auto vendor_send = [&](std::span<const uint8_t> f) {
    std::lock_guard<std::mutex> lock(tx_mutex);
    if (!usb.write_vendor(f))
      logger.warn_rate_limited("dropped a {}-byte vendor frame (TX backpressure)", f.size());
  };
  auto cdc_send = [&](std::span<const uint8_t> f) {
    std::lock_guard<std::mutex> lock(tx_mutex);
    if (!usb.write_cdc(f))
      logger.warn_rate_limited("dropped a {}-byte CDC frame (TX backpressure)", f.size());
  };
  espp::Mcp266Service vendor_service(
      mcp, {.send = vendor_send,
            .mcp_mutex = &mcp_mutex,
            .status_task_config = {.name = "mcp266_status_v", .stack_size_bytes = 8192},
            .log_level = espp::Logger::Verbosity::INFO});
  espp::Mcp266Service cdc_service(
      mcp, {.send = cdc_send,
            .mcp_mutex = &mcp_mutex,
            .status_task_config = {.name = "mcp266_status_c", .stack_size_bytes = 8192},
            .log_level = espp::Logger::Verbosity::INFO});

  espp::DispatcherWorker vendor_link(
      {.send = vendor_send, .task_config = {.name = "mcp266_vendor", .stack_size_bytes = 16384}});
  espp::DispatcherWorker cdc_link(
      {.send = cdc_send, .task_config = {.name = "mcp266_cdc", .stack_size_bytes = 16384}});
  vendor_link.register_module(vendor_service); // module 6 + its discovery metadata
  cdc_link.register_module(cdc_service);
  vendor_link.serve_discovery(usb_cfg.product);
  cdc_link.serve_discovery(usb_cfg.product);

  // The TinyUSB callbacks just queue bytes for the workers.
  usb.set_vendor_receive_callback([&](std::span<const uint8_t> data) { vendor_link.push(data); });
  usb.set_cdc_receive_callback([&](std::span<const uint8_t> data) { cdc_link.push(data); });

Public Types

using send_fn = std::function<void(std::span<const uint8_t> frame)>

Transmits one encoded frame to the host.

Public Functions

inline explicit Mcp266Service(Mcp266 &mcp, const Config &config)

Construct the service (and start its status-streaming task, idle until a host enables the stream).

Parameters:

• mcp – The driver to expose (must outlive the service).

• config – Configuration parameters.

inline uint8_t module_id() const

The dispatcher module id this service answers on.

inline Dispatcher::ModuleInfo module_info() const

Discovery metadata for registering this service on a Dispatcher.

inline std::mutex &mcp_mutex()

The mutex serializing use of the driver (take it around your own Mcp266 calls when sharing the driver with this service).

inline void handle(const espp::stream_frame::Frame &frame)

Dispatcher entry point: handle one routed frame. Frames for other modules and reply-flagged frames (echoes) are ignored.

inline Status read_status()

Take a STATUS snapshot (eight SDO reads under the MCP mutex).

inline void send_status()

Send one STATUS snapshot now (what GET_STATUS and the stream do).

inline void set_status_stream(bool enabled, uint16_t period_ms = 0)

Enable / disable the periodic STATUS stream on this transport.

Parameters:

period_ms – Requested period (0 = default); clamped to the configured range.

inline bool stream_enabled() const

Whether the STATUS stream is currently enabled.

inline uint16_t stream_period_ms() const

The current STATUS stream period.

inline const std::string &get_name() const

Get the name of the component

Note

This is the tag of the logger

Returns:

A const reference to the name of the component

inline void set_log_tag(const std::string_view &tag)

Set the tag for the logger

Parameters:

tag – The tag to use for the logger

inline espp::Logger::Verbosity get_log_level() const

Get the log level for the logger

See also

Logger::Verbosity

See also

Logger::set_verbosity

Returns:

The verbosity level of the logger

inline void set_log_level(espp::Logger::Verbosity level)

Set the log level for the logger

See also

Logger::Verbosity

See also

Logger::set_verbosity

Parameters:

level – The verbosity level to use for the logger

inline void set_log_verbosity(espp::Logger::Verbosity level)

Set the log verbosity for the logger

See also

set_log_level

See also

Logger::Verbosity

See also

Logger::set_verbosity

Note

This is a convenience method that calls set_log_level

Parameters:

level – The verbosity level to use for the logger

inline espp::Logger::Verbosity get_log_verbosity() const

Get the log verbosity for the logger

See also

get_log_level

See also

Logger::Verbosity

See also

Logger::get_verbosity

Note

This is a convenience method that calls get_log_level

Returns:

The verbosity level of the logger

inline void set_log_rate_limit(std::chrono::duration<float> rate_limit)

Set the rate limit for the logger

See also

Logger::set_rate_limit

Note

Only calls to the logger that have _rate_limit suffix will be rate limited

Parameters:

rate_limit – The rate limit to use for the logger

struct Config

Configuration for the Mcp266Service.

Public Members

send_fn send = {nullptr}

Transmits an encoded frame (required). Called from the task that feeds handle() (request replies) AND from this service’s status-stream task, so it must be thread-safe: if the transport does not serialize writers (e.g. UsbDevice::write_vendor), guard it with a mutex.

uint8_t module = {mcp266_protocol::kModuleId}

Dispatcher module id to answer on (the console expects the default).

std::mutex *mcp_mutex = {nullptr}

Mutex serializing every use of the Mcp266 (one SDO channel). Share it between all users of the driver &#8212; other service instances and the application’s own calls. If null the service uses a private mutex.

uint16_t default_stream_period_ms = {200}

STATUS period when the host asks for 0.

uint16_t min_stream_period_ms = {50}

Fastest / slowest STATUS stream period the host may request (8 SDO reads per snapshot). Swapped if given in the wrong order.

Task::BaseConfig status_task_config   = {.name = "mcp266_status", .stack_size_bytes = 8192}

The status-streaming task (one per service instance).

espp::Logger::Verbosity log_level = {espp::Logger::Verbosity::WARN}

Logger verbosity.