RoboClaw.cxx

Go to the documentation of this file.

////////////////////////////////////////////////////////////////////////////////
//
// Package:   Laelasp
//
// Library:   liblaelaps
//
// File:      RoboClaw.cxx
//
/*! \file
 *
 * $LastChangedDate: 2016-04-13 11:47:47 -0600 (Wed, 13 Apr 2016) $
 * $Rev: 4386 $
 *
 * \brief RoboClaw motor controller class implementation.
 *
 * \author Robin Knight (robin.knight@roadnarrows.com)
 *
 * \par Copyright
 *   \h_copy 2015-2017. RoadNarrows LLC.\n
 *   http://www.roadnarrows.com\n
 *   All Rights Reserved
 */
/*
 * @EulaBegin@
 * 
 * Unless otherwise stated explicitly, all materials contained are copyrighted
 * and may not be used without RoadNarrows LLC's written consent,
 * except as provided in these terms and conditions or in the copyright
 * notice (documents and software) or other proprietary notice provided with
 * the relevant materials.
 * 
 * IN NO EVENT SHALL THE AUTHOR, ROADNARROWS LLC, OR ANY 
 * MEMBERS/EMPLOYEES/CONTRACTORS OF ROADNARROWS OR DISTRIBUTORS OF THIS SOFTWARE
 * BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
 * CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
 * DOCUMENTATION, EVEN IF THE AUTHORS OR ANY OF THE ABOVE PARTIES HAVE BEEN
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * THE AUTHORS AND  ROADNARROWS LLC SPECIFICALLY DISCLAIM ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
 * "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
 * 
 * @EulaEnd@
 */
////////////////////////////////////////////////////////////////////////////////

#include <sys/types.h>
#include <unistd.h>
#include <stdio.h>

#include <string>

#include "rnr/rnrconfig.h"
#include "rnr/log.h"
#include "rnr/serdev.h"

#include "Laelaps/RoboClaw.h"

using namespace std;
using namespace motor::roboclaw;

//-----------------------------------------------------------------------------
// Private 
//-----------------------------------------------------------------------------

/*!
 * \brief Test for valid motor index.
 *
 * This macro only within RoboClaw methods.
 *
 * \param motor Motor index.
 *
 * \return On failure, forces return from calling function with the appropriate
 * error code.
 */
#define ROBOCLAW_TRY_MOTOR(motor) \
  do \
  { \
    if( !isValidMotor(motor) ) \
    { \
      LOGERROR("Motor controller 0x%02x: Motor index %d invalid.", \
          m_address, motor); \
      return RC_ERROR; \
    } \
  } while(0)

#undef ROBOCLAW_DBG_ENABLE  ///< debug switch
#ifdef ROBOCLAW_DBG_ENABLE

/*!
 * \brief Debug print.
 *
 * \param addr    Motor controller address
 * \param cmdNum  Command number.
 * \param fmt     Format string.
 * \param ...     Optional variable arguments for fmt.
 */
