DynaCommSerial Class Reference

Dynamixel Serial Bus Communications Class. More...

#include <DynaCommSerial.h>

Inheritance diagram for DynaCommSerial:

Public Types
enum	CtlSignal {   CtlSignalMinGPIO = -999,   CtlSignalMaxGPIO = 999,   CtlSignalModemRTS = 1000,   CtlSignalModemCTS = 1001,   CtlSignalCustom = 1010,   CtlSignalNone = 1011 }
	Half-duplex control signals. More...
Public Types inherited from DynaComm
typedef void(*	HalfDuplexTxFunc_T) (void *pArg)
	Half-duplex control transmit function type.
typedef void(*	HalfDuplexRxFunc_T) (void *pArg, size_t uNumTxBytes)
	Half-duplex control receive function type.

Public Member Functions
	DynaCommSerial ()
	Default constructor.
	DynaCommSerial (const char *sSerialDevName, int nBaudRate)
	Initialization constructor. More...
virtual	~DynaCommSerial ()
	Destructor.
const char *	GetSerialDeviceName () const
virtual int	GetResourceId () const
	Get system-unique resource identifier. More...
virtual int	Open (const char *sSerialDevName, int nBaudRate)
	Open serial communication to dynamixel bus. More...
virtual int	Open ()
	(Re)Open serial communication to dynamixel bus. More...
virtual int	Close ()
	Close serial communication to dynamixel bus. More...
virtual int	SetBaudRate (int nNewBaudRate)
	Set the Dynamixel Bus new baud rate. More...
virtual int	SetHalfDuplexCtl (int nSignal, HalfDuplexTxFunc_T fnEnableTx=NULL, HalfDuplexRxFunc_T fnEnableRx=NULL)
virtual int	Read8 (int nServoId, uint_t uAddr, byte_t *pVal)
	Read an 8-bit value from Dynamixel servo control table. More...
virtual int	Write8 (int nServoId, uint_t uAddr, byte_t byVal)
	Write an 8-bit value to Dynamixel servo control table. More...
virtual int	Read16 (int nServoId, uint_t uAddr, ushort_t *pVal)
	Read a 16-bit value from Dynamixel servo control table. More...
virtual int	Write16 (int nServoId, uint_t uAddr, ushort_t uhVal)
	Write a 16-bit value to Dynamixel servo control table. More...
virtual int	SyncWrite (uint_t uAddr, uint_t uValSize, DynaSyncWriteTuple_T tuples[], uint_t uCount)
	Synchronous Write 8/16-bit values to a list of Dynamixel servos. More...
virtual bool	Ping (int nServoId)
	Ping the servo. More...
virtual int	Reset (int nServoId)
	Reset a servo back to default values. More...
Public Member Functions inherited from DynaComm
	DynaComm ()
	DynaComm (const char *sUri, int nBaudRate)
virtual	~DynaComm ()
	Destructor.
const char *	GetDeviceUri () const
const int	GetBaudRate () const
virtual int	Read8 (int nServoId, uint_t uAddr, uint_t *pVal)
	Read an 8-bit value from Dynamixel servo control table. More...
virtual int	Write8 (int nServoId, uint_t uAddr, uint_t uVal)
	Write an 8-bit value to Dynamixel servo control table. More...
virtual int	Read16 (int nServoId, uint_t uAddr, uint_t *pVal)
	Read a 16-bit value from Dynamixel servo control table. More...
virtual int	Write16 (int nServoId, uint_t uAddr, uint_t uVal)
	Write a 16-bit value to Dynamixel servo control table. More...
virtual int	vSyncWrite (uint_t uAddr, uint_t uValSize, uint_t uCount,...)
	Synchronous write 8/16-bit values to a list of Dynamixel servos. More...
virtual bool	IsOpen ()
	Test if Dynamixel Bus is open. More...
virtual uint_t	GetAlarms ()
	Get current alarms. More...
virtual void	ClearAlarms ()
	Clear current alarms.
virtual uint_t	GetBusStatus ()
	Get the Dynamixel Bus status. More...

Static Public Attributes
static const int	RetryMax = 5
	maximum number of I/O retries
static const int	RetryWait = 20000
	microsecond wait between retries

Protected Member Functions
int	MakeSerialIndex (const char *sSerialDevName)
	Make serial index from serial device name. More...
void	InitRTS ()
	Intialize RTS signalling.
void	InitCTS ()
	Intialize CTS signalling.
void	InitGPIO (int nGpioNum)
	Intialize GPIO signalling. More...

