Motoron.h

Go to the documentation of this file.

// Copyright (C) Pololu Corporation.  See LICENSE.txt for details.


 
#pragma once
 
#include <Arduino.h>
#include <Wire.h>
#include "motoron_protocol.h"

 
extern const PROGMEM uint8_t motoronCrcTable[256];


enum class MotoronCurrentSenseType {
  Motoron18v18 = 0b0001,
  Motoron24v14 = 0b0101,
  Motoron18v20 = 0b1010,
  Motoron24v16 = 0b1101,
  Motoron453   = 0b0110,
};

enum class MotoronVinSenseType {
  Motoron256 = 0b0000,
  MotoronHp = 0b0010,
  Motoron550 = 0b0011,
  Motoron453 = 0b0110,
};
 
struct MotoronCurrentSenseReading
{
  uint16_t raw;
  int16_t speed;
  uint16_t processed;
};

class MotoronBase
{
public:
  MotoronBase()
  {
    lastError = 0;
    protocolOptions = defaultProtocolOptions;
  }

  uint8_t getLastError()
  {
    return lastError;
  }

  void getFirmwareVersion(uint16_t * productId, uint16_t * firmwareVersion)
  {
    uint8_t cmd = MOTORON_CMD_GET_FIRMWARE_VERSION;
    uint8_t response[4];
    sendCommandAndReadResponse(1, &cmd, sizeof(response), response);
    if (productId != nullptr) { *productId = response[0] | (response[1] << 8); }
    if (firmwareVersion != nullptr) { *firmwareVersion = response[2] | (response[3] << 8); }
  }

  void setProtocolOptions(uint8_t options)
  {
    this->protocolOptions = options;
    uint8_t cmd[] = {
      MOTORON_CMD_SET_PROTOCOL_OPTIONS,
      (uint8_t)(options & 0x7F),
      (uint8_t)(~options & 0x7F),
    };
    sendCommandCore(sizeof(cmd), cmd, true);
    if (getLastError()) { return; }
  }

  void setProtocolOptionsLocally(uint8_t options)
  {
    this->protocolOptions = options;
  }

  uint8_t getProtocolOptionsLocally()
  {
    return this->protocolOptions;
  }

  void enableCrc()
  {
    setProtocolOptions(protocolOptions
      | (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS)
      | (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES));
  }

  void disableCrc()
  {
    setProtocolOptions(protocolOptions
      & ~(1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS)
      & ~(1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES));
  }

  void enableCrcForCommands()
  {
    setProtocolOptions(protocolOptions
      | (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS));
  }

  void disableCrcForCommands()
  {
    setProtocolOptions(protocolOptions
      & ~(1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS));
  }

  void enableCrcForResponses()
  {
    setProtocolOptions(protocolOptions
      | (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES));
  }

  void disableCrcForResponses()
  {
    setProtocolOptions(protocolOptions
      & ~(1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES));
  }

  void enableI2cGeneralCall()
  {
    setProtocolOptions(protocolOptions
      | (1 << MOTORON_PROTOCOL_OPTION_I2C_GENERAL_CALL));
  }

  void disableI2cGeneralCall()
  {
    setProtocolOptions(protocolOptions
      & ~(1 << MOTORON_PROTOCOL_OPTION_I2C_GENERAL_CALL));
  }