#define ROBOCLAW_DBG(addr, cmdNum, fmt, ...) \
    fprintf(stderr, "DBG: %s[%d]: Motor controller=0x%02x: Cmd=%d: " fmt, \
        __FILE__, __LINE__, addr, cmdNum, ##__VA_ARGS__)

/*!
 * \brief Debug print message.
 *
 * \param addr    Motor controller address
 * \param cmdNum  Command number.
 * \param buf     Buffer to print.
 * \param len     Buffer length.
 * \param fmt     Format string.
 * \param ...     Optional variable arguments for fmt.
 */
#define ROBOCLAW_DBG_MSG(addr, cmdNum, buf, len, fmt, ...) \
  do \
  { \
    fprintf(stderr, "DBG: %s[%d]: Motor controller=0x%02x: Cmd=%d " fmt, \
        __FILE__, __LINE__, addr, cmdNum, ##__VA_ARGS__); \
    for(int i=0;i<len; ++i) { fprintf(stderr, "0x%02x ", buf[i]); } \
    fprintf(stderr, "\n"); \
  } while(0)

/*!
 * \brief Debug print buffer.
 *
 * \param buf     Buffer to print.
 * \param len     Buffer length.
 * \param fmt     Format string.
 * \param ...     Optional variable arguments for fmt.
 */
#define ROBOCLAW_DBG_BUF(buf, len, fmt, ...) \
  do \
  { \
    fprintf(stderr, "DBG: %s[%d]: " fmt, __FILE__, __LINE__, ##__VA_ARGS__); \
    for(int i=0;i<len; ++i) { fprintf(stderr, "0x%02x ", buf[i]); } \
    fprintf(stderr, "\n"); \
  } while(0)

#else   // Disabled

/*!
 * \brief Debug print disabled.
 */
#define ROBOCLAW_DBG(addr, cmdNum, fmt, ...)

/*!
 * \brief Debug print message disabled.
 */
#define ROBOCLAW_DBG_MSG(addr, cmdNum, buf, len, fmt, ...)

/*!
 * \brief Debug print buffer disabled.
 */
#define ROBOCLAW_DBG_BUF(buf, len, fmt, ...) \

#endif // ROBOCLAW_DBG_ENABLE

/*!
 * \brief Integer absolute value.
 *
 * \param a   Integer value.
 *
 * \return |a|
 */
inline s64_t iabs(s64_t a)
{
  return a>=0? a: -a;
}


//-----------------------------------------------------------------------------
// Class RoboClawChipSelect
//-----------------------------------------------------------------------------

RoboClawChipSelect::RoboClawChipSelect()
{
  m_addrLast = 0;
}

RoboClawChipSelect::~RoboClawChipSelect()
{
  m_addrLast = 0;
}

void RoboClawChipSelect::select(int fd, byte_t addrSel)
{
  m_addrLast = addrSel;
}

/*! Fixed no-op selection object. */
static RoboClawChipSelect csNoOp;


//-----------------------------------------------------------------------------
// Class RoboClawComm
//-----------------------------------------------------------------------------

RoboClawComm::RoboClawComm()
{
  m_nBaudRate   = 0;
  m_fd          = -1;
  m_pChipSelect = &csNoOp;
  m_bDbgEnable  = false;
}

RoboClawComm::RoboClawComm(std::string        &strDevName,
                           int                nBaudRate,
                           RoboClawChipSelect *pChipSelect)
{
  m_nBaudRate   = 0;
  m_fd          = -1;
  m_pChipSelect = &csNoOp;
  m_bDbgEnable  = false;

  open(strDevName, nBaudRate, pChipSelect);
}

RoboClawComm::~RoboClawComm()
{
  close();
}

int RoboClawComm::open(std::string        &strDevName,
                       int                 nBaudRate,
                       RoboClawChipSelect *pChipSelect)
{
  m_fd = SerDevOpen(strDevName.c_str(), nBaudRate, 8, 'N', 1, false, false);

  if( m_fd < 0 )
  {
    LOGERROR("Failed to open %s@%d.", strDevName.c_str(), nBaudRate);
    return RC_ERROR;
  }

  flushInput();

  m_strDevName  = strDevName;
  m_nBaudRate   = nBaudRate;
  m_pChipSelect = pChipSelect == NULL? &csNoOp: pChipSelect;

  LOGDIAG3("Opened interface to RoboClaw motor controller %s@%d.",
      strDevName.c_str(), nBaudRate);

  return OK;
}

int RoboClawComm::close()
{
  if( m_fd >= 0 )
  {
    SerDevClose(m_fd);
    LOGDIAG3("Closed %s interface to RoboClaw motor controller.", 
        m_strDevName.c_str());
  }

  m_strDevName.clear();
  m_nBaudRate   = 0;
  m_fd          = -1;
  m_pChipSelect = &csNoOp;
  m_bDbgEnable  = false;

  return OK;
}

int RoboClawComm::flushInput()
{
  SerDevFIFOInputFlush(m_fd);
}

int RoboClawComm::execCmd(byte_t cmd[], size_t lenCmd)
{
  int   rc;

  LOGWARN("Motor controller 0x%02x: execCmd() function is deprecated.");

  if( (rc = sendCmd(cmd, lenCmd)) != OK )
  {
    LOGERROR("Motor controller 0x%02x: Failed to send command %u.",
        cmd[FieldPosAddr], cmd[FieldPosCmd]);
    flushInput();
  }

  return rc;
}

int RoboClawComm::execCmdWithAckRsp(byte_t cmd[], size_t lenCmd, MsgFmt fmtCmd)
{
  int   rc;

  if( (rc = sendCmd(cmd, lenCmd, fmtCmd)) == OK )
  {
    if( (rc = recvAck()) != OK )
    {
      LOGERROR("Motor controller 0x%02x: Failed to receive ack for command %u.",
        cmd[FieldPosAddr], cmd[FieldPosCmd]);
      rc = RC_ERROR;
    }
  }
  else
  {
    LOGERROR("Motor controller 0x%02x: Failed to send command %u.",
        cmd[FieldPosAddr], cmd[FieldPosCmd]);
    rc = RC_ERROR;
  }

  if( rc == RC_ERROR )
  {
    flushInput();
  }

  return rc;
}

int RoboClawComm::execCmdWithDataRsp(byte_t cmd[],  size_t lenCmd,
                                     byte_t rsp[],  size_t lenRsp,
                                     MsgFmt fmtCmd, MsgFmt fmtRsp)
{
  uint_t  uCrcCalc;
  int     n;
  int     rc;

  if( (rc = sendCmd(cmd, lenCmd, fmtCmd)) == OK )
  {
    if( (n = recvDataRsp(rsp, lenRsp)) == (int)lenRsp )
    {
      if( fmtRsp == MsgWithCrc )
      {
        uCrcCalc = crc16(m_uCrcCmd, rsp, lenRsp-2);
        if( m_uCrcRsp != uCrcCalc )
        {
          LOGERROR("Motor controller 0x%02x: "
            "Command %u response CRC-16 mismatch: "
            "Expected 0x%04x, received 0x%04x.",
            cmd[FieldPosAddr], cmd[FieldPosCmd],
            uCrcCalc, m_uCrcRsp);
          rc = RC_ERROR;
        }
      }
    }
    else if( n > 0 )
    {
      LOGWARN("Motor controller 0x%02x: "
          "Command %u received partial response of %d/%zu bytes - ignoring.",
            cmd[FieldPosAddr], cmd[FieldPosCmd], n, lenRsp);
      rc = RC_ERROR;
    }
    else
    {
      LOGERROR("Motor controller 0x%02x: Command %u received no response.",
            cmd[FieldPosAddr], cmd[FieldPosCmd]);
      rc = RC_ERROR;
    }
  }
  else
  {
    LOGERROR("Motor controller 0x%02x: Failed to send command %u.",
            cmd[FieldPosAddr], cmd[FieldPosCmd]);
    rc = RC_ERROR;
  }

  if( rc == RC_ERROR )
  {
    flushInput();
  }

  return rc;
}

int RoboClawComm::sendCmd(byte_t cmd[], size_t lenCmd, MsgFmt fmtCmd)
{
  m_pChipSelect->select(m_fd, cmd[FieldPosAddr]);

  // command's CRC
  m_uCrcCmd = crc16(0, cmd, lenCmd);

  // append CRC
  if( fmtCmd == MsgWithCrc )
  {
    pack16(m_uCrcCmd, &cmd[lenCmd]);
    lenCmd += 2;
  }

  if( m_bDbgEnable )
  {
    ROBOCLAW_DBG_BUF(cmd, lenCmd,
        "Motor controller=0x%02x: Cmd=%d: CRC=0x%04x: sendCmd(): ",
        cmd[FieldPosAddr], cmd[FieldPosCmd], m_uCrcCmd);
  }

  if( SerDevWrite(m_fd, cmd, lenCmd, CmdTimeout) == lenCmd )
  {
    return OK;
  }
  else
  {
    return RC_ERROR;
  }
}

int RoboClawComm::recvDataRsp(byte_t rsp[], size_t lenRsp)
{
  ssize_t   n;

  if( (n = SerDevRead(m_fd, rsp, lenRsp, RspTimeout)) >= 0 )
  {
    // last two bytes are the 16-bit crc
    if( n >= 2 )
    {
      unpack16(&rsp[n-2], m_uCrcRsp);
    }

    if( m_bDbgEnable )
    {
      ROBOCLAW_DBG_BUF(rsp, n, "recvDataRsp(): ");
    }

    return (int)n;
  }
  else
  {
    return RC_ERROR;
  }
}

int RoboClawComm::recvAck()
{
  ssize_t n;
  byte_t  ack;

  n = SerDevRead(m_fd, &ack, 1, RspTimeout);

  if( n != 1 )
  {
    return RC_ERROR;
  }

  if( m_bDbgEnable )
  {
    ROBOCLAW_DBG_BUF((&ack), n, "recvAck(): ");
  }

  if( ack != RspAck )
  {
    return RC_ERROR;
  }
  else
  {
    return OK;
  }
}

int RoboClawComm::pack16(uint_t val, byte_t buf[])
{
  buf[0] = (byte_t)((val >> 8) & 0xff);
  buf[1] = (byte_t)(val & 0xff);
  return 2;
}

int RoboClawComm::unpack16(byte_t buf[], uint_t &val)
{
  val = ((uint_t)(buf[0]) << 8) | (uint_t)buf[1];
  return 2;
}

int RoboClawComm::unpack16(byte_t buf[], int &val)
{
  s16_t v;

  v = ((s16_t)(buf[0]) << 8) | (s16_t)buf[1];
  val = (int)v;

  return 2;
}

int RoboClawComm::pack32(uint_t val, byte_t buf[])
{
  buf[0] = (byte_t)((val >> 24) & 0xff);
  buf[1] = (byte_t)((val >> 16) & 0xff);
  buf[2] = (byte_t)((val >>  8) & 0xff);
  buf[3] = (byte_t)(val & 0xff);
  return 4;
}

int RoboClawComm::unpack32(byte_t buf[], uint_t &val)
{
  val = ((uint_t)(buf[0]) << 24) |
        ((uint_t)(buf[1]) << 16) |
        ((uint_t)(buf[2]) <<  8) |
        (uint_t)buf[3];
  return 4;
}

int RoboClawComm::unpack32(byte_t buf[], int &val)
{
  s32_t v;

  v = ((s32_t)(buf[0]) << 24) |
      ((s32_t)(buf[1]) << 16) |
      ((s32_t)(buf[2]) <<  8) |
      (s32_t)buf[3];
  val = (int)v;

  return 4;
}

byte_t RoboClawComm::checksum(byte_t buf[], size_t lenBuf, bool bAck)
{
  uint_t  sum;
  byte_t  chksum;
  size_t  i;

  for(i=0, sum=0; i<lenBuf; ++i)
  {
    sum += (uint_t)buf[i];
  }

  chksum = (byte_t)(sum) & CheckSumMask;
 
  if( bAck )
  {
    chksum |= AckReqBit;
  }

  return chksum;
}

uint_t RoboClawComm::crc16(uint_t crc, byte_t buf[], size_t lenBuf)
{
  size_t  i;
  size_t  bit;

  for(i = 0; i < lenBuf; ++i)
  {
    crc = crc ^ ((uint_t)buf[i] << 8);

    for(bit = 0; bit < 8; ++bit)
    {
      if( crc & 0x8000 )
      {
        crc = (crc << 1) ^ 0x1021;
      }
      else
      {
        crc = crc << 1;
      }
    }
  }
  return crc & 0xffff;
}


//-----------------------------------------------------------------------------
// Class RoboClaw
//-----------------------------------------------------------------------------

RoboClaw::RoboClaw(RoboClawComm &comm,
                   const byte_t address,
                   const string &strNameId) :
    m_comm(comm), m_address(address), m_strNameId(strNameId)
{
  for(int i=0; i<NumMotors; ++i)
  {
    m_nMotorDir[i]  = MotorDirNormal;
    m_nEncPrev[i]   = 0;
    m_nEncoder[i]   = 0;
  }
}

RoboClaw::~RoboClaw()
{
  if( m_comm.isOpen() )
  {
    cmdStop();
  }
}

bool RoboClaw::probe(byte_t address)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  int     rc;

  cmd[n++] = address;
  cmd[n++] = CmdReadMainBattVolt; // probe with a safe read command

  rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4);

  return rc == OK;
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Identity Command
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 21
int RoboClaw::cmdReadFwVersion(string &strFwVer)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  uint_t  uCrcCmd;
  uint_t  uCrcRsp;
  uint_t  uCrcCalc;
  size_t  n = 0;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadFwVersion;

  if( m_comm.sendCmd(cmd, n) != OK )
  {
    return RC_ERROR;
  }

  n = m_comm.recvDataRsp(rsp, ParamVerLen);

  if( n > ParamVerLenMin )
  {
    //
    // Validate CRC 
    //
    uCrcCmd  = m_comm.getLastCmdCrc();
    uCrcRsp  = m_comm.getLastRspCrc();
    uCrcCalc = m_comm.crc16(uCrcCmd, rsp, n-2);

    if( uCrcRsp != uCrcCalc )
    {
      LOGERROR("Motor controller 0x%02x: "
            "Command %u response CRC-16 mismatch: "
            "Expected 0x%04x, received 0x%04x.",
            cmd[FieldPosAddr], cmd[FieldPosCmd],
            uCrcCalc, uCrcRsp);
      return RC_ERROR;
    }

    rsp[n-ParamVerLenMin] = 0;  // take off trailing junk
    strFwVer = (char *)rsp;     // convert to string

    LOGDIAG3("%s.", strFwVer.c_str());

    return OK;
  }
  else
  {
    LOGERROR("Motor controller 0x%02x: "
        "Failed to receive firmware version.",
            m_address);
    return RC_ERROR;
  }
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Main Battery Voltage Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 24
int RoboClaw::cmdReadMainBattVoltage(double &volts)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadMainBattVolt;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    m_comm.unpack16(rsp, val);
    volts = (double)val * ParamVoltScale;
  }

  return rc;
}