Static Protected Member Functions
static void	EnableTxRTS (void *pArg)
	Enable transmit via RTS signal. More...
static void	EnableRxRTS (void *pArg, size_t uNumTxBytes)
	Enable receive via RTS signal. More...
static void	EnableTxCTS (void *pArg)
	Enable transmit via CTS signal. More...
static void	EnableRxCTS (void *pArg, size_t uNumTxBytes)
	Enable receive via CTS signal. More...
static void	EnableTxGPIO (void *pArg)
	Enable transmit via GPIO signal. More...
static void	EnableRxGPIO (void *pArg, size_t uNumTxBytes)
	Enable receive via GPIO signal. More...

Protected Attributes
char *	m_sSerialDevName
	serial device name
int	m_fd
	serial file descriptor
int	m_nGpioNum
	gpio number
int	m_fdGpio
	gpio file descriptor
int	m_nGpioVal
	gpio shadow'ed value
libdxl::dxl	m_dxl
	dxl low-level interface
Protected Attributes inherited from DynaComm
char *	m_sDevUri
	dynamixel bus device URI
int	m_nBaudRate
	baud rate
bool	m_bIsOpen
	dynamixel bus communication is [not] open
uint_t	m_uBusStatus
	bus comminication status
uint_t	m_uAlarms
	servo alarms from last I/O operation
shm_mutex_t	m_mutexComm
	synchonization mutex

Additional Inherited Members
Static Public Member Functions inherited from DynaComm
static DynaComm *	New (const char *sUri, int nBaudRate)
	Archetype constructor to create a new Dynamixel bus communication derived instance. More...
static std::string	GetAlarmsString (const uint_t uAlarms, const std::string &strSep="; ")
	Get a formatted servo alarms string associated with the alarms. More...
static std::string	GetAlarmsShortString (const uint_t uAlarms, const std::string &strSep=",")
	Get a formatted servo alarms short string associated with the alarms. More...
static const char *	GetBusStatusString (uint_t uBusStatus)
	Get the string describing the Dynamixel servo communication status. More...
static int	BaudRateToNum (int nBaudRate)
	Map baud rate to Dynamixel baud number. More...
static int	BaudNumToRate (int nBaudRate)
	Map baud number to Dynamixel baud rate. More...
static int	BaudRateAt (int nIndex)
	Get the baud rate associated with the given index. More...
static int	BaudNumAt (int nIndex)
	Get the baud number associated with the given index. More...
Static Protected Attributes inherited from DynaComm
static const key_t	ShmKey = 0x70add12a
	shared memory key More...

Detailed Description

Dynamixel Serial Bus Communications Class.

Definition at line 74 of file DynaCommSerial.h.

Member Enumeration Documentation

enum DynaCommSerial::CtlSignal

Half-duplex control signals.

Note

Keep out of any potential non-modem signal space (e.g. GPIO 123, etc).

Enumerator
CtlSignalMinGPIO	gpio signal is inverted (tx = low, rx = high)
CtlSignalMaxGPIO	gpio signal (tx = high, rx = low)
CtlSignalModemRTS	request to send
CtlSignalModemCTS	clear to send
CtlSignalCustom	custom interface (requires callbacks)
CtlSignalNone	not a signal, but a value to clear signal

Definition at line 86 of file DynaCommSerial.h.

  {
    // GPIO: [-999, 999]
    CtlSignalMinGPIO  = -999, ///< gpio signal is inverted (tx = low, rx = high)
    CtlSignalMaxGPIO  =  999, ///< gpio signal (tx = high, rx = low)

    // modem
    CtlSignalModemRTS = 1000, ///< request to send 
    CtlSignalModemCTS = 1001, ///< clear to send 

    // other
    CtlSignalCustom   = 1010, ///< custom interface (requires callbacks)
    CtlSignalNone     = 1011  ///< not a signal, but a value to clear signal
  };

Constructor & Destructor Documentation

DynaCommSerial::DynaCommSerial	(	const char *	sSerialDevName,
		int	nBaudRate
	)

Initialization constructor.

The given serial device is opened at the baud rate.

Parameters

sSerialDevName	Serial device name.
nBaudRate	Serial device baud rate.

Definition at line 89 of file DynaCommSerial.cxx.

References m_fd, m_fdGpio, m_nGpioNum, m_nGpioVal, m_sSerialDevName, newstr(), and Open().

                                                                        :
                      DynaComm(sSerialDevName, nBaudRate)
{
  m_sSerialDevName  = newstr(sSerialDevName);
  m_fd              = -1;
  m_nGpioNum        = 0;
  m_fdGpio          = -1;
  m_nGpioVal        = -1;

  Open();
}