  void readEeprom(uint8_t offset, uint8_t length, uint8_t * buffer)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_READ_EEPROM,
      (uint8_t)(offset & 0x7F),
      (uint8_t)(length & 0x7F),
    };
    sendCommandAndReadResponse(sizeof(cmd), cmd, length, buffer);
  }

  uint8_t readEepromDeviceNumber()
  {
    uint8_t number;
    readEeprom(MOTORON_SETTING_DEVICE_NUMBER, 1, &number);
    return number;
  }

  void writeEeprom(uint8_t offset, uint8_t value)
  {
    uint8_t cmd[7];
    cmd[0] = MOTORON_CMD_WRITE_EEPROM;
    cmd[1] = offset & 0x7F;
    cmd[2] = value & 0x7F;
    cmd[3] = value >> 7 & 1;
    cmd[4] = cmd[1] ^ 0x7F;
    cmd[5] = cmd[2] ^ 0x7F;
    cmd[6] = cmd[3] ^ 0x7F;
    sendCommand(sizeof(cmd), cmd);
    flushTransmission();
    delay(6);
  }

  void writeEeprom16(uint8_t offset, uint16_t value)
  {
    writeEeprom(offset, value & 0xFF);
    writeEeprom(offset + 1, value >> 8 & 0xFF);
  }

  void writeEepromDeviceNumber(uint16_t number)
  {
    writeEeprom(MOTORON_SETTING_DEVICE_NUMBER, number & 0x7F);
    writeEeprom(MOTORON_SETTING_DEVICE_NUMBER + 1, number >> 7 & 0x7F);
  }

  void writeEepromAlternativeDeviceNumber(uint16_t number)
  {
    writeEeprom(MOTORON_SETTING_ALTERNATIVE_DEVICE_NUMBER, (number & 0x7F) | 0x80);
    writeEeprom(MOTORON_SETTING_ALTERNATIVE_DEVICE_NUMBER + 1, number >> 7 & 0x7F);
  }

  void writeEepromDisableAlternativeDeviceNumber()
  {
    writeEeprom(MOTORON_SETTING_ALTERNATIVE_DEVICE_NUMBER, 0);
    writeEeprom(MOTORON_SETTING_ALTERNATIVE_DEVICE_NUMBER + 1, 0);
  }

  void writeEepromCommunicationOptions(uint8_t options)
  {
    writeEeprom(MOTORON_SETTING_COMMUNICATION_OPTIONS, options);
  }

  void writeEepromBaudRate(uint32_t baud)
  {
    if (baud < MOTORON_MIN_BAUD_RATE) { baud = MOTORON_MIN_BAUD_RATE; }
    if (baud > MOTORON_MAX_BAUD_RATE) { baud = MOTORON_MAX_BAUD_RATE; }
    writeEeprom16(MOTORON_SETTING_BAUD_DIVIDER, (16000000 + (baud >> 1)) / baud);
  }

  void writeEepromResponseDelay(uint8_t delay)
  {
    writeEeprom(MOTORON_SETTING_RESPONSE_DELAY, delay);
  }

  void reinitialize()
  {
    // Always send the reset command with a CRC byte to make it more reliable.
    uint8_t cmd = MOTORON_CMD_REINITIALIZE;
    sendCommandCore(1, &cmd, true);
    protocolOptions = defaultProtocolOptions;
  }

  void reset()
  {
    uint8_t cmd = MOTORON_CMD_RESET;
    sendCommandCore(1, &cmd, true);
    flushTransmission();
    protocolOptions = defaultProtocolOptions;
  }

  void getVariables(uint8_t motor, uint8_t offset, uint8_t length, uint8_t * buffer)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_GET_VARIABLES,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(offset & 0x7F),
      (uint8_t)(length & 0x7F),
    };
    sendCommandAndReadResponse(sizeof(cmd), cmd, length, buffer);
  }

  uint8_t getVar8(uint8_t motor, uint8_t offset)
  {
    uint8_t result;
    getVariables(motor, offset, 1, &result);
    return result;
  }

  uint16_t getVar16(uint8_t motor, uint8_t offset)
  {
    uint8_t buffer[2];
    getVariables(motor, offset, 2, buffer);
    return buffer[0] | ((uint16_t)buffer[1] << 8);
  }

  uint16_t getStatusFlags()
  {
    return getVar16(0, MOTORON_VAR_STATUS_FLAGS);
  }

  bool getProtocolErrorFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_PROTOCOL_ERROR);
  }

  bool getCrcErrorFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_CRC_ERROR);
  }

  bool getCommandTimeoutLatchedFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_COMMAND_TIMEOUT_LATCHED);
  }

  bool getMotorFaultLatchedFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_MOTOR_FAULT_LATCHED);
  }

  bool getNoPowerLatchedFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_NO_POWER_LATCHED);
  }

  bool getUARTErrorFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_UART_ERROR);
  }

  bool getResetFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_RESET);
  }

  bool getMotorFaultingFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_MOTOR_FAULTING);
  }

  bool getNoPowerFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_NO_POWER);
  }

  bool getErrorActiveFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_ERROR_ACTIVE);
  }

  bool getMotorOutputEnabledFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_MOTOR_OUTPUT_ENABLED);
  }

  bool getMotorDrivingFlag()
  {
    return getStatusFlags() & (1 << MOTORON_STATUS_FLAG_MOTOR_DRIVING);
  }

  uint8_t getUARTFaults()
  {
    return getVar8(0, MOTORON_VAR_UART_FAULTS);
  }

  uint16_t getVinVoltage()
  {
    return getVar16(0, MOTORON_VAR_VIN_VOLTAGE);
  }

  uint32_t getVinVoltageMv(uint16_t referenceMv,
    MotoronVinSenseType type = MotoronVinSenseType::Motoron256)
  {
    uint16_t scale = (uint8_t)type & 1 ? 459 : 1047;
    return (uint32_t)getVinVoltage() * referenceMv / 1024 * scale / 47;
  }

  uint16_t getCommandTimeoutMilliseconds()
  {
    return getVar16(0, MOTORON_VAR_COMMAND_TIMEOUT) * 4;
  }

  uint8_t getErrorResponse()
  {
    return getVar8(0, MOTORON_VAR_ERROR_RESPONSE);
  }

  uint16_t getErrorMask()
  {
    return getVar16(0, MOTORON_VAR_ERROR_MASK);
  }

  uint8_t getJumperState()
  {
    return getVar8(0, MOTORON_VAR_JUMPER_STATE);
  }

  int16_t getTargetSpeed(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_TARGET_SPEED);
  }

  uint16_t getTargetBrakeAmount(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_TARGET_BRAKE_AMOUNT);
  }

  int16_t getCurrentSpeed(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_CURRENT_SPEED);
  }

  int16_t getBufferedSpeed(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_BUFFERED_SPEED);
  }

  uint8_t getPwmMode(uint8_t motor)
  {
    return getVar8(motor, MOTORON_MVAR_PWM_MODE);
  }

  uint16_t getMaxAccelerationForward(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_MAX_ACCEL_FORWARD);
  }

  uint16_t getMaxAccelerationReverse(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_MAX_ACCEL_REVERSE);
  }

  uint16_t getMaxDecelerationForward(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_MAX_DECEL_FORWARD);
  }

  uint16_t getMaxDecelerationReverse(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_MAX_DECEL_REVERSE);
  }

 
  // This function is used by Pololu for testing.
  uint16_t getMaxDecelerationTemporary(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_MAX_DECEL_TMP);
  }


  uint16_t getStartingSpeedForward(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_STARTING_SPEED_FORWARD);
  }

  uint16_t getStartingSpeedReverse(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_STARTING_SPEED_REVERSE);
  }

  uint8_t getDirectionChangeDelayForward(uint8_t motor)
  {
    return getVar8(motor, MOTORON_MVAR_DIRECTION_CHANGE_DELAY_FORWARD);
  }

  uint8_t getDirectionChangeDelayReverse(uint8_t motor)
  {
    return getVar8(motor, MOTORON_MVAR_DIRECTION_CHANGE_DELAY_REVERSE);
  }

  uint16_t getCurrentLimit(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_CURRENT_LIMIT);
  }

  MotoronCurrentSenseReading getCurrentSenseReading(uint8_t motor)
  {
    uint8_t buffer[6];
    getVariables(motor, MOTORON_MVAR_CURRENT_SENSE_RAW, sizeof(buffer), buffer);
    MotoronCurrentSenseReading r = {};
    r.raw = buffer[0] | ((uint16_t)buffer[1] << 8);
    r.speed = buffer[2] | ((uint16_t)buffer[3] << 8);
    r.processed = buffer[4] | ((uint16_t)buffer[5] << 8);
    return r;
  }

  MotoronCurrentSenseReading getCurrentSenseRawAndSpeed(uint8_t motor)
  {
    uint8_t buffer[4];
    getVariables(motor, MOTORON_MVAR_CURRENT_SENSE_RAW, sizeof(buffer), buffer);
    MotoronCurrentSenseReading r = {};
    r.raw = buffer[0] | ((uint16_t)buffer[1] << 8);
    r.speed = buffer[2] | ((uint16_t)buffer[3] << 8);
    return r;
  }

  MotoronCurrentSenseReading getCurrentSenseProcessedAndSpeed(uint8_t motor)
  {
    uint8_t buffer[4];
    getVariables(motor, MOTORON_MVAR_CURRENT_SENSE_SPEED, sizeof(buffer), buffer);
    MotoronCurrentSenseReading r = {};
    r.speed = buffer[0] | ((uint16_t)buffer[1] << 8);
    r.processed = buffer[2] | ((uint16_t)buffer[3] << 8);
    return r;
  }

  uint16_t getCurrentSenseRaw(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_CURRENT_SENSE_RAW);
  }

  uint16_t getCurrentSenseProcessed(uint8_t motor)
  {
    return getVar16(motor, MOTORON_MVAR_CURRENT_SENSE_PROCESSED);
  }

  uint8_t getCurrentSenseOffset(uint8_t motor)
  {
    return getVar8(motor, MOTORON_MVAR_CURRENT_SENSE_OFFSET);
  }

  uint16_t getCurrentSenseMinimumDivisor(uint8_t motor)
  {
    return getVar8(motor, MOTORON_MVAR_CURRENT_SENSE_MINIMUM_DIVISOR) << 2;
  }

  void setVariable(uint8_t motor, uint8_t offset, uint16_t value)
  {
    if (value > 0x3FFF) { value = 0x3FFF; }
    uint8_t cmd[] = {
      MOTORON_CMD_SET_VARIABLE,
      (uint8_t)(motor & 0x1F),
      (uint8_t)(offset & 0x7F),
      (uint8_t)(value & 0x7F),
      (uint8_t)((value >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setCommandTimeoutMilliseconds(uint16_t ms)
  {
    // Divide by 4, but round up, and make sure we don't have
    // an overflow if 0xFFFF is passed.
    uint16_t timeout = (ms / 4) + ((ms & 3) ? 1 : 0);
    setVariable(0, MOTORON_VAR_COMMAND_TIMEOUT, timeout);
  }

  void setErrorResponse(uint8_t response)
  {
    setVariable(0, MOTORON_VAR_ERROR_RESPONSE, response);
  }

  void setErrorMask(uint16_t mask)
  {
    setVariable(0, MOTORON_VAR_ERROR_MASK, mask);
  }

  void disableCommandTimeout()
  {
    setErrorMask(defaultErrorMask & ~(1 << MOTORON_STATUS_FLAG_COMMAND_TIMEOUT));
  }

  void clearUARTFaults(uint8_t flags)
  {
    setVariable(0, MOTORON_VAR_UART_FAULTS, ~(uint16_t)flags & 0x3FFF);
  }

  void setPwmMode(uint8_t motor, uint8_t mode)
  {
    setVariable(motor, MOTORON_MVAR_PWM_MODE, mode);
  }

  void setMaxAccelerationForward(uint8_t motor, uint16_t accel)
  {
    setVariable(motor, MOTORON_MVAR_MAX_ACCEL_FORWARD, accel);
  }

  void setMaxAccelerationReverse(uint8_t motor, uint16_t accel)
  {
    setVariable(motor, MOTORON_MVAR_MAX_ACCEL_REVERSE, accel);
  }

  void setMaxAcceleration(uint8_t motor, uint16_t accel)
  {
    setMaxAccelerationForward(motor, accel);
    if (getLastError()) { return; }
    setMaxAccelerationReverse(motor, accel);
  }

  void setMaxDecelerationForward(uint8_t motor, uint16_t decel)
  {
    setVariable(motor, MOTORON_MVAR_MAX_DECEL_FORWARD, decel);
  }

  void setMaxDecelerationReverse(uint8_t motor, uint16_t decel)
  {
    setVariable(motor, MOTORON_MVAR_MAX_DECEL_REVERSE, decel);
  }

  void setMaxDeceleration(uint8_t motor, uint16_t decel)
  {
    setMaxDecelerationForward(motor, decel);
    if (getLastError()) { return; }
    setMaxDecelerationReverse(motor, decel);
  }

  void setStartingSpeedForward(uint8_t motor, uint16_t speed)
  {
    setVariable(motor, MOTORON_MVAR_STARTING_SPEED_FORWARD, speed);
  }

  void setStartingSpeedReverse(uint8_t motor, uint16_t speed)
  {
    setVariable(motor, MOTORON_MVAR_STARTING_SPEED_REVERSE, speed);
  }

  void setStartingSpeed(uint8_t motor, uint16_t speed)
  {
    setStartingSpeedForward(motor, speed);
    if (getLastError()) { return; }
    setStartingSpeedReverse(motor, speed);
  }

  void setDirectionChangeDelayForward(uint8_t motor, uint8_t duration)
  {
    setVariable(motor, MOTORON_MVAR_DIRECTION_CHANGE_DELAY_FORWARD, duration);
  }

  void setDirectionChangeDelayReverse(uint8_t motor, uint8_t duration)
  {
    setVariable(motor, MOTORON_MVAR_DIRECTION_CHANGE_DELAY_REVERSE, duration);
  }

  void setDirectionChangeDelay(uint8_t motor, uint8_t duration)
  {
    setDirectionChangeDelayForward(motor, duration);
    if (getLastError()) { return; }
    setDirectionChangeDelayReverse(motor, duration);
  }

  void setCurrentLimit(uint8_t motor, uint16_t limit)
  {
    setVariable(motor, MOTORON_MVAR_CURRENT_LIMIT, limit);
  }

  void setCurrentSenseOffset(uint8_t motor, uint8_t offset)
  {
    setVariable(motor, MOTORON_MVAR_CURRENT_SENSE_OFFSET, offset);
  }

  void setCurrentSenseMinimumDivisor(uint8_t motor, uint16_t speed)
  {
    setVariable(motor, MOTORON_MVAR_CURRENT_SENSE_MINIMUM_DIVISOR, speed >> 2);
  }

  void coastNow()
  {
    uint8_t cmd = MOTORON_CMD_COAST_NOW;
    sendCommand(1, &cmd);
  }

  void clearMotorFault(uint8_t flags = 0)
  {
    uint8_t cmd[] = { MOTORON_CMD_CLEAR_MOTOR_FAULT, (uint8_t)(flags & 0x7F) };
    sendCommand(sizeof(cmd), cmd);
  }

  void clearMotorFaultUnconditional()
  {
    clearMotorFault(1 << MOTORON_CLEAR_MOTOR_FAULT_UNCONDITIONAL);
  }

  void clearLatchedStatusFlags(uint16_t flags)
  {
    uint8_t cmd[] = { MOTORON_CMD_CLEAR_LATCHED_STATUS_FLAGS,
      (uint8_t)(flags & 0x7F),
      (uint8_t)(flags >> 7 & 0x7F)
    };
    sendCommand(sizeof(cmd), cmd);
  }


  void clearResetFlag()
  {
    clearLatchedStatusFlags(1 << MOTORON_STATUS_FLAG_RESET);
  }

  void setLatchedStatusFlags(uint16_t flags)
  {
    uint8_t cmd[] = { MOTORON_CMD_SET_LATCHED_STATUS_FLAGS,
      (uint8_t)(flags & 0x7F),
      (uint8_t)(flags >> 7 & 0x7F)
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setSpeed(uint8_t motor, int16_t speed)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_SPEED,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(speed & 0x7F),
      (uint8_t)((speed >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setSpeedNow(uint8_t motor, int16_t speed)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_SPEED_NOW,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(speed & 0x7F),
      (uint8_t)((speed >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setBufferedSpeed(uint8_t motor, int16_t speed)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_BUFFERED_SPEED,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(speed & 0x7F),
      (uint8_t)((speed >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }
 

  void setAllSpeeds(int16_t speed1, int16_t speed2, int16_t speed3)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
      (uint8_t)(speed3 & 0x7F),
      (uint8_t)((speed3 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeeds(int16_t speed1, int16_t speed2)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeeds(int16_t speed1)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeedsNow(int16_t speed1, int16_t speed2, int16_t speed3)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS_NOW,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
      (uint8_t)(speed3 & 0x7F),
      (uint8_t)((speed3 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeedsNow(int16_t speed1, int16_t speed2)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS_NOW,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeedsNow(int16_t speed1)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_SPEEDS_NOW,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllBufferedSpeeds(int16_t speed1, int16_t speed2, int16_t speed3)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_BUFFERED_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
      (uint8_t)(speed3 & 0x7F),
      (uint8_t)((speed3 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllBufferedSpeeds(int16_t speed1, int16_t speed2)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_BUFFERED_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
      (uint8_t)(speed2 & 0x7F),
      (uint8_t)((speed2 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllBufferedSpeeds(int16_t speed1)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_ALL_BUFFERED_SPEEDS,
      (uint8_t)(speed1 & 0x7F),
      (uint8_t)((speed1 >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setAllSpeedsUsingBuffers()
  {
    uint8_t cmd = MOTORON_CMD_SET_ALL_SPEEDS_USING_BUFFERS;
    sendCommand(1, &cmd);
  }

  void setAllSpeedsNowUsingBuffers()
  {
    uint8_t cmd = MOTORON_CMD_SET_ALL_SPEEDS_NOW_USING_BUFFERS;
    sendCommand(1, &cmd);
  }

  void setBraking(uint8_t motor, uint16_t amount)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_BRAKING,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(amount & 0x7F),
      (uint8_t)((amount >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void setBrakingNow(uint8_t motor, uint16_t amount)
  {
    uint8_t cmd[] = {
      MOTORON_CMD_SET_BRAKING_NOW,
      (uint8_t)(motor & 0x7F),
      (uint8_t)(amount & 0x7F),
      (uint8_t)((amount >> 7) & 0x7F),
    };
    sendCommand(sizeof(cmd), cmd);
  }

  void resetCommandTimeout()
  {
    uint8_t cmd = MOTORON_CMD_RESET_COMMAND_TIMEOUT;
    sendCommand(1, &cmd);
  }

  static uint8_t calculateCrc(uint8_t length, const uint8_t * buffer,
    uint8_t init = 0)
  {
    uint8_t crc = init;
    for (uint8_t i = 0; i < length; i++)
    {
      crc = pgm_read_byte(&motoronCrcTable[crc ^ buffer[i]]);
 
      // The code below shows an alternative method to calculate the CRC
      // without using a lookup table.  It will generally be slower but save
      // program space.
      // crc ^= buffer[i];
      // for (uint8_t j = 0; j < 8; j++)
      // {
      //   if (crc & 1) { crc ^= 0x91; }
      //   crc >>= 1;
      // }
    }
    return crc;
  }

  static uint16_t calculateCurrentLimit(uint32_t milliamps,
    MotoronCurrentSenseType type, uint16_t referenceMv, uint16_t offset)
  {
    if (milliamps > 1000000) { milliamps = 1000000; }
    uint16_t limit = (uint32_t)(offset * 125 + 64) / 128 +
      milliamps * 20 / (referenceMv * ((uint8_t)type & 3));
    if (limit > 1000) { limit = 1000; }
    return limit;
  }

  static constexpr uint16_t currentSenseUnitsMilliamps(
    MotoronCurrentSenseType type, uint16_t referenceMv)
  {
    return (type == MotoronCurrentSenseType::Motoron453) ? 0
      : ((uint32_t)referenceMv * ((uint8_t)type & 3) * 25 / 512);
  }

  static constexpr uint32_t currentSenseUnitsMicroamps(
    MotoronCurrentSenseType type, uint16_t referenceMv)
  {
    return type == MotoronCurrentSenseType::Motoron453
      ? (uint32_t)referenceMv * 625 / 1056
      : ((uint32_t)referenceMv * ((uint8_t)type & 3) * 3125 / 64);
  }
 
protected:
  uint8_t lastError;

  uint8_t protocolOptions;

  void sendCommand(uint8_t length, const uint8_t * cmd)
  {
    bool sendCrc = protocolOptions & (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS);
    sendCommandCore(length, cmd, sendCrc);
  }

  void sendCommandAndReadResponse(uint8_t cmdLength, const uint8_t * cmd,
    uint8_t responseLength, uint8_t * response)
  {
    sendCommand(cmdLength, cmd);
    if (getLastError())
    {
      memset(response, 0, responseLength);
      return;
    }
    readResponse(responseLength, response);
  }
 
private:
 
  virtual void sendCommandCore(uint8_t length, const uint8_t * cmd, bool sendCrc) = 0;
  virtual void flushTransmission() = 0;
  virtual void readResponse(uint8_t length, uint8_t * response) = 0;
 
  static const uint8_t defaultProtocolOptions =
    (1 << MOTORON_PROTOCOL_OPTION_I2C_GENERAL_CALL) |
    (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS) |
    (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES);
 
  static const uint16_t defaultErrorMask =
    (1 << MOTORON_STATUS_FLAG_COMMAND_TIMEOUT) |
    (1 << MOTORON_STATUS_FLAG_RESET);
};

class MotoronI2C : public MotoronBase
{
public:
  MotoronI2C(uint8_t address = 16) : bus(&Wire), address(address) {}

  void setBus(TwoWire * bus)
  {
    this->bus = bus;
  }

  TwoWire * getBus()
  {
    return this->bus;
  }

  void setAddress(uint8_t address)
  {
    this->address = address;
  }

  uint8_t getAddress()
  {
    return address;
  }
 
private:
  TwoWire * bus;
  uint8_t address;
 
  void sendCommandCore(uint8_t length, const uint8_t * cmd, bool sendCrc) override
  {
    bus->beginTransmission(address);
    for (uint8_t i = 0; i < length; i++)
    {
      bus->write(cmd[i]);
    }
    if (sendCrc)
    {
      bus->write(calculateCrc(length, cmd));
    }
    lastError = bus->endTransmission();
  }
 
  void flushTransmission() { }
 
  void readResponse(uint8_t length, uint8_t * response) override
  {
    bool crcEnabled = protocolOptions & (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES);
    uint8_t byteCount = bus->requestFrom(address, (uint8_t)(length + crcEnabled));
    if (byteCount != length + crcEnabled)
    {
      memset(response, 0, length);
      lastError = 50;
      return;
    }
    lastError = 0;
    uint8_t * ptr = response;
    for (uint8_t i = 0; i < length; i++)
    {
      *ptr = bus->read();
      ptr++;
    }
    if (crcEnabled && bus->read() != calculateCrc(length, response))
    {
      lastError = 51;
      return;
    }
  }
};

class MotoronSerial : public MotoronBase
{
public:
  MotoronSerial(uint16_t deviceNumber = 0xFFFF) :
    port(nullptr), deviceNumber(deviceNumber), communicationOptions(0)
  {
  }

  MotoronSerial(Stream & port, uint16_t deviceNumber = 0xFFFF) :
    port(&port), deviceNumber(deviceNumber), communicationOptions(0)
  {
  }

  void setPort(Stream * port)
  {
    this->port = port;
  }

  Stream * getPort()
  {
    return port;
  }

  void setDeviceNumber(uint16_t deviceNumber)
  {
    this->deviceNumber = deviceNumber;
  }

  uint16_t getDeviceNumber()
  {
    return deviceNumber;
  }

  void setCommunicationOptions(uint8_t options)
  {
    communicationOptions = options;
  }

  uint8_t getCommunicationOptionsLocally()
  {
    return communicationOptions;
  }

  void expect7BitResponses()
  {
    communicationOptions |= (1 << MOTORON_COMMUNICATION_OPTION_7BIT_RESPONSES);
  }

  void expect8BitResponses()
  {
    communicationOptions &= ~(1 << MOTORON_COMMUNICATION_OPTION_7BIT_RESPONSES);
  }

  void use14BitDeviceNumber()
  {
    communicationOptions |= (1 << MOTORON_COMMUNICATION_OPTION_14BIT_DEVICE_NUMBER);
  }

  void use7BitDeviceNumber()
  {
    communicationOptions &= ~(1 << MOTORON_COMMUNICATION_OPTION_14BIT_DEVICE_NUMBER);
  }

  void multiDeviceErrorCheckStart(uint16_t startingDeviceNumber, uint16_t deviceCount)
  {
    if (communicationOptions & (1 << MOTORON_COMMUNICATION_OPTION_14BIT_DEVICE_NUMBER))
    {
      if (deviceCount > 0x3FFF) { lastError = 55; return; }
      uint8_t cmd[] = {
        MOTORON_CMD_MULTI_DEVICE_ERROR_CHECK,
        (uint8_t)(startingDeviceNumber & 0x7F),
        (uint8_t)(startingDeviceNumber >> 7 & 0x7F),
        (uint8_t)(deviceCount & 0x7F),
        (uint8_t)(deviceCount >> 7 & 0x7F),
      };
      sendCommand(sizeof(cmd), cmd);
    }
    else
    {
      if (deviceCount > 0x7F) { lastError = 55; return; }
      uint8_t cmd[] = {
        MOTORON_CMD_MULTI_DEVICE_ERROR_CHECK,
        (uint8_t)(startingDeviceNumber & 0x7F),
        (uint8_t)deviceCount,
      };
      sendCommand(sizeof(cmd), cmd);
    }
 
    port->flush();
  }

  uint16_t multiDeviceErrorCheck(uint16_t startingDeviceNumber, uint16_t deviceCount)
  {
    multiDeviceErrorCheckStart(startingDeviceNumber, deviceCount);
    uint16_t i;
    for (i = 0; i < deviceCount; i++)
    {
      uint8_t response;
      size_t byteCount = port->readBytes(&response, 1);
      if (byteCount < 1 || response != MOTORON_ERROR_CHECK_CONTINUE)
      {
        break;
      }
    }
    return i;
  }

  void multiDeviceWrite(uint16_t startingDeviceNumber, uint16_t deviceCount,
    uint8_t bytesPerDevice, uint8_t commandByte, const uint8_t * data)
  {
    if (port == nullptr) { lastError = 52; return; }
    if (bytesPerDevice > 15) { lastError = 56; return; }
 
    bool sendCrc = protocolOptions & (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_COMMANDS);
 
    uint8_t header[10] = { 0 };
    if (communicationOptions & (1 << MOTORON_COMMUNICATION_OPTION_14BIT_DEVICE_NUMBER))
    {
      if (deviceCount > 0x3FFF) { lastError = 55; return; }
 
      header[4] = startingDeviceNumber & 0x7F;
      header[5] = startingDeviceNumber >> 7 & 0x7F;
      header[6] = deviceCount & 0x7F;
      header[7] = deviceCount >> 7 & 0x7F;
      header[8] = bytesPerDevice;
      header[9] = commandByte & 0x7F;
 
      if (deviceNumber == 0xFFFF)
      {
        header[3] = MOTORON_CMD_MULTI_DEVICE_WRITE;
        port->write(header + 3, 7);
      }
      else
      {
        header[0] = 0xAA;
        header[1] = deviceNumber & 0x7F;
        header[2] = deviceNumber >> 7 & 0x7F;
        header[3] = MOTORON_CMD_MULTI_DEVICE_WRITE & 0x7F;
        port->write(header, 10);
      }
    }
    else
    {
      if (deviceCount > 0x7F) { lastError = 55; return; }
 
      header[6] = startingDeviceNumber & 0x7F;
      header[7] = deviceCount;
      header[8] = bytesPerDevice;
      header[9] = commandByte & 0x7F;
 
      if (deviceNumber == 0xFFFF)
      {
        header[5] = MOTORON_CMD_MULTI_DEVICE_WRITE;
        port->write(header + 5, 5);
      }
      else
      {
        header[3] = 0xAA;
        header[4] = deviceNumber & 0x7F;
        header[5] = MOTORON_CMD_MULTI_DEVICE_WRITE & 0x7F;
        port->write(header + 3, 7);
      }
    }
 
    uint8_t crc = 0;
    if (sendCrc) { crc = calculateCrc(sizeof(header), header); }
 
    if (bytesPerDevice)
    {
      while (deviceCount)
      {
        port->write(data, bytesPerDevice);
        if (sendCrc) { crc = calculateCrc(bytesPerDevice, data, crc); }
        data += bytesPerDevice;
        deviceCount--;
      }
    }
 
    if (sendCrc) { port->write(crc); }
  }
 
private:
  Stream * port;
  uint16_t deviceNumber;
  uint8_t communicationOptions;
 
  void sendCommandCore(uint8_t length, const uint8_t * cmd, bool sendCrc) override
  {
    if (port == nullptr) { lastError = 52; return; }
 
    if (deviceNumber == 0xFFFF)
    {
      port->write(cmd, length);
      if (sendCrc)
      {
        port->write(calculateCrc(length, cmd));
      }
    }
    else
    {
      uint8_t header[4];
      if (communicationOptions & (1 << MOTORON_COMMUNICATION_OPTION_14BIT_DEVICE_NUMBER))
      {
        header[0] = 0xAA;
        header[1] = deviceNumber & 0x7F;
        header[2] = deviceNumber >> 7 & 0x7F;
        header[3] = cmd[0] & 0x7F;
        port->write(header, 4);
      }
      else
      {
        header[0] = 0;
        header[1] = 0xAA;
        header[2] = deviceNumber & 0x7F;
        header[3] = cmd[0] & 0x7F;
        port->write(header + 1, 3);
      }
      port->write(cmd + 1, length - 1);
      if (sendCrc)
      {
        uint8_t crc = calculateCrc(sizeof(header), header);
        crc = calculateCrc(length - 1, cmd + 1, crc);
        port->write(crc);
      }
    }
    lastError = 0;
  }
 
  void flushTransmission()
  {
    if (port == nullptr) { return; }
    port->flush();
  }
 
  void readResponse(uint8_t length, uint8_t * response) override
  {
    if (port == nullptr)
    {
      lastError = 52;
      memset(response, 0, length);
      return;
    }
 
    bool response7Bit = communicationOptions & (1 << MOTORON_COMMUNICATION_OPTION_7BIT_RESPONSES);
    if (response7Bit && length > 7)
    {
      // In 7-bit response mode, the Motoron does not support response
      // payloads longer than 7 bytes.  That seems short enough that it would be
      // good to signal it with a special error code.
      lastError = 53;
      memset(response, 0, length);
      return;
    }
 
    port->flush();
 
    size_t byteCount = port->readBytes(response, length);
    if (byteCount != length)
    {
      lastError = 50;
      memset(response, 0, length);
      return;
    }
 
    uint8_t msbs = 0;
    if (response7Bit)
    {
      if (port->readBytes(&msbs, 1) != 1)
      {
        lastError = 54;
        return;
      }
    }
 
    bool crcEnabled = protocolOptions & (1 << MOTORON_PROTOCOL_OPTION_CRC_FOR_RESPONSES);
    if (crcEnabled)
    {
      uint8_t crc = 0;
      if (port->readBytes(&crc, 1) != 1)
      {
        lastError = 49;
        return;
      }
 
      uint8_t expected_crc = calculateCrc(length, response);
      if (response7Bit)
      {
        expected_crc = calculateCrc(1, &msbs, expected_crc);
      }
 
      if (crc != expected_crc)
      {
        lastError = 51;
        return;
      }
    }
 
    if (response7Bit)
    {
      for (uint8_t i = 0; i < length; i++)
      {
        if (msbs & 1) { response[i] |= 0x80; }
        msbs >>= 1;
      }
    }
 
    lastError = 0;
  }
};