void RoboClaw::getMainBattCutoffRange(double &minmin, double &maxmax) const
{
  minmin = ParamVoltMainMin;
  maxmax = ParamVoltMax;
}

// Command 59
int RoboClaw::cmdReadMainBattCutoffs(double &min, double &max)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  valMin, valMax;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadMainBattCutoffs;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 6)) == OK )
  {
    n   = m_comm.unpack16(rsp, valMin);
    n  += m_comm.unpack16(rsp+n, valMax);

    ROBOCLAW_DBG(cmd[FieldPosAddr], cmd[FieldPosCmd],
        "MainBatt Cutoffs [%u, %u]\n", valMin, valMax);

    min = (double)valMin * ParamVoltScale;
    max = (double)valMax * ParamVoltScale;
  }

  return rc;
}

// Command 57
int RoboClaw::cmdSetMainBattCutoffs(const double min, const double max)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;
  double  fMin, fMax;
  uint_t  valMin, valMax;
  int     rc;

  fMin    = min / ParamVoltScale;
  valMin  = (uint_t)fcap(fMin, ParamVoltMainMin, ParamVoltMax);

  fMax    = max / ParamVoltScale;
  valMax  = (uint_t)fcap(fMax, fMin, ParamVoltMax);

  //
  // header
  //
  cmd[n++] = m_address;
  cmd[n++] = CmdSetMainBattCutoffs;

  //
  // values
  //
  k = m_comm.pack16(valMin, cmd+n);
  n += k;

  k = m_comm.pack16(valMax, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Motor Current Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 49
//
// Note: RoboClaw User Manual v5 error. The data are signed 16-bit integers
//
int RoboClaw::cmdReadMotorCurrentDraw(double &amps1, double &amps2)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  k, n = 0;
  int     val1, val2;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadMotorDraw;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 6)) == OK )
  {
    n = 0;

    k = m_comm.unpack16(rsp, val1);
    n += k;

    k  = m_comm.unpack16(rsp+n, val2);
    n += k;

    amps1 = (double)val1 * ParamAmpScale;
    amps2 = (double)val2 * ParamAmpScale;
  }

  return rc;
}