Member Function Documentation

int DynaCommSerial::Close ( )

virtual

Close serial communication to dynamixel bus.

Implements DynaComm.

Definition at line 185 of file DynaCommSerial.cxx.

References libdxl::dxl::close(), DYNA_OK, DynaComm::m_bIsOpen, m_dxl, and m_fd.

Referenced by GetResourceId(), Open(), and ~DynaCommSerial().

{
  if( m_bIsOpen )
  {
    m_dxl.close();
    m_fd = -1;
    m_bIsOpen = false;
  }
  return DYNA_OK;
}

void DynaCommSerial::EnableRxCTS ( void *  pArg, size_t  uNumTxBytes  )

staticprotected

Enable receive via CTS signal.

Parameters

pArg	Pointer to this.
uNumTxBytes	Number of transmit bytes (being) transmitted.

Definition at line 598 of file DynaCommSerial.cxx.

References m_fd.

Referenced by SetHalfDuplexCtl().

{
  DynaCommSerial *pThis = (DynaCommSerial *)pArg;

  SerDevFIFOOutputDrain(pThis->m_fd);
  SerDevDeassertCTS(pThis->m_fd);
}

void DynaCommSerial::EnableRxGPIO ( void *  pArg, size_t  uNumTxBytes  )

staticprotected

Enable receive via GPIO signal.

Parameters

pArg	Pointer to this.
uNumTxBytes	Number of transmit bytes (being) transmitted.

Definition at line 638 of file DynaCommSerial.cxx.

References m_fdGpio, DynaComm::m_nBaudRate, m_nGpioNum, and m_nGpioVal.

Referenced by SetHalfDuplexCtl().

{
  static double TuneMargin = -30.0;   // sleep margin in usec

  DynaCommSerial *pThis   = (DynaCommSerial *)pArg;
  int             val;
  float           usecTx;

  if( pThis->m_fdGpio < 0 )
  {
    return;
  }

  val = pThis->m_nGpioNum < 0? 1: 0;

  // already in receive mode
  if( val == pThis->m_nGpioVal )
  {
    return;
  }

  // tcdrain() takes too long to return - disappointing. so can't use
  //SerDevFIFOOutputDrain(pThis->m_fd);

  //
  // Estimate time to transmit. Each byte = 10 bits with Start and Stop bits.
  //
  usecTx  = (double)(uNumTxBytes * 10)/(double)pThis->m_nBaudRate * 1000000.0 
                + TuneMargin;

  if( usecTx > 0.0 )
  {
    unsigned int usec = (unsigned int)usecTx;

    // wait
    usleep(usec);
  }

  // already at this value
  if( gpioQuickWrite(pThis->m_fdGpio, val) == OK )
  {
    pThis->m_nGpioVal = val;
  }
}

void DynaCommSerial::EnableRxRTS ( void *  pArg, size_t  uNumTxBytes  )

staticprotected

Enable receive via RTS signal.

Parameters

pArg	Pointer to this.
uNumTxBytes	Number of transmit bytes (being) transmitted.

Definition at line 578 of file DynaCommSerial.cxx.

References m_fd.

Referenced by SetHalfDuplexCtl().

{
  DynaCommSerial *pThis = (DynaCommSerial *)pArg;

  SerDevFIFOOutputDrain(pThis->m_fd);
  SerDevDeassertRTS(pThis->m_fd);
}

void DynaCommSerial::EnableTxCTS ( void *  pArg )

staticprotected

Enable transmit via CTS signal.

Parameters

pArg	Pointer to this.

Definition at line 591 of file DynaCommSerial.cxx.

References m_fd.

Referenced by SetHalfDuplexCtl().

{
  DynaCommSerial *pThis = (DynaCommSerial *)pArg;

  SerDevAssertCTS(pThis->m_fd);
}

void DynaCommSerial::EnableTxGPIO ( void *  pArg )

staticprotected

Enable transmit via GPIO signal.

Parameters

pArg	Pointer to this.

Definition at line 623 of file DynaCommSerial.cxx.

References m_fdGpio, m_nGpioNum, and m_nGpioVal.

Referenced by SetHalfDuplexCtl().

{
  DynaCommSerial *pThis = (DynaCommSerial *)pArg;
  int             val;

  if( pThis->m_fdGpio >= 0 )
  {
    val = pThis->m_nGpioNum < 0? 0: 1;
    if( gpioQuickWrite(pThis->m_fdGpio, val) == OK )
    {
      pThis->m_nGpioVal = val;
    }
  }
}

