#include "Myo.h" #include // ============================================================================= // Protocol Constants (ASPEP / MCP) // ============================================================================= namespace { constexpr uint8_t TYPE_POS = 3; constexpr uint8_t ELT_IDENTIFIER_POS = 6; constexpr uint8_t MOTOR_MASK = 0x07; constexpr uint16_t CMD_MASK = 0xFFF8; // Raw data packet type constexpr uint8_t TYPE_DATA_RAW = (5 << TYPE_POS); // ------------------------------------------------------------------------ // Motor-control register identifiers // ------------------------------------------------------------------------ constexpr uint16_t MC_REG_SPEED_RAMP = (6 << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW; constexpr uint16_t MC_REG_TORQUE_RAMP = (7 << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW; constexpr uint16_t MC_REG_POSITION_RAMP = (14 << ELT_IDENTIFIER_POS) | TYPE_DATA_RAW; // ------------------------------------------------------------------------ // MCP commands // ------------------------------------------------------------------------ constexpr uint16_t SET_DATA_ELEMENT = 0x8; constexpr uint16_t START_MOTOR = 0x18; constexpr uint16_t STOP_MOTOR = 0x20; constexpr uint16_t SET_CAN_BAUD = 0x100; constexpr uint16_t SET_CAN_NODE_ID = 0x108; // ------------------------------------------------------------------------ // ASPEP packet types // ------------------------------------------------------------------------ constexpr uint8_t DATA_PACKET = 0x9; // ------------------------------------------------------------------------ // Torque scaling: amps -> controller current units // ------------------------------------------------------------------------ constexpr float TORQUE_SCALE = 993.0f; // ======================================================================== // CRC-4 Lookup Tables // ======================================================================== const uint8_t CRC4_LOOKUP8[256] PROGMEM = { 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x07, 0x05, 0x03, 0x01, 0x0f, 0x0d, 0x0b, 0x09, 0x07, 0x05, 0x03, 0x01, 0x0f, 0x0d, 0x0b, 0x09, 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x00, 0x09, 0x0b, 0x0d, 0x0f, 0x01, 0x03, 0x05, 0x07, 0x09, 0x0b, 0x0d, 0x0f, 0x01, 0x03, 0x05, 0x07, 0x0e, 0x0c, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x00, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05, 0x0c, 0x0e, 0x08, 0x0a, 0x04, 0x06, 0x00, 0x02, 0x0c, 0x0e, 0x08, 0x0a, 0x04, 0x06, 0x00, 0x02, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05, 0x05, 0x07, 0x01, 0x03, 0x0d, 0x0f, 0x09, 0x0b, 0x02, 0x00, 0x06, 0x04, 0x0a, 0x08, 0x0e, 0x0c, 0x02, 0x00, 0x06, 0x04, 0x0a, 0x08, 0x0e, 0x0c, 0x05, 0x07, 0x01, 0x03, 0x0d, 0x0f, 0x09, 0x0b, 0x01, 0x03, 0x05, 0x07, 0x09, 0x0b, 0x0d, 0x0f, 0x06, 0x04, 0x02, 0x00, 0x0e, 0x0c, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x00, 0x0e, 0x0c, 0x0a, 0x08, 0x01, 0x03, 0x05, 0x07, 0x09, 0x0b, 0x0d, 0x0f, 0x0f, 0x0d, 0x0b, 0x09, 0x07, 0x05, 0x03, 0x01, 0x08, 0x0a, 0x0c, 0x0e, 0x00, 0x02, 0x04, 0x06, 0x08, 0x0a, 0x0c, 0x0e, 0x00, 0x02, 0x04, 0x06, 0x0f, 0x0d, 0x0b, 0x09, 0x07, 0x05, 0x03, 0x01, 0x0a, 0x08, 0x0e, 0x0c, 0x02, 0x00, 0x06, 0x04, 0x0d, 0x0f, 0x09, 0x0b, 0x05, 0x07, 0x01, 0x03, 0x0d, 0x0f, 0x09, 0x0b, 0x05, 0x07, 0x01, 0x03, 0x0a, 0x08, 0x0e, 0x0c, 0x02, 0x00, 0x06, 0x04, 0x04, 0x06, 0x00, 0x02, 0x0c, 0x0e, 0x08, 0x0a, 0x03, 0x01, 0x07, 0x05, 0x0b, 0x09, 0x0f, 0x0d, 0x03, 0x01, 0x07, 0x05, 0x0b, 0x09, 0x0f, 0x0d, 0x04, 0x06, 0x00, 0x02, 0x0c, 0x0e, 0x08, 0x0a}; const uint8_t CRC4_LOOKUP4[16] PROGMEM = { 0x00, 0x07, 0x0e, 0x09, 0x0b, 0x0c, 0x05, 0x02, 0x01, 0x06, 0x0f, 0x08, 0x0a, 0x0d, 0x04, 0x03}; // ======================================================================== // Packed MCP Payload Structures // ======================================================================== #pragma pack(push, 1) struct SpeedRampPayload { uint16_t size = 6; int32_t targetRpm; uint16_t durationMs; }; struct TorqueRampPayload { uint16_t size = 6; int32_t targetTorque; uint16_t durationMs; }; struct PositionRampPayload { uint16_t size = 8; float targetPosition; float durationSeconds; }; #pragma pack(pop) // ======================================================================== // ASPEP Header CRC // ======================================================================== uint32_t computeHeaderCRC(uint32_t header) { header &= 0x0FFFFFFF; uint8_t crc = 0; crc = pgm_read_byte( &CRC4_LOOKUP8[crc ^ (header & 0xFF)]); crc = pgm_read_byte( &CRC4_LOOKUP8[crc ^ ((header >> 8) & 0xFF)]); crc = pgm_read_byte( &CRC4_LOOKUP8[crc ^ ((header >> 16) & 0xFF)]); crc = pgm_read_byte( &CRC4_LOOKUP4[crc ^ ((header >> 24) & 0x0F)]); return header | ((uint32_t)crc << 28); } } // namespace // ============================================================================= // Construction // ============================================================================= Myo::Myo(uint32_t canId, uint32_t canBaud, uint32_t uartBaud, uint8_t motorId, uint32_t rxCanId) : _interface(Interface::UART), _uartActive(false), _canActive(false), _debug(true), _canID(canId), _rxCanID(rxCanId), _canBaud(canBaud), _uartBaud(uartBaud), _motorId(motorId), _rxLength(0), _telemetryAvailable(false), _ackAvailable(false), _errorAvailable(false), _telemetryCallback(nullptr), _ackCallback(nullptr), _errorCallback(nullptr) { _telemetry = {}; _ack = {}; _error = {}; memset(_rxBuffer, 0, sizeof(_rxBuffer)); } // ============================================================================= // Initialization // ============================================================================= bool Myo::begin(bool useCan, bool enableUartConfig) { _interface = useCan ? Interface::CAN : Interface::UART; // UART is always required in UART mode, and is required in CAN mode // whenever the caller wants access to the CAN configuration commands. _uartActive = (!useCan) || enableUartConfig; bool ok = true; if (_uartActive) { MYO_UART_PORT.begin(_uartBaud); delay(10); // Flush any stale bytes from a previous session. while (MYO_UART_PORT.available()) { MYO_UART_PORT.read(); } ok = connect(); } if (useCan) { _can.begin(); _can.setBaudRate(_canBaud); _canActive = true; if (_debug) { MYO_DEBUG_PORT.print("CAN up @ "); MYO_DEBUG_PORT.print(_canBaud); MYO_DEBUG_PORT.print(" bit/s, TX ID 0x"); MYO_DEBUG_PORT.print(_canID, HEX); MYO_DEBUG_PORT.print(", RX ID 0x"); MYO_DEBUG_PORT.println(_rxCanID, HEX); } } return ok; } bool Myo::connect() { if (!_uartActive) { return false; } if (_debug) { MYO_DEBUG_PORT.println("ASPEP: sending BEACON..."); } sendBeacon(); delay(50); if (_debug) { MYO_DEBUG_PORT.println("ASPEP: capability sync..."); } size_t capsLen = capabilitySync( _rxBuffer, sizeof(_rxBuffer)); if (capsLen == 0) { if (_debug) { MYO_DEBUG_PORT.println("ASPEP: capability sync failed."); } return false; } if (_debug) { MYO_DEBUG_PORT.println("ASPEP: sending PING..."); } ping(); return receivePacket( _rxBuffer, sizeof(_rxBuffer), 50) > 0; } // ============================================================================= // Raw ASPEP Packet I/O // ============================================================================= void Myo::sendPacket( const uint8_t *data, size_t length, bool includePayloadPause) { if (data == nullptr || length == 0 || !_uartActive) { return; } if (includePayloadPause && length > 4) { // Send ASPEP header first. MYO_UART_PORT.write(data, 4); MYO_UART_PORT.flush(); if (_debug) { MYO_DEBUG_PORT.print("TX Header: "); printHex(data, 4); } // Required delay for MCU DMA setup. delay(2); // Send MCP payload. MYO_UART_PORT.write(data + 4, length - 4); MYO_UART_PORT.flush(); if (_debug) { MYO_DEBUG_PORT.print("TX Payload: "); printHex(data + 4, length - 4); } } else { MYO_UART_PORT.write(data, length); MYO_UART_PORT.flush(); if (_debug) { MYO_DEBUG_PORT.print("TX: "); printHex(data, length); } } } size_t Myo::receivePacket( uint8_t *buffer, size_t maxLen, uint32_t delayMs) { if (buffer == nullptr || maxLen == 0 || !_uartActive) { return 0; } delay(delayMs); size_t bytesRead = 0; while (MYO_UART_PORT.available() && bytesRead < maxLen) { buffer[bytesRead++] = MYO_UART_PORT.read(); } if (buffer == _rxBuffer) { _rxLength = bytesRead; } if (_debug) { if (bytesRead > 0) { MYO_DEBUG_PORT.print("RX: "); printHex(buffer, bytesRead); } else { MYO_DEBUG_PORT.println("RX: (no response)"); } } return bytesRead; } // ============================================================================= // Connection Management // ============================================================================= void Myo::sendBeacon() { const uint8_t beacon[4] = { 0x85, 0xFF, 0xFF, 0xBF}; sendPacket(beacon, sizeof(beacon)); } bool Myo::ping() { if (!_uartActive) { return false; } const uint8_t pingFrame[4] = { 0x06, 0x00, 0x00, 0x60}; sendPacket(pingFrame, sizeof(pingFrame)); return true; } size_t Myo::capabilitySync( uint8_t *buffer, size_t maxLen) { delay(50); size_t capsLen = receivePacket( buffer, maxLen); if (capsLen == 0) { if (_debug) { MYO_DEBUG_PORT.println( "No capabilities response received"); } return 0; } if (_debug) { MYO_DEBUG_PORT.println( "Echoing capabilities back..."); } sendPacket( buffer, capsLen); delay(50); return receivePacket( buffer, maxLen); } // ============================================================================= // MCP Protocol // ============================================================================= void Myo::sendMcpCommand( const uint8_t *payload, size_t payloadLen) { if (payload == nullptr || payloadLen == 0 || !_uartActive) { return; } uint32_t header = (DATA_PACKET & 0x0F) | ((payloadLen & 0x3FFF) << 4); header = computeHeaderCRC(header); uint8_t frame[MYO_RX_BUFFER_SIZE]; if (payloadLen > sizeof(frame) - 4) { if (_debug) { MYO_DEBUG_PORT.println( "ERROR: MCP payload too large"); } return; } memcpy( frame, &header, sizeof(header)); memcpy( frame + 4, payload, payloadLen); sendPacket( frame, 4 + payloadLen, true); } uint16_t Myo::buildMcpHeader( uint16_t command, uint8_t motorId) { return ((motorId + 1) & MOTOR_MASK) | (command & CMD_MASK); } size_t Myo::buildSetDataElementPacket( uint16_t regId, const uint8_t *regData, size_t dataLen, uint8_t *outBuf, uint8_t motorId) { if (regData == nullptr || outBuf == nullptr) { return 0; } // This intentionally matches the known-working implementation. uint16_t mcpHeader = SET_DATA_ELEMENT; uint8_t motorBits = (motorId + 1) & MOTOR_MASK; uint16_t regIdWithMotor = regId | motorBits; memcpy( outBuf, &mcpHeader, sizeof(mcpHeader)); memcpy( outBuf + 2, ®IdWithMotor, sizeof(regIdWithMotor)); memcpy( outBuf + 4, regData, dataLen); return 4 + dataLen; } // ============================================================================= // UART Command Implementations // ============================================================================= bool Myo::uartStart() { uint16_t mcpHeader = buildMcpHeader( START_MOTOR, _motorId); sendMcpCommand( reinterpret_cast(&mcpHeader), sizeof(mcpHeader)); return _uartActive; } bool Myo::uartStop() { uint16_t mcpHeader = buildMcpHeader( STOP_MOTOR, _motorId); sendMcpCommand( reinterpret_cast(&mcpHeader), sizeof(mcpHeader)); return _uartActive; } bool Myo::uartSetSpeed(float rpm, uint16_t rampMs) { SpeedRampPayload speed; speed.targetRpm = static_cast(rpm); speed.durationMs = rampMs; uint8_t payload[16]; size_t len = buildSetDataElementPacket( MC_REG_SPEED_RAMP, reinterpret_cast(&speed), sizeof(speed), payload, _motorId); sendMcpCommand(payload, len); return _uartActive; } bool Myo::uartSetTorque(float amps, uint16_t rampMs) { TorqueRampPayload torque; // Same conversion as the known-working implementation. torque.targetTorque = static_cast(amps * TORQUE_SCALE); torque.durationMs = rampMs; uint8_t payload[16]; size_t len = buildSetDataElementPacket( MC_REG_TORQUE_RAMP, reinterpret_cast(&torque), sizeof(torque), payload, _motorId); sendMcpCommand(payload, len); return _uartActive; } bool Myo::uartSetPosition(float radians, float durationSec) { PositionRampPayload position; position.targetPosition = radians; position.durationSeconds = durationSec; uint8_t payload[16]; size_t len = buildSetDataElementPacket( MC_REG_POSITION_RAMP, reinterpret_cast(&position), sizeof(position), payload, _motorId); sendMcpCommand(payload, len); return _uartActive; } // ============================================================================= // CAN Transmission // ============================================================================= bool Myo::sendCanCommand( uint8_t command, const uint8_t *data, uint8_t dataLength) { if (!_canActive) { return false; } CAN_message_t message = {}; message.id = _canID; message.flags.extended = 0; // Command byte message.buf[0] = command; // Optional payload if (data != nullptr && dataLength > 0) { memcpy( &message.buf[1], data, dataLength); } message.len = 1 + dataLength; return _can.write(message) > 0; } // ============================================================================= // Motion Commands (transport-agnostic) // ============================================================================= bool Myo::start() { if (usingCan()) { return sendCanCommand( static_cast(Command::Start)); } return uartStart(); } bool Myo::stop() { if (usingCan()) { return sendCanCommand( static_cast(Command::Stop)); } return uartStop(); } bool Myo::setSpeed(float rpm, uint16_t rampMs) { // The public API is in output-shaft RPM; the controller expects rotor RPM. const float maxRpm = maxOutputRpm(); _speedClamped = false; if (rpm > maxRpm) { rpm = maxRpm; _speedClamped = true; } else if (rpm < -maxRpm) { rpm = -maxRpm; _speedClamped = true; } if (_speedClamped && _debug) { MYO_DEBUG_PORT.print("WARNING: speed clamped to "); MYO_DEBUG_PORT.print(rpm); MYO_DEBUG_PORT.println(" RPM at the output shaft"); } const float rotorRpm = rpm * MYO_GEAR_RATIO; if (usingCan()) { uint8_t data[6]; memcpy(&data[0], &rotorRpm, sizeof(float)); memcpy(&data[4], &rampMs, sizeof(uint16_t)); return sendCanCommand( static_cast(Command::Speed), data, sizeof(data)); } return uartSetSpeed(rotorRpm, rampMs); } bool Myo::setTorque(float amps, uint16_t rampMs) { if (usingCan()) { uint8_t data[6]; memcpy(&data[0], &s, sizeof(float)); memcpy(&data[4], &rampMs, sizeof(uint16_t)); return sendCanCommand( static_cast(Command::Torque), data, sizeof(data)); } return uartSetTorque(amps, rampMs); } bool Myo::setPosition(float radians, float durationSec) { if (usingCan()) { // Classic CAN carries 8 bytes, one of which is the command byte. // float radians + float seconds would need 9, so the duration is // sent as milliseconds in a uint16_t, matching the speed and // torque frames. Total frame: 1 + 4 + 2 = 7 bytes. float clampedSec = durationSec; if (clampedSec < 0.0f) { clampedSec = 0.0f; } else if (clampedSec > 65.535f) { clampedSec = 65.535f; } uint16_t durationMs = static_cast(clampedSec * 1000.0f); uint8_t data[6]; memcpy(&data[0], &radians, sizeof(float)); memcpy(&data[4], &durationMs, sizeof(uint16_t)); return sendCanCommand( static_cast(Command::Position), data, sizeof(data)); } return uartSetPosition(radians, durationSec); } bool Myo::requestTelemetry() { if (!usingCan()) { if (_debug) { MYO_DEBUG_PORT.println( "requestTelemetry() is CAN-only."); } return false; } return sendCanCommand( static_cast(Command::Telemetry)); } bool Myo::sendInvalidCommand() { if (!usingCan()) { return false; } return sendCanCommand( static_cast(Command::Invalid)); } // ============================================================================= // CAN Configuration // // These always travel over UART, then the local CAN side is updated to match. // ============================================================================= bool Myo::setCanBaud(uint32_t baudrate) { switch (baudrate) { case 500000UL: case 1000000UL: break; default: if (_debug) { MYO_DEBUG_PORT.print("ERROR: Unsupported CAN baudrate: "); MYO_DEBUG_PORT.println(baudrate); } return false; } if (!_uartActive) { if (_debug) { MYO_DEBUG_PORT.println( "ERROR: setCanBaud() requires the UART link."); } return false; } // MCP command 0x100 = CAN baudrate configuration. // For motor ID 0: buildMcpHeader(0x100, 0) -> 0x0101 (little-endian 01 01) // Followed by uint32_t baudrate, little-endian. uint16_t mcpHeader = buildMcpHeader(SET_CAN_BAUD, 0); uint8_t payload[6]; memcpy(payload, &mcpHeader, sizeof(mcpHeader)); memcpy(payload + 2, &baudrate, sizeof(baudrate)); if (_debug) { MYO_DEBUG_PORT.print("Setting CAN baudrate to "); MYO_DEBUG_PORT.println(baudrate); MYO_DEBUG_PORT.print("CAN Config Payload: "); printHex(payload, sizeof(payload)); } sendMcpCommand(payload, sizeof(payload)); // Consume the controller's UART acknowledgement before switching the // local bus over, so the two sides never disagree mid-transaction. receivePacket(_rxBuffer, sizeof(_rxBuffer), 50); _canBaud = baudrate; if (_canActive) { _can.setBaudRate(_canBaud); if (_debug) { MYO_DEBUG_PORT.print("Local CAN bus re-timed to "); MYO_DEBUG_PORT.println(_canBaud); } } return true; } bool Myo::setCanId(uint16_t canId) { // Standard CAN ID is 11 bits. if (canId > 0x7FF) { if (_debug) { MYO_DEBUG_PORT.println("ERROR: Invalid CAN ID"); } return false; } if (!_uartActive) { if (_debug) { MYO_DEBUG_PORT.println( "ERROR: setCanId() requires the UART link."); } return false; } // MCP command 0x108 = CAN node ID configuration. // For motor ID 0: buildMcpHeader(0x108, 0) -> 0x0109 (little-endian 09 01) // Followed by uint16_t CAN ID. uint16_t mcpHeader = buildMcpHeader(SET_CAN_NODE_ID, 0); uint8_t payload[4]; memcpy(payload, &mcpHeader, sizeof(mcpHeader)); memcpy(payload + 2, &canId, sizeof(canId)); if (_debug) { MYO_DEBUG_PORT.print("Setting CAN ID to "); MYO_DEBUG_PORT.println(canId); MYO_DEBUG_PORT.print("CAN Config Payload: "); printHex(payload, sizeof(payload)); } sendMcpCommand(payload, sizeof(payload)); receivePacket(_rxBuffer, sizeof(_rxBuffer), 50); // The firmware writes the node ID straight into FilterID1 with a 0x7FF // mask, so the value passed here IS the ID the controller listens on -- // there is no base-address offset. Retarget outgoing frames to match. setTxCanId(canId); // NOTE: the controller's CAN_ReplyID is not touched by this command, so // _rxCanID deliberately stays where it is. Change it with setRxCanId() // only if the firmware's reply ID is changed too. return true; } void Myo::setTxCanId(uint16_t canId) { _canID = canId; if (_debug) { MYO_DEBUG_PORT.print("Outgoing CAN ID now 0x"); MYO_DEBUG_PORT.println(_canID, HEX); } } void Myo::setRxCanId(uint16_t canId) { _rxCanID = canId; if (_debug) { MYO_DEBUG_PORT.print("Accepting responses on CAN ID 0x"); MYO_DEBUG_PORT.println(_rxCanID, HEX); } } // ============================================================================= // Reception // ============================================================================= size_t Myo::update() { if (usingCan()) { CAN_message_t message; size_t frames = 0; while (_can.read(message)) { processMessage(message); frames++; } return frames; } // UART: non-blocking drain into the internal buffer. if (!_uartActive) { return 0; } size_t bytesRead = 0; while (MYO_UART_PORT.available() && bytesRead < sizeof(_rxBuffer)) { _rxBuffer[bytesRead++] = MYO_UART_PORT.read(); } if (bytesRead > 0) { _rxLength = bytesRead; if (_debug) { MYO_DEBUG_PORT.print("RX: "); printHex(_rxBuffer, bytesRead); } } return bytesRead; } size_t Myo::receiveResponse(uint32_t timeoutMs) { if (usingCan()) { uint32_t startTime = millis(); size_t frames = 0; do { frames += update(); } while (frames == 0 && (millis() - startTime) < timeoutMs); if (frames == 0 && _debug) { MYO_DEBUG_PORT.println("RX: (no CAN response)"); } return frames; } return receivePacket( _rxBuffer, sizeof(_rxBuffer), timeoutMs); } // ============================================================================= // CAN Message Decoding // ============================================================================= void Myo::processMessage( const CAN_message_t &message) { // Ignore empty frames. if (message.len == 0) { return; } // Only accept frames on the controller's reply ID (CAN_ReplyID). if (message.id != _rxCanID) { return; } switch (message.buf[0]) { case static_cast(Response::StartAck): processAck(Command::Start, message); break; case static_cast(Response::StopAck): processAck(Command::Stop, message); break; case static_cast(Response::SpeedAck): processAck(Command::Speed, message); break; case static_cast(Response::TorqueAck): processAck(Command::Torque, message); break; case static_cast(Response::PositionAck): processAck(Command::Position, message); break; case static_cast(Response::Telemetry): processTelemetry(message); break; case static_cast(Response::Error): processError(message); break; default: // Unknown response. // Intentionally ignored by the high-level API. break; } } void Myo::processAck( Command command, const CAN_message_t &message) { if (message.len < 2) { return; } MotorAck ack; ack.command = command; ack.success = (message.buf[1] == 0x01); _ack = ack; _ackAvailable = true; if (_ackCallback != nullptr) { _ackCallback(ack); } } void Myo::processTelemetry( const CAN_message_t &message) { // Telemetry requires: // // Byte 0 = response header // Byte 1 = state // Byte 2 = RPM low byte // Byte 3 = RPM high byte if (message.len < 4) { return; } MotorTelemetry telemetry; telemetry.state = message.buf[1]; telemetry.rotorRpm = static_cast( message.buf[2] | (static_cast(message.buf[3]) << 8)); telemetry.rpm = static_cast(telemetry.rotorRpm) / MYO_GEAR_RATIO; _telemetry = telemetry; _telemetryAvailable = true; if (_telemetryCallback != nullptr) { _telemetryCallback(telemetry); } } void Myo::processError( const CAN_message_t &message) { if (message.len < 2) { return; } MotorError error; error.badCommand = message.buf[1]; _error = error; _errorAvailable = true; if (_errorCallback != nullptr) { _errorCallback(error); } } // ============================================================================= // Polling API // ============================================================================= bool Myo::getTelemetry( MotorTelemetry &telemetry) { if (!_telemetryAvailable) { return false; } telemetry = _telemetry; _telemetryAvailable = false; return true; } bool Myo::getAck( MotorAck &ack) { if (!_ackAvailable) { return false; } ack = _ack; _ackAvailable = false; return true; } bool Myo::getError( MotorError &error) { if (!_errorAvailable) { return false; } error = _error; _errorAvailable = false; return true; } // ============================================================================= // Callback Registration // ============================================================================= void Myo::onTelemetry( TelemetryCallback callback) { _telemetryCallback = callback; } void Myo::onAck( AckCallback callback) { _ackCallback = callback; } void Myo::onError( ErrorCallback callback) { _errorCallback = callback; } // ============================================================================= // Raw CAN Access // ============================================================================= bool Myo::readRaw( CAN_message_t &message) { if (!_canActive) { return false; } return _can.read(message); } // ============================================================================= // Utility // ============================================================================= void Myo::printHex( const uint8_t *buffer, size_t length) { for (size_t i = 0; i < length; i++) { if (buffer[i] < 0x10) { MYO_DEBUG_PORT.print('0'); } MYO_DEBUG_PORT.print(buffer[i], HEX); if (i < length - 1) { MYO_DEBUG_PORT.print(' '); } } MYO_DEBUG_PORT.println(); }