// Command 135, 136
//
// Note: RoboClaw User Manual v5 error. Each value is 32 bits.
//
int RoboClaw::cmdReadMotorMaxCurrentLimit(int motor, double &maxAmps)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  k, n = 0;
  uint_t  val1, val2;
  double  minAmps;      // always zero - so ignore
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdReadMaxCurrentMot1: CmdReadMaxCurrentMot2;

  m_comm.enableDbg(true);

  rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 10);

  if( rc == OK )
  {
    n = 0;

    k = m_comm.unpack32(rsp, val1);
    n += k;

    k  = m_comm.unpack32(rsp+n, val2);
    n += k;

    maxAmps = (double)val1 * ParamAmpScale;
    minAmps = (double)val2 * ParamAmpScale;
  }

  m_comm.enableDbg(false);

  return rc;
}

// Command 133, 134
//
// Note: RoboClaw User Manual v5 error. Each value is 32 bits.
//
int RoboClaw::cmdSetMotorMaxCurrentLimit(int motor, const double maxAmps)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  k, n = 0;
  double  fMax;
  uint_t  valMin;   // always zero
  uint_t  valMax;
  int     rc;

  // convert interface values 
  // RDK fMax    = fcap(maxAmps, ParamAmpMinSane, ParamAmpMax);
  fMax    = fcap(maxAmps, ParamAmpMinSane, 100.0); // RDK
  valMax  = (uint_t)(fMax / ParamAmpScale);

  valMin  = 0;

  //
  // pack header
  //
  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdSetMaxCurrentMot1: CmdSetMaxCurrentMot2;

  //
  // pack values
  //
  k = m_comm.pack32(valMax, cmd+n);
  n += k;

  k = m_comm.pack32(valMin, cmd+n);
  n += k;

  // execute
  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Logic Battery Voltage Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 25