void DynaCommSerial::EnableTxRTS ( void *  pArg )

staticprotected

Enable transmit via RTS signal.

Parameters

pArg	Pointer to this.

Definition at line 571 of file DynaCommSerial.cxx.

References m_fd.

Referenced by SetHalfDuplexCtl().

{
  DynaCommSerial *pThis = (DynaCommSerial *)pArg;

  SerDevAssertRTS(pThis->m_fd);
}

virtual int DynaCommSerial::GetResourceId ( ) const

inlinevirtual

Get system-unique resource identifier.

Returns

Resource id.

Implements DynaComm.

Definition at line 136 of file DynaCommSerial.h.

References Close(), m_fd, Open(), Ping(), Read16(), Read8(), Reset(), SetBaudRate(), SetHalfDuplexCtl(), SyncWrite(), Write16(), and Write8().

  {
    return m_fd;
  }

const char* DynaCommSerial::GetSerialDeviceName ( ) const

inline

Get the Dynamixel Bus serial device name.

Returns

Returns name.

Definition at line 126 of file DynaCommSerial.h.

References m_sSerialDevName.

  {
    return m_sSerialDevName;
  }

void DynaCommSerial::InitGPIO ( int  nGpioNum )

protected

Intialize GPIO signalling.

Parameters

nGpioNum

GPIO number.

Definition at line 606 of file DynaCommSerial.cxx.

References m_fdGpio, m_nGpioNum, and m_nGpioVal.

Referenced by SetHalfDuplexCtl().

{
  if( m_fdGpio >= 0 )
  {
    gpioClose(m_fdGpio);
    m_fdGpio = -1;
  }

  m_nGpioNum = nGpioNum;
  m_nGpioVal = -1;

  if( (m_fdGpio = gpioOpen(abs(m_nGpioNum))) < 0 )
  {
    LOGERROR("GPIO %d: Failed to open.", abs(m_nGpioNum));
  }
}

int DynaCommSerial::MakeSerialIndex ( const char *  sSerialDevName )

protected

Make serial index from serial device name.

Examples:

/dev/ttyUSB0 0
COM3 3

Parameters

sSeriaDevName

Serial device name.

Returns

Returns serial index on success.

Definition at line 534 of file DynaCommSerial.cxx.

References DYNA_ECODE_BAD_DEV, and DYNA_ECODE_BAD_VAL.

Referenced by Open().

{
  int   i, j, index;

  if( (sSerialDevName == NULL) || (*sSerialDevName == 0) )
  {
    return -DYNA_ECODE_BAD_VAL;
  }

  if( access(sSerialDevName, F_OK) )
  {
    return -DYNA_ECODE_BAD_DEV;
  }

  for(i=j=(int)strlen(sSerialDevName)-1; i>=0; --i)
  {
    if( !isdigit(sSerialDevName[i]) )
    {
      break;
    }
  }

  if( (i < 0) || (i == j) )
  {
    return -DYNA_ECODE_BAD_VAL;
  }

  index = atoi(sSerialDevName+i+1);

  return index;
}

int DynaCommSerial::Open ( const char *  sSerialDevName, int  nBaudRate  )

virtual

Open serial communication to dynamixel bus.

The given serial device is opened at the given baud rate.

Parameters

sSerialDevName	Serial device name.
nBaudRate	Serial device baud rate.

Implements DynaComm.

Definition at line 111 of file DynaCommSerial.cxx.

References Close(), DynaComm::m_nBaudRate, m_sSerialDevName, newstr(), and Open().

{
  Close();

  if( m_sSerialDevName != NULL )
  {
    delete[] m_sSerialDevName;
  }

  m_sSerialDevName  = newstr(sSerialDevName);
  m_nBaudRate       = nBaudRate;

  return Open();
}

int DynaCommSerial::Open ( )

virtual

(Re)Open serial communication to dynamixel bus.

Implements DynaComm.

Definition at line 126 of file DynaCommSerial.cxx.

References DynaComm::BaudRateToNum(), Close(), libdxl::dxl::dxl_initialize(), DYNA_ECODE_BAD_DEV, DYNA_ECODE_BAD_VAL, DYNA_LOG_ERROR, DYNA_OK, libdxl::dxl::getFd(), DynaComm::m_bIsOpen, m_dxl, m_fd, DynaComm::m_nBaudRate, m_sSerialDevName, MakeSerialIndex(), and libdxl::dxl::open().

Referenced by DynaCommSerial(), GetResourceId(), and Open().