int RoboClaw::cmdReadLogicVoltage(double &volts)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadLogicVolt;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    m_comm.unpack16(rsp, val);
    volts = (double)val * ParamVoltScale;
  }

  return rc;
}

void RoboClaw::getLogicCutoffRange(double &minmin, double &maxmax) const
{
  minmin = ParamVoltLogicMin;
  maxmax = ParamVoltMax;
}

// Command 60
int RoboClaw::cmdReadLogicCutoffs(double &min, double &max)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  valMin, valMax;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadLogicCutoffs;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 6)) == OK )
  {
    n   = m_comm.unpack16(rsp, valMin);
    n  += m_comm.unpack16(rsp+n, valMax);

    ROBOCLAW_DBG(cmd[FieldPosAddr], cmd[FieldPosCmd],
        "Logic Cutoffs [%u, %u]\n", valMin, valMax);

    min = (double)valMin * ParamVoltScale;
    max = (double)valMax * ParamVoltScale;
  }

  return rc;
}

// Command 58
int RoboClaw::cmdSetLogicCutoffs(const double min, const double max)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;
  double  fMin, fMax;
  uint_t  valMin, valMax;
  int     rc;

  fMin    = min / ParamVoltScale;
  valMin  = (uint_t)fcap(fMin, ParamVoltLogicMin, ParamVoltMax);

  fMax    = max / ParamVoltScale;
  valMax  = (uint_t)fcap(fMax, fMin, ParamVoltMax);

  cmd[n++] = m_address;
  cmd[n++] = CmdSetLogicCutoffs;

  k  = m_comm.pack16(valMin, cmd+n);
  n += k;

  k  = m_comm.pack16(valMax, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Motor PID Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 55, 56
int RoboClaw::cmdReadVelocityPidConst(int   motor,
                                      u32_t &Kp,
                                      u32_t &Ki,
                                      u32_t &Kd,
                                      u32_t &qpps)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  k, n = 0;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdReadVelPidMot1: CmdReadVelPidMot2;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 18)) == OK )
  {
    n = 0;

    k = m_comm.unpack32(rsp,   Kp);
    n += k;

    k = m_comm.unpack32(rsp+n, Ki);
    n += k;

    k = m_comm.unpack32(rsp+n, Kd);
    n += k;

    k = m_comm.unpack32(rsp+n, qpps);
    n += k;

    ROBOCLAW_DBG(cmd[FieldPosAddr], cmd[FieldPosCmd],
        "VelPid: motor=%d, P=0x%08x, I=0x%08x, D=0x%08x, Q=%u\n",
        motor, Kp, Ki, Kd, qpps);
  }

  return rc;
}

// Command 28,29
int RoboClaw::cmdSetVelocityPidConst(int         motor,
                                     const u32_t Kp,
                                     const u32_t Ki,
                                     const u32_t Kd,
                                     const u32_t qpps)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  k, n = 0;
  int     rc;

  ROBOCLAW_TRY_MOTOR(motor);

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdSetVelPidMot1: CmdSetVelPidMot2;

  // order is strangly D,P,I,QPPS
  k  = m_comm.pack32(Kd, cmd+n);
  n += k;

  k  = m_comm.pack32(Kp, cmd+n);
  n += k;

  k  = m_comm.pack32(Ki, cmd+n);
  n += k;

  k  = m_comm.pack32(qpps, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Health and Status Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 82
int RoboClaw::cmdReadBoardTemperature(double &temp)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadTemp;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    m_comm.unpack16(rsp, val);
    temp = (double)val * ParamTempScale;
  }

  return rc;
}

// Command 90
int RoboClaw::cmdReadStatus(uint_t &status)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadStatus;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    m_comm.unpack16(rsp, status);
  }

  return rc;
}

// Command 47
int RoboClaw::cmdReadCmdBufLengths(uint_t &len1, uint_t &len2)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadBufLen;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    len1 = (uint_t)rsp[0];
    len2 = (uint_t)rsp[1];
  }

  return rc;
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Encoder Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 91
int RoboClaw::cmdReadEncoderMode(byte_t &mode1, byte_t &mode2)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdReadEncoderMode;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 4)) == OK )
  {
    mode1 = rsp[0];
    mode2 = rsp[1];
    //LOGDIAG3("Motor controller 0x%02x: mode1=0x%02x, mode2=0x%02x.",
    //    m_address, mode1, mode2);
  }

  return rc;
}

// Command 92, 93
int RoboClaw::cmdSetEncoderMode(int motor, byte_t mode)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  mask = ParamEncModeRCAnalogBit | ParamEncModeQuadAbsBit;
  size_t  n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdSetEncoderModeMot1: CmdSetEncoderModeMot2;
  cmd[n++] = (byte_t)(mask & mode);

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 92, 93
int RoboClaw::cmdSetEncoderMode2(byte_t mode1, byte_t mode2)
{
  int rc;

  if( (rc = cmdSetEncoderMode(Motor1, mode1)) == OK )
  {
    rc = cmdSetEncoderMode(Motor2, mode2);
  }

  return rc;
}

// Command 20
int RoboClaw::cmdResetQEncoders()
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  n = 0;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdResetEncoderCntrs;

  if( (rc = m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc)) == OK )
  {
    for(int i=0; i<NumMotors; ++i)
    {
      m_nEncPrev[i] = 0;
      m_nEncoder[i] = 0;
    }
  }

  return rc;
}