{
#if 0 // old dxl library
  int nSerialIndex;
  int nBaudNum;
#endif
  int rc;

  Close();

  if( (m_sSerialDevName == NULL) || (*m_sSerialDevName == 0) )
  {
    rc = -DYNA_ECODE_BAD_VAL;
    DYNA_LOG_ERROR(rc, "Unspecified serial device.");
  }

#if 0 // old dxl library
  // make serial index from serial device
  else if( (nSerialIndex = MakeSerialIndex(m_sSerialDevName)) < 0 )
  {
    DYNA_LOG_ERROR(nSerialIndex, "Bad serial device name: \"%s\".",
        m_sSerialDevName);
    rc = nSerialIndex;
  }

  // map baudrate to internal baud enumeration value
  else if( (nBaudNum = BaudRateToNum(m_nBaudRate)) < 0 )
  {
    DYNA_LOG_ERROR(nBaudNum, "%d: Unsupported baudrate.", m_nBaudRate);
    rc = nBaudNum;
  }

  else if( m_dxl.dxl_initialize(nSerialIndex, nBaudNum) == 0 ) 
  {
    rc = -DYNA_ECODE_BAD_DEV;
    DYNA_LOG_ERROR(rc, "Serial device \"%s\": Failed to initalize interface.",
        m_sSerialDevName);
  }
#endif

  else if( m_dxl.open(m_sSerialDevName, m_nBaudRate) == 0 ) 
  {
    rc = -DYNA_ECODE_BAD_DEV;
    DYNA_LOG_ERROR(rc, "Serial device \"%s\": Failed to initalize interface.",
        m_sSerialDevName);
  }

  else
  {
    m_bIsOpen = true;
    m_fd = m_dxl.getFd();
    rc = DYNA_OK;
    LOGDIAG3("Dynamixel bus communication opened on \"%s\"@%d.",
        m_sSerialDevName, m_nBaudRate);
  }

  return rc;
}

bool DynaCommSerial::Ping ( int  nServoId )

virtual

Ping the servo.

Parameters

nServoId

Servo id.

Returns

Returns true if the servo responded, else false.

Implements DynaComm.

Definition at line 487 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DYNA_TRY_COMM, libdxl::dxl::getResult(), m_dxl, DynaComm::m_mutexComm, and libdxl::dxl::ping().

Referenced by GetResourceId().

{
  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  m_dxl.ping(nServoId);

  shm_mutex_unlock(&m_mutexComm);

  return m_dxl.getResult() == DXL_COMM_RXSUCCESS? true: false;
}

int DynaCommSerial::Read16 ( int  nServoId, uint_t  uAddr, ushort_t *  pVal  )

virtual

Read a 16-bit value from Dynamixel servo control table.

Parameters

	nServoId	Servo id.
	uAddr	Servo control table address.
[out]	pVal	Value read.

Implements DynaComm.

Definition at line 338 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DYNA_ECODE_RX_TIMEOUT, DYNA_LOG_ERROR, DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getResult(), libdxl::dxl::getRxPacketErrBits(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uAlarms, DynaComm::m_uBusStatus, libdxl::dxl::readWord(), RetryMax, and RetryWait.

Referenced by GetResourceId().

{
  int   nTries = 0;
  int   rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  do
  {
    *pVal = (ushort_t)m_dxl.readWord(nServoId, (int)uAddr);

    m_uBusStatus = (uint_t)m_dxl.getResult();

    if( m_uBusStatus == DXL_COMM_RXSUCCESS )
    {
      m_uAlarms = (uint_t)m_dxl.getRxPacketErrBits();
      DYNA_LOG_SERVO_ALARMS(nServoId, m_uAlarms);
      rc = DYNA_OK;
    }
    else
    {
      rc = -DynaMapDxlToEcode(m_uBusStatus);
      usleep(RetryWait);
    }
  } while( (rc == -DYNA_ECODE_RX_TIMEOUT) && (++nTries < RetryMax) );

  shm_mutex_unlock(&m_mutexComm);

  if (rc < 0 )
  {
    DYNA_LOG_ERROR(rc, "Read16(%d, 0x%02x)", nServoId, uAddr);
  }

  return rc;
}

int DynaCommSerial::Read8 ( int  nServoId, uint_t  uAddr, byte_t *  pVal  )

virtual

Read an 8-bit value from Dynamixel servo control table.

Parameters

	nServoId	Servo id.
	uAddr	Servo control table address.
[out]	pVal	Value read.

Implements DynaComm.

Definition at line 262 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DYNA_ECODE_RX_TIMEOUT, DYNA_LOG_ERROR, DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getResult(), libdxl::dxl::getRxPacketErrBits(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uAlarms, DynaComm::m_uBusStatus, libdxl::dxl::readByte(), RetryMax, and RetryWait.

Referenced by GetResourceId().

{
  int   nTries = 0;
  int   rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  do
  {
    *pVal = (byte_t)m_dxl.readByte(nServoId, (int)uAddr);

    m_uBusStatus = (uint_t)m_dxl.getResult();
   
    if( m_uBusStatus == DXL_COMM_RXSUCCESS )
    {
      m_uAlarms = (uint_t)m_dxl.getRxPacketErrBits();
      DYNA_LOG_SERVO_ALARMS(nServoId, m_uAlarms);
      rc = DYNA_OK;
    }
    else
    {
      rc = -DynaMapDxlToEcode(m_uBusStatus);
      usleep(RetryWait);
    }
  } while( (rc == -DYNA_ECODE_RX_TIMEOUT) && (++nTries < RetryMax) );

  shm_mutex_unlock(&m_mutexComm);

  if (rc < 0 )
  {
    DYNA_LOG_ERROR(rc, "Read8(%d, 0x%02x)", nServoId, uAddr);
  }

  return rc;
}

int DynaCommSerial::Reset ( int  nServoId )

virtual

Reset a servo back to default values.

Warning

All configuration is lost.

Parameters

nServoId

Servo id.

Implements DynaComm.

Definition at line 500 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DXL_INST_RESET, DYNA_LOG_ERROR, DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getResult(), libdxl::dxl::getRxPacketErrBits(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uAlarms, DynaComm::m_uBusStatus, libdxl::dxl::setTxPacketId(), libdxl::dxl::setTxPacketInstruction(), libdxl::dxl::setTxPacketLength(), and libdxl::dxl::txrxPacket().

Referenced by GetResourceId().

{
  int   rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  // make reset packet
  m_dxl.setTxPacketId(nServoId);
  m_dxl.setTxPacketInstruction(DXL_INST_RESET);
  m_dxl.setTxPacketLength(2);

  m_dxl.txrxPacket();
  
  m_uBusStatus = (uint_t)m_dxl.getResult();

  if( m_uBusStatus == DXL_COMM_RXSUCCESS )
  {
    m_uAlarms = (uint_t)m_dxl.getRxPacketErrBits();
    DYNA_LOG_SERVO_ALARMS(nServoId, m_uAlarms);
    rc = DYNA_OK;
  }
  else
  {
    rc = -DynaMapDxlToEcode(m_uBusStatus);
    DYNA_LOG_ERROR(rc, "Reset(%d)", nServoId);
  }

  shm_mutex_unlock(&m_mutexComm);

  return rc;
}

int DynaCommSerial::SetBaudRate ( int  nNewBaudRate )

virtual

Set the Dynamixel Bus new baud rate.

Parameters

nNewBaudRate

New baud rate.

Implements DynaComm.

Definition at line 196 of file DynaCommSerial.cxx.

References DynaComm::BaudRateToNum(), DYNA_ECODE_SYS, DYNA_LOG_ERROR, DYNA_LOG_SYS_ERROR, DYNA_OK, DynaComm::m_bIsOpen, m_dxl, DynaComm::m_nBaudRate, m_sSerialDevName, and libdxl::dxl::setBaudRate().

Referenced by GetResourceId().

{
  int   rc;   // return code

  // check baud rate against supported
  if( (rc = BaudRateToNum(nNewBaudRate)) < 0 )
  {
    DYNA_LOG_ERROR(rc, "%d: Unsupported baudrate.", nNewBaudRate);
    return rc;
  }
  
  // already open, adjust serial parameters
  if( m_bIsOpen )
  {
    if( !m_dxl.setBaudRate(nNewBaudRate) )
    {
      rc = -DYNA_ECODE_SYS;
      DYNA_LOG_SYS_ERROR(rc,
          "Serial device \"%s\": Failed to set baud rate %d.",
          m_sSerialDevName, nNewBaudRate);
      return rc;
    }
  }

  m_nBaudRate = nNewBaudRate;

  return DYNA_OK;
}

int DynaCommSerial::SetHalfDuplexCtl ( int  nSignal, DynaComm::HalfDuplexTxFunc_T  fnEnableTx = NULL, DynaComm::HalfDuplexRxFunc_T  fnEnableRx = NULL  )

virtual

Set Dynamixel Bus half-duplex software control.

The Dynamixel 3-wire bus is half-duplex. Hardware may automatically control toggling between transmit and receive (e.g. RoadNarrows DynaUSB dongle). If there is no hardware support, then software must provide the tx/rx toggle functions.

Parameters

nSignal	Signal assign to toggle.
fnEnableTx	Enable transmit function.
fnEnableRx	Enable receive function.

Implements DynaComm.

Definition at line 225 of file DynaCommSerial.cxx.

References CtlSignalCustom, CtlSignalModemCTS, CtlSignalModemRTS, CtlSignalNone, DYNA_OK, EnableRxCTS(), EnableRxGPIO(), EnableRxRTS(), EnableTxCTS(), EnableTxGPIO(), EnableTxRTS(), InitCTS(), InitGPIO(), InitRTS(), m_dxl, m_fd, and libdxl::dxl::setHalfDuplexCallbacks().

Referenced by GetResourceId().

{
  switch( nSignal )
  {
    case CtlSignalModemRTS: // serial request to send
      InitRTS();
      SerDevSetHwFlowControl(m_fd, true);
      m_dxl.setHalfDuplexCallbacks(DynaCommSerial::EnableTxRTS,
                                   DynaCommSerial::EnableRxRTS,
                                   this);
      break;
    case CtlSignalModemCTS: // serial clear to send
      InitCTS();
      SerDevSetHwFlowControl(m_fd, true);
      m_dxl.setHalfDuplexCallbacks(DynaCommSerial::EnableTxCTS,
                                   DynaCommSerial::EnableRxCTS,
                                   this);
      break;
    case CtlSignalNone:     // clear callbacks
      m_dxl.setHalfDuplexCallbacks(NULL, NULL, NULL);
      break;
    case CtlSignalCustom:   // custom signal
      m_dxl.setHalfDuplexCallbacks(fnEnableTx, fnEnableRx, this);
      break;
    default:                // gpio signal
      InitGPIO(nSignal);
      m_dxl.setHalfDuplexCallbacks(DynaCommSerial::EnableTxGPIO,
                                   DynaCommSerial::EnableRxGPIO,
                                   this);
      break;
  }

  return DYNA_OK;
}

int DynaCommSerial::SyncWrite ( uint_t  uAddr, uint_t  uValSize, DynaSyncWriteTuple_T  tuples[], uint_t  uCount  )

virtual

Synchronous Write 8/16-bit values to a list of Dynamixel servos.

Parameters

uAddr	Servo control table write address.
uValSize	Value storage size at addtess. 1 or 2 bytes.
tuples	Array of servo id, write value 2-tuples.
uCount	Number of tuples.

Implements DynaComm.

Definition at line 414 of file DynaCommSerial.cxx.

References DXL_BROADCAST_ID, DXL_COMM_RXSUCCESS, DXL_INST_SYNC_WRITE, DYNA_ECODE_RX_TIMEOUT, DYNA_LOG_ERROR, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getHighByte(), libdxl::dxl::getLowByte(), libdxl::dxl::getResult(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uBusStatus, DynaSyncWriteTuple_T::m_uVal, RetryMax, RetryWait, libdxl::dxl::setTxPacketId(), libdxl::dxl::setTxPacketInstruction(), libdxl::dxl::setTxPacketLength(), libdxl::dxl::setTxPacketParameter(), and libdxl::dxl::txrxPacket().

Referenced by GetResourceId().

{
  int i;
  int j;
  int nTries = 0;
  int rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  //
  // packet header
  //
  j = 0;
  m_dxl.setTxPacketId(DXL_BROADCAST_ID);              // broadcast id
  m_dxl.setTxPacketInstruction(DXL_INST_SYNC_WRITE);  // instruction
  m_dxl.setTxPacketParameter(j++, (int)uAddr);        // write address
  m_dxl.setTxPacketParameter(j++, (int)uValSize);     // value size at address

  //
  // servo_id, val_lsb[, val_msb]
  //
  for(i=0; i<(int)uCount; ++i)
  {
    m_dxl.setTxPacketParameter(j++, tuples[i].m_nServoId);

    if( uValSize == 1 )
    {
      m_dxl.setTxPacketParameter(j++, m_dxl.getLowByte((int)tuples[i].m_uVal));
    }
    else
    {
      m_dxl.setTxPacketParameter(j++, m_dxl.getLowByte((int)tuples[i].m_uVal));
      m_dxl.setTxPacketParameter(j++, m_dxl.getHighByte((int)tuples[i].m_uVal));
    }
  }

  // packet length in header
  m_dxl.setTxPacketLength(j+2);

  do
  {
    // send sync packet
    m_dxl.txrxPacket();
  
    m_uBusStatus = (uint_t)m_dxl.getResult();

    if( m_uBusStatus == DXL_COMM_RXSUCCESS )
    {
      rc = DYNA_OK;
    }
    else
    {
      rc = -DynaMapDxlToEcode(m_uBusStatus);
      usleep(RetryWait);
    }
  } while( (rc == -DYNA_ECODE_RX_TIMEOUT) && (++nTries < RetryMax) );

  shm_mutex_unlock(&m_mutexComm);

  if (rc < 0 )
  {
    DYNA_LOG_ERROR(rc, "SyncWrite(0x%02x, %d, tuples, %d)",
        uAddr, uValSize, uCount);
  }

  return rc;
}

int DynaCommSerial::Write16 ( int  nServoId, uint_t  uAddr, ushort_t  uhVal  )

virtual

Write a 16-bit value to Dynamixel servo control table.

Parameters

nServoId	Servo id.
uAddr	Servo control table address.
uhVal	Value written.

Implements DynaComm.

Definition at line 376 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DYNA_ECODE_RX_TIMEOUT, DYNA_LOG_ERROR, DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getResult(), libdxl::dxl::getRxPacketErrBits(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uAlarms, DynaComm::m_uBusStatus, RetryMax, RetryWait, and libdxl::dxl::writeWord().

Referenced by GetResourceId().

{
  int   nTries = 0;
  int   rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  do
  {
    m_dxl.writeWord(nServoId, (int)uAddr, (int)uhVal);

    m_uBusStatus = (uint_t)m_dxl.getResult();

    if( m_uBusStatus == DXL_COMM_RXSUCCESS )
    {
      m_uAlarms = (uint_t)m_dxl.getRxPacketErrBits();
      DYNA_LOG_SERVO_ALARMS(nServoId, m_uAlarms);
      rc = DYNA_OK;
    }
    else
    {
      rc = -DynaMapDxlToEcode(m_uBusStatus);
      usleep(RetryWait);
    }
  } while( (rc == -DYNA_ECODE_RX_TIMEOUT) && (++nTries < RetryMax) );

  shm_mutex_unlock(&m_mutexComm);

  if (rc < 0 )
  {
    DYNA_LOG_ERROR(rc, "Write16(%d, 0x%02x, 0x%04x)", nServoId, uAddr, uhVal);
  }

  return rc;
}

int DynaCommSerial::Write8 ( int  nServoId, uint_t  uAddr, byte_t  byVal  )

virtual

Write an 8-bit value to Dynamixel servo control table.

Parameters

nServoId	Servo id.
uAddr	Servo control table address.
byVal	Value written.

Implements DynaComm.

Definition at line 300 of file DynaCommSerial.cxx.

References DXL_COMM_RXSUCCESS, DYNA_ECODE_RX_TIMEOUT, DYNA_LOG_ERROR, DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaMapDxlToEcode(), libdxl::dxl::getResult(), libdxl::dxl::getRxPacketErrBits(), m_dxl, DynaComm::m_mutexComm, DynaComm::m_uAlarms, DynaComm::m_uBusStatus, RetryMax, RetryWait, and libdxl::dxl::writeByte().

Referenced by GetResourceId().

{
  int   nTries = 0;
  int   rc;

  DYNA_TRY_COMM(*this);

  shm_mutex_lock(&m_mutexComm);

  do
  {
    m_dxl.writeByte(nServoId, (int)uAddr, (int)byVal);

    m_uBusStatus = (uint_t)m_dxl.getResult();
   
    if( m_uBusStatus == DXL_COMM_RXSUCCESS )
    {
      m_uAlarms = (uint_t)m_dxl.getRxPacketErrBits();
      DYNA_LOG_SERVO_ALARMS(nServoId, m_uAlarms);
      rc = DYNA_OK;
    }
    else
    {
      rc = -DynaMapDxlToEcode(m_uBusStatus);
      usleep(RetryWait);
    }
  } while( (rc == -DYNA_ECODE_RX_TIMEOUT) && (++nTries < RetryMax) );

  shm_mutex_unlock(&m_mutexComm);

  if (rc < 0 )
  {
    DYNA_LOG_ERROR(rc, "Write8(%d, 0x%02x, 0x%02x)", nServoId, uAddr, byVal);
  }

  return rc;
}

---

The documentation for this class was generated from the following files:

• include/Dynamixel/DynaCommSerial.h
• libDynamixel/DynaCommSerial.cxx