// Command 16, 17
int RoboClaw::cmdReadQEncoder(int motor, s64_t &encoder)
{
  static s64_t  MaxEncDelta = 2048; // maximum encoder delta on wrap

  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  enc32;      // 32-bit encoder value (quad pulses).
  byte_t  status;     // status bits indicating sign, and over/under flow

  s64_t   enc64;      // 64-bit signed encoder value (quad pulses).
  s64_t   delta;      // encoder delta from previous value
  int     rc;

  ROBOCLAW_TRY_MOTOR(motor);

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdReadEncoderMot1: CmdReadEncoderMot2;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 7)) == OK )
  {
    ROBOCLAW_DBG_MSG(cmd[FieldPosAddr], cmd[FieldPosCmd], rsp, 7,
        "cmdReadQEncoder(motor=%d): rsp = ", motor);

    //
    // Get 32-bit unsigned encoder value plus its associated status.
    //
    // Note:  This 2-tuple should be atomically kept insync by the firmware but
    //        this is not always the case. During a sharp velocity change
    //        (e.g. stopping) the velocity pid will rightly try to back drive.
    //        The status byte will indicate the _desired_ direction, not the
    //        actual.
    //
    n = m_comm.unpack32(rsp, enc32);
    status = rsp[n];
      
    enc64 = (s64_t)enc32;

    //
    // Debug left_front encoder to reverse engineer and test encoder algorithm.
    //
    //if( m_address == 0x80 && motor == Motor1 && m_nEncPrev[motor] != enc64 )
    //{
    //  fprintf(stderr, "DBG %lld 0x%02x\n", enc64, status);
    //}

    //
    // Track underflow/overflow for now. Not too sure if any action or special
    // calculations are needed.
    //
    // Note: Condition is cleared after reading.
    //
    if( status & ParamEncStatusUnderFlow )
    {
      LOGDIAG2("Motor controller 0x%02x: Motor %d: Encoder underflow: "
                "Encoder32=%u.",
              m_address, motor, enc32);
    }
    if( status & ParamEncStatusOverFlow )
    {
      LOGDIAG2("Motor controller 0x%02x: Motor %d: Encoder overflow: "
                "Encoder32=%u",
              m_address, motor, enc32);
    }

    //
    // Moving backwards. There are two cases:
    //  Case 1: no wrap:  example: previous 1500, current: 1000
    //  Case 2: wrap:     example: previous 15,   current: 4294967200
    //
    if( status & ParamEncStatusDirBackward )
    {
      if( enc64 <= m_nEncPrev[motor] )  // no wrap
      {
        delta = enc64 - m_nEncPrev[motor];
      }
      else  // wrap
      {
        delta = enc64 - ParamEncQuadMax - m_nEncPrev[motor];
        if( iabs(delta) > MaxEncDelta ) // not really backwards
        {
          delta = enc64 - m_nEncPrev[motor];
        }
      }
    }

    //
    // Moving forwards. There are two cases:
    //  Case 1: no wrap:  example: previous 1000,       current: 1500
    //  Case 2: wrap:     example: previous 4294967200, current: 15
    //
    else
    {
      if( enc64 >= m_nEncPrev[motor] ) // no wrap
      {
        delta = enc64 - m_nEncPrev[motor];
      }
      else  // wrap
      {
        delta = enc64 + ParamEncQuadMax - m_nEncPrev[motor];
        if( iabs(delta) > MaxEncDelta ) // not really forwards
        {
          delta = enc64 - m_nEncPrev[motor];
        }
      }
    }

    //
    // Update accumulated value and save current as new previous.
    //
    m_nEncoder[motor] += (m_nMotorDir[motor] * delta);
    m_nEncPrev[motor] = enc64;
    encoder           = m_nEncoder[motor];
  }

  return rc;
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Duty Cycle Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 34
int RoboClaw::cmdDutyDrive2(double duty1, double duty2)
{
  byte_t  cmd[CmdMaxBufLen];
  int     speed1, speed2;
  size_t  k, n = 0;

  speed1 = (int)((double)duty1 * ParamDutyCycleMax);
  speed2 = (int)((double)duty2 * ParamDutyCycleMax);

  cmd[n++] = m_address;
  cmd[n++] = CmdDriveDuty;

  k  = m_comm.pack16((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack16((int)speed2, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Speed and Position Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 18, 19
int RoboClaw::cmdReadQSpeed(int motor, s32_t &speed)
{
  byte_t  cmd[CmdMaxBufLen];
  byte_t  rsp[RspMaxBufLen];
  size_t  n = 0;
  uint_t  val;
  byte_t  status;
  int     rc;

  ROBOCLAW_TRY_MOTOR(motor);

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdReadSpeedMot1: CmdReadSpeedMot2;

  if( (rc = m_comm.execCmdWithDataRsp(cmd, n, rsp, 7)) == OK )
  {
    ROBOCLAW_DBG_MSG(cmd[FieldPosAddr], cmd[FieldPosCmd], rsp, 7,
        "cmdReadQSpeed(motor=%d): rsp = ", motor);

    n = m_comm.unpack32(rsp, val);
    status = rsp[n];
    speed = (s32_t)val;

    if( status & ParamEncStatusDirBackward )
    {
      speed = -speed;
    }
  }

  return rc;
}

// Command 35, 36
int RoboClaw::cmdQDrive(int motor, s32_t speed)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed *= m_nMotorDir[motor];

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdDriveQMot1: CmdDriveQMot2;

  k  = m_comm.pack32((int)speed, cmd+n);
  n += k;

  ROBOCLAW_DBG_MSG(cmd[FieldPosAddr], cmd[FieldPosCmd], cmd, n,
      "cmdQDrive(motor=%d, speed=%d): cmd = ", motor, speed);

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 37
int RoboClaw::cmdQDrive2(s32_t speed1, s32_t speed2)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  speed1 *= m_nMotorDir[Motor1];
  speed2 *= m_nMotorDir[Motor2];

  cmd[n++] = m_address;
  cmd[n++] = CmdDriveQ;

  k  = m_comm.pack32((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed2, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 38, 39
int RoboClaw::cmdQDriveWithAccel(int motor, s32_t speed, u32_t accel)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed *= m_nMotorDir[motor];

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdDriveQAccelMot1: CmdDriveQAccelMot2;

  k  = m_comm.pack32((uint_t)accel, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 50
int RoboClaw::cmdQDriveWithAccel(s32_t speed1, u32_t accel1,
                                 s32_t speed2, u32_t accel2)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  speed1 *= m_nMotorDir[Motor1];
  speed2 *= m_nMotorDir[Motor2];

  cmd[n++] = m_address;
  cmd[n++] = CmdDriveQAccel2;

  k  = m_comm.pack32((uint_t)accel1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)accel2, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed2, cmd+n);
  n += k;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 41, 42
int RoboClaw::cmdQDriveForDist(int motor, s32_t speed, u32_t dist, bool bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed *= m_nMotorDir[motor];

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdBufDriveQDistMot1: CmdBufDriveQDistMot2;

  k  = m_comm.pack32((int)speed, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 43
int RoboClaw::cmdQDriveForDist(s32_t speed1, u32_t dist1,
                               s32_t speed2, u32_t dist2,
                               bool  bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  speed1 *= m_nMotorDir[Motor1];
  speed2 *= m_nMotorDir[Motor2];

  cmd[n++] = m_address;
  cmd[n++] = CmdBufDriveQDist;

  k  = m_comm.pack32((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed2, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist2, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 44, 45
int RoboClaw::cmdQDriveWithAccelForDist(int   motor,
                                        s32_t speed,
                                        u32_t accel,
                                        u32_t dist,
                                        bool  bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed *= m_nMotorDir[motor];

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdBufDriveQAccelDistMot1:
                              CmdBufDriveQAccelDistMot2;

  k  = m_comm.pack32((uint_t)accel, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 51
int RoboClaw::cmdQDriveWithAccelForDist(s32_t speed1,
                                        u32_t accel1,
                                        u32_t dist1,
                                        s32_t speed2,
                                        u32_t accel2,
                                        u32_t dist2,
                                        bool  bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  speed1 *= m_nMotorDir[Motor1];
  speed2 *= m_nMotorDir[Motor2];

  cmd[n++] = m_address;
  cmd[n++] = CmdBufDriveQAccel2Dist;

  k  = m_comm.pack32((uint_t)accel1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)accel2, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed2, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)dist2, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 65, 66
int RoboClaw::cmdQDriveWithProfileToPos(int   motor,
                                        s32_t speed,
                                        u32_t accel,
                                        u32_t deccel,
                                        s32_t pos,
                                        bool  bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed *= m_nMotorDir[motor];

  cmd[n++] = m_address;
  cmd[n++] = motor == Motor1? CmdBufDriveQProfPosMot1:
                              CmdBufDriveQProfPosMot2;

  k  = m_comm.pack32((uint_t)accel, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)deccel, cmd+n);
  n += k;

  k  = m_comm.pack32((int)pos, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 67
int RoboClaw::cmdQDriveWithProfileToPos(s32_t speed1,
                                        u32_t accel1,
                                        u32_t deccel1,
                                        s32_t pos1,
                                        s32_t speed2,
                                        u32_t accel2,
                                        u32_t deccel2,
                                        s32_t pos2,
                                        bool  bQueue)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  k, n = 0;

  speed1 *= m_nMotorDir[Motor1];
  speed2 *= m_nMotorDir[Motor2];

  cmd[n++] = m_address;
  cmd[n++] = CmdBufDriveQProfPos;

  k  = m_comm.pack32((uint_t)accel1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)deccel1, cmd+n);
  n += k;

  k  = m_comm.pack32((int)pos1, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)accel2, cmd+n);
  n += k;

  k  = m_comm.pack32((int)speed2, cmd+n);
  n += k;

  k  = m_comm.pack32((uint_t)deccel2, cmd+n);
  n += k;

  k  = m_comm.pack32((int)pos2, cmd+n);
  n += k;

  cmd[n++] = bQueue? ParamCmdBufQueue: ParamCmdBufPreempt;

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 0, 1, 4, 5
int RoboClaw::cmdSDrive(int motor, int speed)
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  n = 0;

  ROBOCLAW_TRY_MOTOR(motor);

  speed  = RoboClaw::cap(speed, MotorSpeedMaxBackward, MotorSpeedMaxForward);
  speed *= m_nMotorDir[motor];

  if( speed >= 0 )
  {
    cmd[n++] = m_address;
    cmd[n++] = motor == Motor1? CmdDriveForwardMot1: CmdDriveForwardMot2;
    cmd[n++] = (byte_t)speed;
  }
  else
  {
    cmd[n++] = m_address;
    cmd[n++] = motor == Motor1? CmdDriveBackwardMot1: CmdDriveBackwardMot2;
    cmd[n++] = (byte_t)(-speed);
  }

  return m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);
}

// Command 0, 1, 4, 5
int RoboClaw::cmdSDrive2(int speed1, int speed2)
{
  int rc;

  if( (rc = cmdSDrive(Motor1, speed1)) == OK )
  {
    rc = cmdSDrive(Motor2, speed2);
  }
  return rc;
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Stop Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

// Command 35, 36
int RoboClaw::cmdStop(int motor)
{
  return cmdQDrive(motor, 0);
}

// Command 38, 39
int RoboClaw::cmdStopWithDecel(int motor, u32_t decel)
{
  return cmdQDriveWithAccel(motor, 0, decel);
}

// Command 37
int RoboClaw::cmdStop()
{
  return cmdQDrive2(0, 0);
}

// Command 50
int RoboClaw::cmdStopWithDecel(u32_t decel)
{
  return cmdQDriveWithAccel(0, decel, 0, decel);
}

// Command 37
int RoboClaw::cmdEStop()
{
  return cmdStop();
}

// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
// Misc Commands
// . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

//
// Command 94
//
// Note: RoboClaw User Manual v5 errors.
//   The command appends a 4-btye magic number
//   The command includes a CRC checksum.
//
int RoboClaw::cmdWriteSettingsToEEPROM()
{
  byte_t  cmd[CmdMaxBufLen];
  size_t  n = 0;
  int     rc;

  cmd[n++] = m_address;
  cmd[n++] = CmdWriteEEPROM;

  // magic number
  cmd[n++] = 0xe2;
  cmd[n++] = 0x2e;
  cmd[n++] = 0xab;
  cmd[n++] = 0x7a;

  rc = m_comm.execCmdWithAckRsp(cmd, n, MsgWithCrc);

  // give a little time for the controller to write to its NVM
  if( rc == OK )
  {
    usleep(100000);   // 0.1 second to give controller time to write to nvm
  }

  return rc;
}