DynaChain Class Reference

Dynamixel Chain Container Base Class. More...

#include <DynaChain.h>

Public Member Functions
	DynaChain (DynaComm &comm)
	Default initialization constructor. More...
virtual	~DynaChain ()
	Destructor.
virtual bool	HasServo (int nServoId) const
virtual DynaServo *	GetServo (int nServoId)
virtual uint_t	GetNumberInChain () const
	Get the number of servos currently in chain. More...
virtual int	LinkServos (int nServoIdMaster, int nServoIdSlave, bool bIsReversed)
	Software link two unlinked servos in a master-slave configuration. More...
virtual int	UnlinkServos (int nServoIdMaster)
	Unlink two software linked servos. More...
virtual bool	IsMaster (int nServoId) const
	Test if the servo is a master. More...
bool	IsLinkedMaster (int nServoId) const
	Test if this servo is a linked master. More...
bool	IsLinkedSlave (int nServoId) const
	Test if this servo is a linked slave. More...
bool	IsUnlinked (int nServoId) const
	Test if this servo is unlinked. More...
int	GetLinkedMateId (int nServoId) const
	Get linked mate. More...
virtual uint_t	GetNumberOfMastersInChain ()
	Get the number of servos currently in chain. More...
virtual int	AddNewServo (int nServoId)
	Create a new servo and add it to the Dynamixel chain. More...
virtual int	AddNewServosByScan ()
	Scan and add all discovered and created servos to the Dynamixel chain. More...
virtual int	AddNewServoForced (int nServoId, uint_t uModelNum)
	Force create and add a servo to the Dynamixel chain. More...
virtual int	RemoveServo (int nServoId)
	Remove from chain and delete. More...
virtual int	RemoveAllServos ()
	Remove all servos from chain and delete. More...
virtual int	MoveTo (int nServoId, int nGoalOdPos)
	Move to the goal postition. More...
virtual int	vSyncMoveTo (uint_t uCount,...)
	Synchronous move servos to new goal positions. More...
virtual int	SyncMoveTo (DynaPosTuple_T tuplesPos[], uint_t uCount)
	Synchronous move servos to new goal positions in tuple list. More...
virtual int	MoveAtSpeedTo (int nServoId, int nGoalSpeed, int nGoalOdPos)
	Move at speed to the goal postition. More...
virtual int	vSyncMoveAtSpeedTo (uint_t uCount,...)
	Synchronous move servos to new goal positions at the given speeds. More...
virtual int	SyncMoveAtSpeedTo (DynaSpeedPosTuple_T tuplesSpeedPos[], uint_t uCount)
	Synchronous move servos to new goal positions at the given speeds. More...
virtual int	MoveAtSpeed (int nServoId, int nGoalSpeed)
	Move at speed. More...
virtual int	vSyncMoveAtSpeed (uint_t uCount,...)
	Synchronous move servos at the given speeds. More...
virtual int	SyncMoveAtSpeed (DynaSpeedTuple_T tuplesSpeed[], uint_t uCount)
	Synchronous move servos at the given speeds. More...
virtual int	EStop ()
	Emergency stop all servos. More...
virtual int	Stop ()
	Stop all servos. More...
virtual int	Freeze ()
	Freeze all servos at current position. More...
virtual int	Release ()
	Release all servos from any applied torque. More...
virtual int	SyncWriteTorqueEnable (bool bState)
	Synchronous write to all of the servos in the chain to enable/disable applied torque. More...
virtual int	vSyncWriteGoalPos (uint_t uCount,...)
	Synchronous write new goal positions for servos. More...
virtual int	SyncWriteGoalPos (DynaPosTuple_T tuplesPos[], uint_t uCount)
	Synchronous write new goal positions for servos. More...
virtual int	vSyncWriteGoalSpeed (uint_t uCount,...)
	Synchronous write new goal speed for for servos. More...
virtual int	SyncWriteGoalSpeed (DynaSpeedTuple_T tuplesSpeed[], uint_t uCount)
	Synchronous write new goal speed for servos. More...
virtual int	IterStart (int *pIter)
	Start iteration over of entire servo chain. More...
virtual int	IterNext (int *pIter)
	Next iteration over of entire servo chain. More...
virtual int	IterStartMaster (int *pIter)
	Start iteration master servos over of entire servo chain. More...
virtual int	IterNextMaster (int *pIter)
	Next iteration of master servos over of entire servo chain. More...
const DynaServo *	operator[] (int nServoId)
	Subscript operator. More...

Protected Member Functions
void	Init ()
	Initialize chain class instance.
virtual int	ChainEntryNew (int nServoId)
	Create new dervied DyanServo object and insert into chain. More...
virtual int	ChainEntryDelete (int nServoId)
	Remove servo from chain and delete. More...
virtual void	AuditLinks ()
	Audit servo links and repair or delete. More...
virtual void	SetLinkData (int nServoId, uint_t uLinkType, int nServoIdMate, bool bRotReversed)
	Set local link data for the given servo. More...
virtual void	ClearLinkData (int nServoId)
	Clear local link data for the given servo.

Protected Attributes
DynaComm &	m_comm
	bus communication instance
DynaServo *	m_pChain [DYNA_ID_NUMOF]
	chain of servos
int	m_nIIdx [DYNA_ID_NUMOF]
	indirect index
DynaServoLink_T	m_links [DYNA_ID_NUMOF]
	servo linked info
uint_t	m_uNumInChain
	number of servos in chain
uint_t	m_uNumMastersInChain
	num of master servos in chain

Detailed Description

Dynamixel Chain Container Base Class.

The DynaChain base class provides a coherent and synchronized interface to a set of servos communicating on the same Dynamixel bus.

Definition at line 75 of file DynaChain.h.

Constructor & Destructor Documentation

DynaChain::DynaChain

(

DynaComm &

comm

)

Default initialization constructor.

The chain is bound to the given Dynamixel bus communication object. No servos are present in the chain until explicitly added.

A background thread may be automatically started (default) to:

• control the positioning of servos configured in continuous mode.
• monitor the dynamics of the servos in the chain.
• monitor the health of the servos in the chain.

It is recommended that if any of the servos in the chain are in continuous (wheel) mode and have 360 ° positioning data, that the background thread be started.

Parameters

comm	Dynamixel bus communication instance.

Definition at line 82 of file DynaChain.cxx.

References Init().

                                   : m_comm(comm)
{
  Init();
}

Member Function Documentation

int DynaChain::AddNewServo ( int  nServoId )

virtual

Create a new servo and add it to the Dynamixel chain.

Communication with servo is checked prior to adding the servo.

Note

Any existing servo with the same servo id will be replaced by this servo.
Any existing linked servo configuration will be preserved if possible.

Parameters

nServoId

Servo id to add.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 206 of file DynaChain.cxx.

References AuditLinks(), ChainEntryNew(), DYNA_ECODE_NO_SERVO, DYNA_LOG_ERROR, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_SERVO_ID, and m_comm.

Referenced by GetLinkedMateId().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_ID(nServoId);

  // Ping the servo
  if( !DynaServo::Ping(m_comm, nServoId) )
  {
    rc = -DYNA_ECODE_NO_SERVO;
    DYNA_LOG_ERROR(rc, "Pinging servo %d failed. Cannot add.", nServoId);
  }

  // create and insert chain entry
  else if( (rc = ChainEntryNew(nServoId)) == DYNA_OK )
  {
    AuditLinks();
  }

  return rc;
}

int DynaChain::AddNewServoForced ( int  nServoId, uint_t  uModelNum  )

virtual

Force create and add a servo to the Dynamixel chain.

No communication with servo is attempted, nor any data validation performed.

Note

Any existing servo with the same servo id will be replaced by this servo.
Any existing linked servo configuration will be preserved if possible.

Parameters

nServoId	Servo id to add.
uModelNum	Servo model number.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Todo:

TODO force add servo to chain

Definition at line 259 of file DynaChain.cxx.

References DYNA_ECODE_NOT_SUPP.

Referenced by GetLinkedMateId().

{
  /*! \todo TODO force add servo to chain */
  return -DYNA_ECODE_NOT_SUPP;
}

int DynaChain::AddNewServosByScan ( )

virtual

Scan and add all discovered and created servos to the Dynamixel chain.

Note

All existing servos in the chain are removed and deleted prior to the scan.
Any existing linked servo configuration will be preserved if possible.

Returns

Returns the number of servos added.

Definition at line 229 of file DynaChain.cxx.

References AuditLinks(), ChainEntryDelete(), ChainEntryNew(), DYNA_ID_MIN, DYNA_ID_NUMOF, DYNA_OK, DYNA_TRY_COMM, and m_comm.

Referenced by DynaShellCmdScan::Exec(), and GetLinkedMateId().

{
  int     nServoId;
  int     nNumAdded;

  DYNA_TRY_COMM(m_comm);

  // remove and delete all servos
  for(nServoId=DYNA_ID_MIN; nServoId<DYNA_ID_NUMOF; ++nServoId)
  {
    ChainEntryDelete(nServoId);
  }

  // add all by scan
  for(nServoId=DYNA_ID_MIN, nNumAdded=0; nServoId<DYNA_ID_NUMOF; ++nServoId)
  {
    if( DynaServo::Ping(m_comm, nServoId) )
    {
      if( ChainEntryNew(nServoId) == DYNA_OK )
      {
        ++nNumAdded;
      }
    }
  }

  AuditLinks();

  return nNumAdded;
}

void DynaChain::AuditLinks ( )

protectedvirtual

Audit servo links and repair or delete.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 388 of file DynaChain.cxx.

References ClearLinkData(), DYNA_ID_MAX, DYNA_ID_MIN, DYNA_ID_NUMOF, DYNA_LINK_MASTER, DYNA_LINK_SLAVE, DynaServo::GetModelNumber(), GetNumberOfMastersInChain(), DynaServo::Link(), DynaServoLink_T::m_bRotReversed, m_links, DynaServoLink_T::m_nServoIdMate, m_pChain, DynaServoLink_T::m_uLinkType, SetLinkData(), and DynaServo::Unlink().

Referenced by AddNewServo(), AddNewServosByScan(), RemoveAllServos(), and RemoveServo().

{
  int         nServoIdThis;
  uint_t      uLinkType;
  int         nServoIdMate;
  bool        bRotReversed;
  DynaServo  *pServoThis;
  DynaServo  *pServoMate;

  for(nServoIdThis=DYNA_ID_MIN; nServoIdThis<DYNA_ID_NUMOF; ++nServoIdThis)
  {
    uLinkType     = m_links[nServoIdThis].m_uLinkType;
    nServoIdMate  = m_links[nServoIdThis].m_nServoIdMate;
    bRotReversed  = m_links[nServoIdThis].m_bRotReversed;
    pServoThis    = m_pChain[nServoIdThis];

    // this servo is not present in the chain
    if( pServoThis == NULL )
    {
      ClearLinkData(nServoIdThis);
    }

    // this link is either the master or slave servo
    else if( (uLinkType == DYNA_LINK_MASTER) || (uLinkType == DYNA_LINK_SLAVE) )
    {
      // no linked mate
      if( (nServoIdMate < DYNA_ID_MIN) ||
          (nServoIdMate > DYNA_ID_MAX) ||
          ((pServoMate = m_pChain[nServoIdMate]) == NULL) )
      {
        pServoThis->Unlink();
        ClearLinkData(nServoIdThis);
      }

      // linked servos mismatch
      else if(pServoThis->GetModelNumber() != pServoMate->GetModelNumber())
      {
        pServoThis->Unlink();
        pServoMate->Unlink();
        ClearLinkData(nServoIdThis);
      }

      // this link is the master servo
      else if( uLinkType == DYNA_LINK_MASTER )
      {
        pServoThis->Link(DYNA_LINK_MASTER, pServoMate, bRotReversed);
        pServoMate->Link(DYNA_LINK_SLAVE,  pServoThis, bRotReversed);
        SetLinkData(nServoIdMate, DYNA_LINK_SLAVE, nServoIdThis, bRotReversed);
      }

      // this link is the slave servo
      else
      {
        pServoThis->Link(DYNA_LINK_SLAVE,  pServoMate, bRotReversed);
        pServoMate->Link(DYNA_LINK_MASTER, pServoThis, bRotReversed);
        SetLinkData(nServoIdMate, DYNA_LINK_MASTER, nServoIdThis, bRotReversed);
      }
    }

    // this servo has no link 
    else // DYNA_LINK_NONE
    {
      pServoThis->Unlink();
      ClearLinkData(nServoIdThis);
    }
  }

  GetNumberOfMastersInChain();
}

int DynaChain::ChainEntryDelete ( int  nServoId )

protectedvirtual

Remove servo from chain and delete.

Parameters

nServoId

Servo Id to delete.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 346 of file DynaChain.cxx.

References DYNA_ID_NONE, DYNA_OK, DYNA_TRY_SERVO_ID, DynaServo::GetModelName(), DynaServo::GetModelNumber(), m_nIIdx, m_pChain, and m_uNumInChain.

Referenced by AddNewServosByScan(), ChainEntryNew(), RemoveAllServos(), and RemoveServo().

{
  DynaServo *pServo;
  uint_t     i, j;

  DYNA_TRY_SERVO_ID(nServoId);

  // okay not to be in chain
  if( (pServo = m_pChain[nServoId]) == NULL )
  {
    return DYNA_OK;
  }

  // remove from indirect index list
  for(i=0; i<m_uNumInChain; ++i)
  {
    if( m_nIIdx[i] == nServoId )
    {
      m_nIIdx[i] = DYNA_ID_NONE;
      m_uNumInChain--;
      break;
    }
  }

  // compress indirect index list
  for(j=i; j<m_uNumInChain; ++j)
  {
    m_nIIdx[j] = m_nIIdx[j+1];
    m_nIIdx[j+1] = DYNA_ID_NONE;
  }

  LOGDIAG3("%s servo %d (model number 0x%04x) removed from chain.",
      pServo->GetModelName(), nServoId, pServo->GetModelNumber());

  // delete from chain
  delete m_pChain[nServoId];

  m_pChain[nServoId] = NULL;

  return DYNA_OK;
}

int DynaChain::ChainEntryNew ( int  nServoId )

protectedvirtual

Create new dervied DyanServo object and insert into chain.

Parameters

nServoId

Servo Id to delete.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 312 of file DynaChain.cxx.

References ChainEntryDelete(), DYNA_ECODE_NO_SERVO, DYNA_LOG_ERROR, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_SERVO_ID, DynaServo::GetModelName(), DynaServo::GetModelNumber(), m_comm, m_nIIdx, m_pChain, m_uNumInChain, and DynaServo::New().

Referenced by AddNewServo(), and AddNewServosByScan().

{
  DynaServo  *pServo;
  int         rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_ID(nServoId);

  // create derived servo object 
  pServo = DynaServo::New(m_comm, nServoId);

  if( pServo == NULL )
  {
    rc = -DYNA_ECODE_NO_SERVO;
    DYNA_LOG_ERROR(rc, "Failed to create servo %d. Cannot add.", nServoId);
    return rc;
  }

  // remove any existing servo with the same id
  if( m_pChain[nServoId] != NULL )
  {
    ChainEntryDelete(nServoId);
  }

  // insert into chain
  m_pChain[nServoId] = pServo;
  m_nIIdx[m_uNumInChain++] = nServoId;

  LOGDIAG3("%s servo %d (model number 0x%04x) added to chain.",
      pServo->GetModelName(), nServoId, pServo->GetModelNumber());

  return DYNA_OK;
}

int DynaChain::EStop ( )

virtual

Emergency stop all servos.

All torque is removed.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 714 of file DynaChain.cxx.

References DYNA_OK, DYNA_TRY_COMM, m_comm, and SyncWriteTorqueEnable().

Referenced by DynaShellCmdEStop::Exec(), and GetLinkedMateId().

{
  int nMaxTries = 3;
  int nTries;
  int rc;

  DYNA_TRY_COMM(m_comm);

  for(nTries=0; nTries<nMaxTries; ++nTries)
  {
    if( (rc = SyncWriteTorqueEnable(false)) == DYNA_OK )
    {
      break;
    }
  }

  return rc;
}

int DynaChain::Freeze ( )

virtual

Freeze all servos at current position.

Torque is applied.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 751 of file DynaChain.cxx.

References DYNA_OK, DYNA_TRY_COMM, m_comm, Stop(), and SyncWriteTorqueEnable().

Referenced by DynaShellCmdFreeze::Exec(), GetLinkedMateId(), and DynaShellCmdPlay::SecureArm().

{
  int         rc_tmp;     // temporary return code
  int         rc;         // return code

  DYNA_TRY_COMM(m_comm);

  rc = SyncWriteTorqueEnable(true);

  rc_tmp = Stop();

  if( rc == DYNA_OK )
  {
    rc = rc_tmp;
  }

  return rc;
}

int DynaChain::GetLinkedMateId ( int  nServoId ) const

inline

Get linked mate.

Parameters

nServoId

Servo id.

Returns

If the servo is linked, returns its linked mate's servo id.
Else returns DYNA_ID_NONE.

Definition at line 238 of file DynaChain.h.

References AddNewServo(), AddNewServoForced(), AddNewServosByScan(), DYNA_ID_MAX, DYNA_ID_MIN, DYNA_ID_NONE, EStop(), Freeze(), GetNumberOfMastersInChain(), IterNext(), IterNextMaster(), IterStart(), IterStartMaster(), m_links, DynaServoLink_T::m_nServoIdMate, MoveAtSpeed(), MoveAtSpeedTo(), MoveTo(), operator[](), Release(), RemoveAllServos(), RemoveServo(), Stop(), SyncMoveAtSpeed(), SyncMoveAtSpeedTo(), SyncMoveTo(), SyncWriteGoalPos(), SyncWriteGoalSpeed(), SyncWriteTorqueEnable(), vSyncMoveAtSpeed(), vSyncMoveAtSpeedTo(), vSyncMoveTo(), vSyncWriteGoalPos(), and vSyncWriteGoalSpeed().

Referenced by DynaShellCmdList::Exec().

  {
    return ((nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX))?
             m_links[nServoId].m_nServoIdMate: DYNA_ID_NONE;
  }

virtual uint_t DynaChain::GetNumberInChain ( ) const

inlinevirtual

Get the number of servos currently in chain.

Returns

Number in chain.

Definition at line 140 of file DynaChain.h.

References LinkServos(), m_uNumInChain, and UnlinkServos().

Referenced by DynaShellCmd::ChkChainNotEmpty(), DynaShellCmdScan::Exec(), DynaBgThread::RegisterChainAgent(), DynaShellCmdSetPid::TabCompletion(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdLink::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), DynaShellCmdUnlink::TabCompletion(), DynaShellCmdBgtStart::TabCompletion(), DynaShellCmdReadMaxTorqueLimit::TabCompletion(), DynaShellCmdWriteMaxTorqueLimit::TabCompletion(), DynaShellCmdReadByte::TabCompletion(), DynaShellCmdWriteByte::TabCompletion(), and DynaShellCmdPlay::WaitStop().

  {
    return m_uNumInChain;
  }

uint_t DynaChain::GetNumberOfMastersInChain ( )

virtual

Get the number of servos currently in chain.

Returns

Number of masters in chain.

Definition at line 182 of file DynaChain.cxx.

References DYNA_ID_NONE, IsMaster(), m_nIIdx, m_uNumInChain, and m_uNumMastersInChain.

Referenced by AuditLinks(), DynaShellCmdPlay::Exec(), GetLinkedMateId(), LinkServos(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), and UnlinkServos().

{
  int   i;
  int   nServoId;

  m_uNumMastersInChain = 0;

  for(i=0; i<(int)m_uNumInChain; ++i)
  {
    nServoId = m_nIIdx[i];

    if( (nServoId != DYNA_ID_NONE) && IsMaster(nServoId) )
    {
      ++m_uNumMastersInChain;
    }
  }

  return m_uNumMastersInChain;
}

virtual DynaServo* DynaChain::GetServo ( int  nServoId )

inlinevirtual

Get the servo in the chain

Parameters

nServoId

Servo id.

Returns

Returns pointer to the servo if servo is the chain.
Returns NULL otherwise.

Definition at line 129 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, and m_pChain.

Referenced by DynaShellCmd::ChkChainIsMasterServo(), DynaShellCmdPlay::ControlToGoals(), DynaShellCmdCfgWriteServoMode::doExec(), DynaShellCmdWriteTorqueEnable::doExec(), DynaShellCmdWriteGoalSpeed::doExec(), DynaShellCmdSetPid::Exec(), DynaShellCmdList::Exec(), DynaShellCmdChainIn::Exec(), DynaShellCmdSetOdometer::Exec(), DynaShellCmdTrain::Exec(), DynaShellCmdPlay::Exec(), DynaShellCmdSetSoftTorqueTh::Exec(), DynaShellCmdPlay::PidInit(), DynaShellCmdPlay::PidSetPoint(), DynaShellCmdPlay::PlotWriteData(), DynaShellCmdTrain::RecordTraining(), and DynaShellCmdPlay::WaitStop().

  {
    return ((nServoId < DYNA_ID_MIN) || (nServoId > DYNA_ID_MAX))?
              NULL: m_pChain[nServoId];
  }

virtual bool DynaChain::HasServo ( int  nServoId ) const

inlinevirtual

Test if servo is in the chain.

Parameters

nServoId

Servo id.

Returns

Returns true if servo is the chain, false otherwise.

Definition at line 113 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, and m_pChain.

Referenced by DynaShellCmd::ChkChainHasServo().

  {
    return ((nServoId >= DYNA_ID_MIN) &&
            (nServoId <= DYNA_ID_MAX) &&
            (m_pChain[nServoId] != NULL))?  true: false;
  }

bool DynaChain::IsLinkedMaster ( int  nServoId ) const

inline

Test if this servo is a linked master.

A linked master is the master servo in a linked master-slave configuration.

Parameters

nServoId

Servo id.

Returns

Returns true if a linked master, false otherwise.

Definition at line 196 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, DYNA_LINK_MASTER, and m_links.

Referenced by DynaShellCmdList::Exec(), and DynaShellCmdUnlink::TabCompletion().

  {
    return ((nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX) &&
             (m_links[nServoId].m_uLinkType == DYNA_LINK_MASTER))? true: false;
  }

bool DynaChain::IsLinkedSlave ( int  nServoId ) const

inline

Test if this servo is a linked slave.

A linked slave is the slave servo in a linked master-slave configuration.

Parameters

nServoId

Servo id.

Returns

Returns true if a linked slave, false otherwise.

Definition at line 211 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, DYNA_LINK_SLAVE, and m_links.

Referenced by DynaShellCmdList::Exec().

  {
    return ((nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX) &&
             (m_links[nServoId].m_uLinkType == DYNA_LINK_SLAVE))? true: false;
  }

virtual bool DynaChain::IsMaster ( int  nServoId ) const

inlinevirtual

Test if the servo is a master.

A master is either the master servo in a linked master-slave configuration, or an unlinked servo (a master unto thyself).

Parameters

nServoId

Servo id.

Returns

Returns true if a master, false otherwise.

Definition at line 180 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, DYNA_LINK_MASTER, DYNA_LINK_NONE, and m_links.

Referenced by GetNumberOfMastersInChain(), IterNextMaster(), and IterStartMaster().

  {
    return ((nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX) &&
            ((m_links[nServoId].m_uLinkType == DYNA_LINK_MASTER) ||
             (m_links[nServoId].m_uLinkType == DYNA_LINK_NONE)))? true: false;
  }

bool DynaChain::IsUnlinked ( int  nServoId ) const

inline

Test if this servo is unlinked.

Parameters

nServoId

Servo id.

Returns

Returns true if unlinked (or non-existent), false otherwise.

Definition at line 224 of file DynaChain.h.

References DYNA_ID_MAX, DYNA_ID_MIN, DYNA_LINK_NONE, and m_links.

Referenced by DynaShellCmdLink::TabCompletion().

  {
    return ((nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX) &&
             (m_links[nServoId].m_uLinkType == DYNA_LINK_NONE))? true: false;
  }

int DynaChain::IterNext ( int *  pIter )

virtual

Next iteration over of entire servo chain.

Parameters

pIter

Pointer to iterator.

Returns

On success, returns the next servo id.
On end of chain or error, DYNA_ID_NONE is returned.

Definition at line 1020 of file DynaChain.cxx.

References DYNA_ID_NONE, m_nIIdx, and m_uNumInChain.

Referenced by DynaShellCmdSetPid::Exec(), DynaShellCmdList::Exec(), DynaShellCmdChainIn::Exec(), DynaShellCmdChainOut::Exec(), DynaShellCmdReadMaxTorqueLimit::Exec(), DynaShellCmdWriteMaxTorqueLimit::Exec(), DynaShellCmdReadByte::Exec(), DynaShellCmdWriteByte::Exec(), GetLinkedMateId(), SyncWriteTorqueEnable(), DynaShellCmdSetPid::TabCompletion(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdLink::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), DynaShellCmdReadMaxTorqueLimit::TabCompletion(), DynaShellCmdWriteMaxTorqueLimit::TabCompletion(), DynaShellCmdReadByte::TabCompletion(), DynaShellCmdWriteByte::TabCompletion(), and DynaShellCmdPlay::WaitStop().

{
  int   i;

  i = *pIter;

  if( (i >= (int)m_uNumInChain) || (m_nIIdx[i] == DYNA_ID_NONE) )
  {
    return DYNA_ID_NONE;
  }

  *pIter = i + 1;

  return m_nIIdx[i];
}

int DynaChain::IterNextMaster ( int *  pIter )

virtual

Next iteration of master servos over of entire servo chain.

Parameters

pIter

Pointer to iterator.

Returns

On success, returns the next servo id.
On end of chain or error, DYNA_ID_NONE is returned.

Definition at line 1058 of file DynaChain.cxx.

References DYNA_ID_NONE, IsMaster(), m_nIIdx, m_pChain, and m_uNumInChain.

Referenced by DynaShellCmdMoveAtSpeedTo::Exec(), DynaShellCmdChainIn::Exec(), DynaShellCmdChainOut::Exec(), DynaShellCmdTrain::Exec(), DynaShellCmdPlay::Exec(), GetLinkedMateId(), DynaShellCmdPlay::PidInit(), DynaShellCmdTrain::RecordTraining(), Stop(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), and DynaShellCmdUnlink::TabCompletion().

{
  int   i;
  int   nServoId;

  for(i = *pIter; i<(int)m_uNumInChain; ++i)
  {
    nServoId = m_nIIdx[i];
    if( nServoId == DYNA_ID_NONE )
    {
      return DYNA_ID_NONE;
    }
    else if( m_pChain[nServoId]->IsMaster() )
    {
      *pIter = i + 1;
      return nServoId;
    }
  }

  return DYNA_ID_NONE;
}

int DynaChain::IterStart ( int *  pIter )

virtual

Start iteration over of entire servo chain.

Parameters

pIter

Pointer to iterator.

Returns

On success, returns the starting servo id.
On end of chain or error, DYNA_ID_NONE is returned.

Definition at line 1008 of file DynaChain.cxx.

References DYNA_ID_NONE, m_nIIdx, and m_uNumInChain.

Referenced by DynaShellCmdSetPid::Exec(), DynaShellCmdList::Exec(), DynaShellCmdChainIn::Exec(), DynaShellCmdChainOut::Exec(), DynaShellCmdReadMaxTorqueLimit::Exec(), DynaShellCmdWriteMaxTorqueLimit::Exec(), DynaShellCmdReadByte::Exec(), DynaShellCmdWriteByte::Exec(), GetLinkedMateId(), SyncWriteTorqueEnable(), DynaShellCmdSetPid::TabCompletion(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdLink::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), DynaShellCmdReadMaxTorqueLimit::TabCompletion(), DynaShellCmdWriteMaxTorqueLimit::TabCompletion(), DynaShellCmdReadByte::TabCompletion(), DynaShellCmdWriteByte::TabCompletion(), and DynaShellCmdPlay::WaitStop().

{
  if( m_uNumInChain == 0 )
  {
    return DYNA_ID_NONE;
  }

  *pIter = 1;

  return m_nIIdx[0];
}

int DynaChain::IterStartMaster ( int *  pIter )

virtual

Start iteration master servos over of entire servo chain.

Parameters

pIter

Pointer to iterator.

Returns

On success, returns the starting servo id.
On end of chain or error, DYNA_ID_NONE is returned.

Definition at line 1036 of file DynaChain.cxx.

References DYNA_ID_NONE, IsMaster(), m_nIIdx, m_pChain, and m_uNumInChain.

Referenced by DynaShellCmdMoveAtSpeedTo::Exec(), DynaShellCmdChainIn::Exec(), DynaShellCmdChainOut::Exec(), DynaShellCmdTrain::Exec(), DynaShellCmdPlay::Exec(), GetLinkedMateId(), DynaShellCmdPlay::PidInit(), DynaShellCmdTrain::RecordTraining(), Stop(), DynaShellCmdChainIn::TabCompletion(), DynaShellCmdChainOut::TabCompletion(), and DynaShellCmdUnlink::TabCompletion().

{
  int   i;
  int   nServoId;

  for(i=0; i<(int)m_uNumInChain; ++i)
  {
    nServoId = m_nIIdx[i];
    if( nServoId == DYNA_ID_NONE )
    {
      return DYNA_ID_NONE;
    }
    else if( m_pChain[nServoId]->IsMaster() )
    {
      *pIter = i + 1;
      return nServoId;
    }
  }

  return DYNA_ID_NONE;
}

int DynaChain::LinkServos ( int  nServoIdMaster, int  nServoIdSlave, bool  bIsReversed  )

virtual

Software link two unlinked servos in a master-slave configuration.

The servos must be of the same Dynamixel model number.

Parameters

nServoIdMaster	Master servo id.
nServoIdSlave	Slave servo id.
bRotReversed	The linked servos do [not] rotate in opposite directions.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 105 of file DynaChain.cxx.

References DYNA_ECODE_LINKED, DYNA_LINK_MASTER, DYNA_LINK_NONE, DYNA_LINK_SLAVE, DYNA_LOG_ERROR, DYNA_OK, DYNA_TRY_EXPR, DYNA_TRY_SERVO_IN_CHAIN, DynaServo::GetModelName(), DynaServo::GetModelNumber(), GetNumberOfMastersInChain(), DynaServo::Link(), m_links, m_pChain, and SetLinkData().

Referenced by DynaShellCmdLink::Exec(), and GetNumberInChain().

{
  DynaServo  *pServoM;
  DynaServo  *pServoS;
  int         rc;

  DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdMaster);
  DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdSlave);

  DYNA_TRY_EXPR(((m_links[nServoIdMaster].m_uLinkType == DYNA_LINK_NONE) &&
                 (m_links[nServoIdSlave].m_uLinkType == DYNA_LINK_NONE)),
      DYNA_ECODE_LINKED,
      "Servos %d and/or %d: Already have linkage.",
      nServoIdMaster, nServoIdSlave);
 
  pServoM = m_pChain[nServoIdMaster];
  pServoS = m_pChain[nServoIdSlave];

  if( pServoM->GetModelNumber() != pServoS->GetModelNumber() )
  {
    rc = -DYNA_ECODE_LINKED;
    DYNA_LOG_ERROR(rc, "Cannot link unmatched servos: "
        "servo %d model %s(%d) != servo %d model %s(%d).",
        nServoIdMaster, pServoM->GetModelName(), pServoM->GetModelNumber(),
        nServoIdSlave,  pServoS->GetModelName(), pServoS->GetModelNumber());
    return rc;
  }

  pServoM->Link(DYNA_LINK_MASTER, pServoS, bIsReversed);
  pServoS->Link(DYNA_LINK_SLAVE,  pServoM, bIsReversed);

  SetLinkData(nServoIdMaster, DYNA_LINK_MASTER, nServoIdSlave, bIsReversed);
  SetLinkData(nServoIdSlave, DYNA_LINK_SLAVE, nServoIdMaster, bIsReversed);

  GetNumberOfMastersInChain();

  return DYNA_OK;
}

int DynaChain::MoveAtSpeed ( int  nServoId, int  nGoalSpeed  )

virtual

Move at speed.

Speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify rotation in the clockwise direction, stop, or rotation in the counterclockwise direction, respectively.

For servos in servo mode, the goal direction is not applicable, and therefore, the speeds are in the range [0,max]. The special value 0 is equivalent to the maximum rpm speed without servo speed control.

If this servo is the master in a linked pair of servos, both servos will move synchronously.

Operational Modes:

Continuous mode (DYNA_MODE_CONTINUOUS).

Parameters

nServoId	Servo Id
nGoalSpeed	Goal speed.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 655 of file DynaChain.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_CHAIN, m_comm, and m_pChain.

Referenced by GetLinkedMateId().

{
  DynaServo *pServo;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_IN_CHAIN(this, nServoId);

  pServo = m_pChain[nServoId];

  DYNA_TRY_IS_MASTER(pServo);

  return pServo->MoveAtSpeed(nGoalSpeed);
}

int DynaChain::MoveAtSpeedTo ( int  nServoId, int  nGoalSpeed, int  nGoalOdPos  )

virtual

Move at speed to the goal postition.

For servos in continuous mode, speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify rotation in the clockwise direction, stop, or rotation in the counterclockwise direction, respectively.

For servos in servo mode, the goal direction is not applicable, and therefore, the speeds are in the range [0,max]. The special value 0 is equivalent to the maximum rpm speed without servo speed control.

If the servo is in continuous mode, but supports 360 ° positioning data, then a registered external user control function is required.

If this servo is the master in a linked pair of servos, both servos will move synchronously.

Operational Modes:

Servo mode (DYNA_MODE_SERVO). The direction will be ignored.
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

nServoId	Servo Id
uGoalSpeed	Goal speed (raw).
uGoalOdPos	Goal position (odometer ticks).

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 559 of file DynaChain.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_POS_CTL, DYNA_TRY_SERVO_IN_CHAIN, m_comm, and m_pChain.

Referenced by DynaShellCmdMoveAtSpeedTo::doExec(), and GetLinkedMateId().

{
  DynaServo *pServo;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_IN_CHAIN(this, nServoId);

  pServo = m_pChain[nServoId];

  DYNA_TRY_IS_MASTER(pServo);
  DYNA_TRY_SERVO_HAS_POS_CTL(pServo);

  return pServo->MoveAtSpeedTo(nGoalSpeed, nGoalOdPos);
}

int DynaChain::MoveTo ( int  nServoId, int  nGoalOdPos  )

virtual

Move to the goal postition.

The move will proceed at the current goal speed. If the servo is in continuous mode, but supports 360 ° positioning data, then a registered external user control function is required.

If this servo is the master in a linked pair of servos, both servos will move synchronously.

Operational Modes:

Servo mode (DYNA_MODE_SERVO).
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

nServoId	Servo Id
uGoalOdPos	Goal positon (odometer ticks).

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 480 of file DynaChain.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_CHAIN, m_comm, and m_pChain.

Referenced by DynaShellCmdMoveTo::doExec(), and GetLinkedMateId().

{
  DynaServo *pServo;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_IN_CHAIN(this, nServoId);

  pServo = m_pChain[nServoId];

  DYNA_TRY_IS_MASTER(pServo);

  return pServo->MoveTo(nGoalOdPos);
}

const DynaServo * DynaChain::operator[]

(

int

nServoId

)

Subscript operator.

Parameters

nServoId

Servo id.

Returns

On success, returns pointer to servo object in chain.
On failure, return NULL.

Definition at line 1085 of file DynaChain.cxx.

References DYNA_ID_MAX, DYNA_ID_MIN, and m_pChain.

Referenced by GetLinkedMateId().

{
  if( (nServoId >= DYNA_ID_MIN) && (nServoId <= DYNA_ID_MAX) )
  {
    return m_pChain[nServoId];
  }
  else
  {
    return NULL;
  }
}

int DynaChain::Release ( )

virtual

Release all servos from any applied torque.

Servos are free to rotate.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 770 of file DynaChain.cxx.

References DYNA_TRY_COMM, m_comm, and SyncWriteTorqueEnable().

Referenced by DynaShellCmdRelease::Exec(), and GetLinkedMateId().

{
  DYNA_TRY_COMM(m_comm);

  return SyncWriteTorqueEnable(false);
}

int DynaChain::RemoveAllServos ( )

virtual

Remove all servos from chain and delete.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 275 of file DynaChain.cxx.

References AuditLinks(), ChainEntryDelete(), DYNA_ID_MIN, DYNA_ID_NUMOF, DYNA_OK, and m_pChain.

Referenced by GetLinkedMateId().

{
  int   nServoId;

  for(nServoId=DYNA_ID_MIN; nServoId<DYNA_ID_NUMOF; ++nServoId)
  {
    if( m_pChain[nServoId] != NULL )
    {
      ChainEntryDelete(nServoId);
    }
  }

  AuditLinks();

  return DYNA_OK;
}

int DynaChain::RemoveServo ( int  nServoId )

virtual

Remove from chain and delete.

Parameters

nServoId

Servo Id to delete.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 265 of file DynaChain.cxx.

References AuditLinks(), ChainEntryDelete(), DYNA_OK, and DYNA_TRY_SERVO_IN_CHAIN.

Referenced by GetLinkedMateId().

{
  DYNA_TRY_SERVO_IN_CHAIN(this, nServoId);

  ChainEntryDelete(nServoId);
  AuditLinks();

  return DYNA_OK;
}

void DynaChain::SetLinkData ( int  nServoId, uint_t  uLinkType, int  nServoIdMate, bool  bRotReversed  )

protectedvirtual

Set local link data for the given servo.

Parameters

nServiId	Servo id.
uLinkType	This servo's link type.
nServoIdMate	Mate servo id.
bRotReversed	The linked servos do [not] rotate in opposite directions.

Definition at line 458 of file DynaChain.cxx.

References DynaServoLink_T::m_bRotReversed, m_links, DynaServoLink_T::m_nServoIdMate, and DynaServoLink_T::m_uLinkType.

Referenced by AuditLinks(), and LinkServos().

{
  m_links[nServoId].m_uLinkType     = uLinkType;
  m_links[nServoId].m_nServoIdMate  = nServoIdMate;
  m_links[nServoId].m_bRotReversed  = bRotReversed;
}

int DynaChain::Stop ( )

virtual

Stop all servos.

Current torque settings are kept.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 733 of file DynaChain.cxx.

References DYNA_ID_NONE, DYNA_TRY_COMM, IterNextMaster(), IterStartMaster(), m_comm, and m_pChain.

Referenced by Freeze(), and GetLinkedMateId().

{
  int         nServoId;   // working servo id
  int         iter;       // iterator
  int         rc;         // return code

  DYNA_TRY_COMM(m_comm);

  for(nServoId=IterStartMaster(&iter);
      nServoId!=DYNA_ID_NONE;
      nServoId=IterNextMaster(&iter))
  {
    rc = m_pChain[nServoId]->Stop();
  }

  return rc;
}

int DynaChain::SyncMoveAtSpeed ( DynaSpeedTuple_T  tuplesSpeed[], uint_t  uCount  )

virtual

Synchronous move servos at the given speeds.

If any servo is a linked master, the linked pair will moved synchronously at the appropriate speeds and directions.

Servo Operational Modes:

Continuous mode (DYNA_MODE_CONTINUOUS).

Parameters

tuplesSpeed	Array of servo id, goal speed 2-tuples.
uCount	Number of tuples.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 695 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_MODE_CONTINUOUS, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_CHAIN, DYNA_TRY_SERVO_IN_MODE, m_comm, m_pChain, and SyncWriteGoalSpeed().

Referenced by GetLinkedMateId().

{
  int   i;
  int   rc;

  DYNA_TRY_COMM(m_comm);

  for(i=0; i<(int)uCount && i<DYNA_ID_NUMOF; ++i)
  {
    DYNA_TRY_SERVO_IN_CHAIN(this, tuplesSpeed[i].m_nServoId);
    DYNA_TRY_IS_MASTER(m_pChain[tuplesSpeed[i].m_nServoId]);
    DYNA_TRY_SERVO_IN_MODE(m_pChain[tuplesSpeed[i].m_nServoId],
                          DYNA_MODE_CONTINUOUS);
  }

  return SyncWriteGoalSpeed(tuplesSpeed, (uint_t)i);
}

int DynaChain::SyncMoveAtSpeedTo ( DynaSpeedPosTuple_T  tuplesSpeedPos[], uint_t  uCount  )

virtual

Synchronous move servos to new goal positions at the given speeds.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions at the appropriate speeds.

Todo:

Optimize. Add support for bg task continuous mode sync write, then control.

Operational Modes:

Servo mode (DYNA_MODE_SERVO).
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

tuplesSpeedPos

Array of servo id, goal speed, and goal odometer position 3-tuples.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 596 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_POS_CTL, DYNA_TRY_SERVO_IN_CHAIN, DynaServo::HasAgent(), m_comm, DynaPosTuple_T::m_nPos, DynaPosTuple_T::m_nServoId, DynaSpeedTuple_T::m_nServoId, DynaSpeedPosTuple_T::m_nServoId, DynaSpeedTuple_T::m_nSpeed, m_pChain, SyncWriteGoalPos(), and SyncWriteGoalSpeed().

Referenced by DynaShellCmdMoveAtSpeedTo::doExec(), GetLinkedMateId(), and vSyncMoveAtSpeedTo().

{
  DynaSpeedTuple_T  tuplesSpeed[DYNA_ID_NUMOF];
  DynaPosTuple_T    tuplesPos[DYNA_ID_NUMOF];
  DynaServo        *pServo;
  int               i, j;
  int               rc;

  DYNA_TRY_COMM(m_comm);

  for(i=0, j=0, rc=DYNA_OK;
      i<(int)uCount && i<DYNA_ID_NUMOF && rc==DYNA_OK;
      ++i)
  {
    DYNA_TRY_SERVO_IN_CHAIN(this, tuplesSpeedPos[i].m_nServoId);

    pServo = m_pChain[tuplesSpeedPos[i].m_nServoId];

    DYNA_TRY_IS_MASTER(pServo);
    DYNA_TRY_SERVO_HAS_POS_CTL(pServo);

    //
    // In continuous mode: Start host controlled positioning.
    //
    if( pServo->HasAgent() )
    {
      rc = pServo->MoveAtSpeedTo(tuplesSpeedPos[i].m_nSpeed,
                                 tuplesSpeedPos[i].m_nPos);
    }

    //
    // Servo controlled move.
    //
    else
    {
      tuplesSpeed[j].m_nServoId   = tuplesSpeedPos[i].m_nServoId;
      tuplesSpeed[j].m_nSpeed     = tuplesSpeedPos[i].m_nSpeed;

      tuplesPos[j].m_nServoId     = tuplesSpeedPos[i].m_nServoId;
      tuplesPos[j].m_nPos         = tuplesSpeedPos[i].m_nPos;

      ++j;
    }
  }

  if( (j > 0) && (rc == DYNA_OK) )
  {
    rc = SyncWriteGoalSpeed(tuplesSpeed, (uint_t)j);

    if( rc == DYNA_OK )
    {
      rc = SyncWriteGoalPos(tuplesPos, (uint_t)j);
    }
  }

  return rc;
}

int DynaChain::SyncMoveTo ( DynaPosTuple_T  tuplesPos[], uint_t  uCount  )

virtual

Synchronous move servos to new goal positions in tuple list.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions.

The moves will proceed at the current goal speeds.

Operational Modes:

Servo mode (DYNA_MODE_SERVO).
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

tuplesPos	Array of servo id, goal odometer position tuples.
uCount	Number of tuples.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 513 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_MODE_CONTINUOUS, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_POS_CTL, DYNA_TRY_SERVO_IN_CHAIN, DynaServo::GetServoMode(), m_comm, DynaPosTuple_T::m_nPos, DynaPosTuple_T::m_nServoId, m_pChain, and SyncWriteGoalPos().

Referenced by DynaShellCmdMoveTo::doExec(), GetLinkedMateId(), DynaShellCmdPlay::SetRecordGoals(), and vSyncMoveTo().

{
  DynaServo            *pServo;
  DynaPosTuple_T        tuple[DYNA_ID_NUMOF];
  int                   i;
  int                   j;
  int                   rc;

  DYNA_TRY_COMM(m_comm);

  for(i=0, j=0, rc=DYNA_OK;
      i<(int)uCount && i<DYNA_ID_NUMOF && rc==DYNA_OK;
      ++i)
  {
    DYNA_TRY_SERVO_IN_CHAIN(this, tuplesPos[i].m_nServoId);

    pServo = m_pChain[tuplesPos[i].m_nServoId];

    DYNA_TRY_IS_MASTER(pServo);
    DYNA_TRY_SERVO_HAS_POS_CTL(pServo);

    //
    // In continuous mode: Start host controlled positioning.
    //
    if( pServo->GetServoMode() == DYNA_MODE_CONTINUOUS )
    {
      rc = pServo->MoveTo(tuplesPos[i].m_nPos);
    }
    //
    // Let Dynmixel firmware take control of the move.
    else
    {
      tuple[j].m_nServoId = tuplesPos[i].m_nServoId;
      tuple[j].m_nPos     = tuplesPos[i].m_nPos;
      ++j;
    }
  }

  if( (rc == DYNA_OK) && (j > 0) )
  {
    rc = SyncWriteGoalPos(tuplesPos, (uint_t)j);
  }

  return rc;
}

int DynaChain::SyncWriteGoalPos ( DynaPosTuple_T  tuplesPos[], uint_t  uCount  )

virtual

Synchronous write new goal positions for servos.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions.

Control Table:

dyna_servo_memmap_goal_pos.
Dynamixel Position Common Values.

Parameters

tuplesPos	Array of servo id, goal odometer position tuples.
uCount	Number of tuples.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 837 of file DynaChain.cxx.

References DYNA_ADDR_GOAL_POS_LSB, DYNA_ECODE_BAD_VAL, DYNA_ID_NUMOF, DYNA_LINK_MASTER, DYNA_MODE_SERVO, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DYNA_TRY_RC, DYNA_TRY_SERVO_IN_CHAIN, DynaServo::GetServoId(), DynaServo::GetServoMode(), m_comm, m_links, DynaPosTuple_T::m_nPos, DynaSyncWriteTuple_T::m_nServoId, DynaPosTuple_T::m_nServoId, DynaServoLink_T::m_nServoIdMate, m_pChain, DynaServo::m_spec, DynaServo::m_state, DynaServoState_T::m_uGoalPos, DynaServoSpec_T::m_uRawPosMin, DynaSyncWriteTuple_T::m_uVal, DynaServo::OdometerToEncoder(), and DynaComm::SyncWrite().

Referenced by GetLinkedMateId(), SyncMoveAtSpeedTo(), SyncMoveTo(), and vSyncWriteGoalPos().

{
  DynaSyncWriteTuple_T  tuples[DYNA_ID_NUMOF];  // servo_id, enc_pos tuples
  int                   nServoIdM;              // master servo id
  int                   nServoIdS;              // slave servo id
  DynaServo            *pServoM;                // master servo
  DynaServo            *pServoS;                // slave servo
  int                   nGoalOdPosM;            // master goal odometer position
  int                   nGoalOdPosS;            // slave goal odometer position
  int                   nEncPos;                // working encoder position
  bool                  bRotReversed;           // slave rotation is reverse
  int                   i, j;                   // working indices
  int                   rc;                     // return code

  DYNA_TRY_COMM(m_comm);

  // 
  // Loop through tuples.
  //
  for(i=0, j=0; (i<(int)uCount) && (j<DYNA_ID_NUMOF-1); ++i)
  {
    nServoIdM   = tuplesPos[i].m_nServoId;
    nGoalOdPosM = tuplesPos[i].m_nPos;

    DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdM);

    pServoM = m_pChain[nServoIdM];

    DYNA_TRY_IS_MASTER(pServoM);

    nEncPos = pServoM->OdometerToEncoder(nGoalOdPosM);

    if( pServoM->GetServoMode() == DYNA_MODE_SERVO )
    {
      DYNA_TRY_EXPR(((nEncPos >= pServoM->m_spec.m_uRawPosMin) &&
                     (nEncPos <= pServoM->m_spec.m_uRawPosMax)),
        DYNA_ECODE_BAD_VAL,
        "Master servo %d: Goal odometer position %d(encoder=%d): Out of range.",
        pServoM->GetServoId(), nGoalOdPosM, nEncPos);
    }

    tuples[j].m_nServoId  = nServoIdM;
    tuples[j].m_uVal      = (uint_t)nEncPos;

    j++;

    // master-slave linked servos
    if( m_links[nServoIdM].m_uLinkType == DYNA_LINK_MASTER )
    {
      nServoIdS = m_links[nServoIdM].m_nServoIdMate;

      DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdS);

      pServoS = m_pChain[nServoIdS];

      rc = pServoM->CalcMatesGoalPos(nGoalOdPosM, &nGoalOdPosS);

      DYNA_TRY_RC(rc, "Slave servo %d: Failed.", nServoIdS);

      nEncPos = pServoS->OdometerToEncoder(nGoalOdPosS);

      tuples[j].m_nServoId  = nServoIdS;
      tuples[j].m_uVal      = (uint_t)nEncPos;

      j++;
    }
  }

  // sync write
  rc = m_comm.SyncWrite(DYNA_ADDR_GOAL_POS_LSB, 2, tuples, (uint_t)j);

  if( rc == DYNA_OK )
  {
    for(i=0; i<j; ++i)
    {
      m_pChain[tuples[i].m_nServoId]->m_state.m_uGoalPos = tuples[i].m_uVal;
    }
  }

  return rc;
}

int DynaChain::SyncWriteGoalSpeed ( DynaSpeedTuple_T  tuplesSpeed[], uint_t  uCount  )

virtual

Synchronous write new goal speed for servos.

If a servo is in continuous mode, rotation will occur indefinitely in the direction specified and at the given speed.

If the servo is in servo mode, rotation will occur until the goal position is achieved.

If any servo is a linked master, the linked pair will moved synchronously at the appropriate speeds and directions.

Control Table:

dyna_servo_memmap_goal_speed.
Dynamixel Angular Speed Common Values.
Dynamixel Direction Common Values.

Parameters

tuplesSpeed	Array of servo id,speed 2-tuples.
uCount	Number of tuples.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 940 of file DynaChain.cxx.

References DYNA_ADDR_GOAL_SPEED_LSB, DYNA_ECODE_BAD_VAL, DYNA_ID_NUMOF, DYNA_LINK_MASTER, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_CHAIN, DynaServo::GetServoId(), iabs(), m_comm, m_links, DynaServoState_T::m_nGoalSpeed, DynaSyncWriteTuple_T::m_nServoId, DynaSpeedTuple_T::m_nServoId, DynaServoLink_T::m_nServoIdMate, DynaSpeedTuple_T::m_nSpeed, m_pChain, DynaServo::m_spec, DynaServo::m_state, DynaServoSpec_T::m_uRawSpeedMax, DynaSyncWriteTuple_T::m_uVal, and DynaComm::SyncWrite().

Referenced by DynaShellCmdPlay::ControlToGoals(), DynaShellCmdWriteGoalSpeed::doExec(), GetLinkedMateId(), SyncMoveAtSpeed(), SyncMoveAtSpeedTo(), vSyncMoveAtSpeed(), and vSyncWriteGoalSpeed().

{
  DynaSyncWriteTuple_T  tuples[DYNA_ID_NUMOF];
  int                   nServoIdM;
  int                   nServoIdS;
  DynaServo            *pServoM;
  DynaServo            *pServoS;
  DynaServo            *pServo;
  int                   nGoalSpeedM;
  int                   nGoalSpeedS;
  int                   i, j;
  int                   rc;

  DYNA_TRY_COMM(m_comm);

  for(i=0, j=0; (i<(int)uCount) && (j<DYNA_ID_NUMOF-1); ++i)
  {
    nServoIdM       = tuplesSpeed[i].m_nServoId;
    nGoalSpeedM     = tuplesSpeed[i].m_nSpeed;

    DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdM);

    pServoM = m_pChain[tuplesSpeed[i].m_nServoId];

    DYNA_TRY_IS_MASTER(pServoM);

    DYNA_TRY_EXPR( (iabs(nGoalSpeedM) <= pServoM->m_spec.m_uRawSpeedMax),
        DYNA_ECODE_BAD_VAL,
        "Master servo %d: Goal speed %d: Out of range.",
        pServoM->GetServoId(), nGoalSpeedM);

    tuples[j].m_nServoId  = nServoIdM;
    tuples[j].m_uVal = pServoM->PackGoalSpeed(nGoalSpeedM);
    j++;

    // master-slave linked servos
    if( m_links[nServoIdM].m_uLinkType == DYNA_LINK_MASTER )
    {
      nServoIdS = m_links[nServoIdM].m_nServoIdMate;
      pServoS   = m_pChain[nServoIdS];

      nGoalSpeedS = pServoM->CalcMatesGoalSpeed(nGoalSpeedM);

      tuples[j].m_nServoId  = nServoIdS;
      tuples[j].m_uVal = pServoS->PackGoalSpeed(nGoalSpeedS);
      j++;
    }
  }

  rc = m_comm.SyncWrite(DYNA_ADDR_GOAL_SPEED_LSB, 2, tuples, (uint_t)j);

  if( rc == DYNA_OK )
  {
    for(i=0; i<j; ++i)
    {
      pServo = m_pChain[tuples[i].m_nServoId];
      pServo->m_state.m_nGoalSpeed = pServo->UnpackGoalSpeed(tuples[i].m_uVal);
    }
  }

  return rc;
}

int DynaChain::SyncWriteTorqueEnable ( bool  bState )

virtual

Synchronous write to all of the servos in the chain to enable/disable applied torque.

If the torque state is false (off), no power is applied to the servo, allowing it to be free rotated by any external force.

Control Table:

dyna_servo_memmap_torque_en.

Parameters

bState

Torque enabled (true) or disabled (off) state.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 782 of file DynaChain.cxx.

References DYNA_ADDR_TORQUE_EN, DYNA_ID_NONE, DYNA_ID_NUMOF, DYNA_OK, DYNA_TORQUE_EN_OFF, DYNA_TORQUE_EN_ON, DYNA_TRY_COMM, IterNext(), IterStart(), DynaServoState_T::m_bTorqueEnabled, m_comm, DynaSyncWriteTuple_T::m_nServoId, m_pChain, DynaServo::m_state, DynaSyncWriteTuple_T::m_uVal, and DynaComm::SyncWrite().

Referenced by EStop(), Freeze(), GetLinkedMateId(), and Release().

{
  uint_t                uVal;
  int                   nServoId;
  int                   iter;
  DynaSyncWriteTuple_T  tuples[DYNA_ID_NUMOF];
  uint_t                uCount;
  int                   rc;

  DYNA_TRY_COMM(m_comm);

  uVal = bState? DYNA_TORQUE_EN_ON: DYNA_TORQUE_EN_OFF;
 
  for(nServoId=IterStart(&iter), uCount=0;
      nServoId!=DYNA_ID_NONE;
      nServoId=IterNext(&iter), ++uCount)
  {
    tuples[uCount].m_nServoId = nServoId;
    tuples[uCount].m_uVal     = uVal;
  }

  rc = m_comm.SyncWrite(DYNA_ADDR_TORQUE_EN, 1, tuples, uCount);

  if( rc == DYNA_OK )
  {
    for(iter=0; iter<(int)uCount; ++iter)
    {
      m_pChain[tuples[iter].m_nServoId]->m_state.m_bTorqueEnabled = bState;
    }
  }

  return rc;
}

int DynaChain::UnlinkServos ( int  nServoIdMaster )

virtual

Unlink two software linked servos.

Parameters

nServoIdMaster

Master servo id.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 146 of file DynaChain.cxx.

References ClearLinkData(), DYNA_ECODE_LINKED, DYNA_LINK_NONE, DYNA_OK, DYNA_TRY_EXPR, DYNA_TRY_SERVO_IN_CHAIN, GetNumberOfMastersInChain(), m_links, DynaServoLink_T::m_nServoIdMate, m_pChain, and DynaServo::Unlink().

Referenced by DynaShellCmdUnlink::Exec(), and GetNumberInChain().

{
  int         nServoIdSlave;
  DynaServo  *pServoM;
  DynaServo  *pServoS;
  int         rc;

  DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdMaster);

  pServoM = m_pChain[nServoIdMaster];

  nServoIdSlave = m_links[nServoIdMaster].m_nServoIdMate;

  DYNA_TRY_SERVO_IN_CHAIN(this, nServoIdSlave);

  pServoS = m_pChain[nServoIdSlave];

  DYNA_TRY_EXPR(
      ((m_links[nServoIdMaster].m_uLinkType != DYNA_LINK_NONE) &&
       (m_links[nServoIdSlave].m_uLinkType != DYNA_LINK_NONE) &&
       (m_links[nServoIdSlave].m_nServoIdMate == nServoIdMaster)),
      DYNA_ECODE_LINKED,
      "Servos %d and/or %d: Not linked.",
      nServoIdMaster, nServoIdSlave);
 
  pServoM->Unlink();
  pServoS->Unlink();

  ClearLinkData(nServoIdMaster);
  ClearLinkData(nServoIdSlave);

  GetNumberOfMastersInChain();

  return DYNA_OK;
}

int DynaChain::vSyncMoveAtSpeed ( uint_t  uCount,   ...  )

virtual

Synchronous move servos at the given speeds.

If any servo is a linked master, the linked pair will moved synchronously at the appropriate speeds and directions.

Servo Operational Modes:

Continuous mode (DYNA_MODE_CONTINUOUS).

Parameters

uCount	Number of tuples.
...	A variable argument list of uCount 2-tuples of type (int,int) specifying the servo id and goal speed.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 669 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_MODE_CONTINUOUS, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_CHAIN, DYNA_TRY_SERVO_IN_MODE, m_comm, DynaSpeedTuple_T::m_nServoId, DynaSpeedTuple_T::m_nSpeed, m_pChain, and SyncWriteGoalSpeed().

Referenced by GetLinkedMateId().

{
  DynaSpeedTuple_T  tuplesSpeed[DYNA_ID_NUMOF];
  va_list           ap;
  int               i;

  DYNA_TRY_COMM(m_comm);

  va_start(ap, uCount);

  for(i=0; i<uCount && i<DYNA_ID_NUMOF; ++i)
  {
    tuplesSpeed[i].m_nServoId = va_arg(ap, int);
    tuplesSpeed[i].m_nSpeed   = va_arg(ap, int);

    DYNA_TRY_SERVO_IN_CHAIN(this, tuplesSpeed[i].m_nServoId);
    DYNA_TRY_IS_MASTER(m_pChain[tuplesSpeed[i].m_nServoId]);
    DYNA_TRY_SERVO_IN_MODE(m_pChain[tuplesSpeed[i].m_nServoId],
                          DYNA_MODE_CONTINUOUS);
  }

  va_end(ap);

  return SyncWriteGoalSpeed(tuplesSpeed, (uint_t)i);
}

int DynaChain::vSyncMoveAtSpeedTo ( uint_t  uCount,   ...  )

virtual

Synchronous move servos to new goal positions at the given speeds.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions at the appropriate speeds.

Operational Modes:

Servo mode (DYNA_MODE_SERVO).
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

uCount	Number of tuples.
...	A variable argument list of uCount 3-tuples of type (int,int,int) specifying the servo id, goal speed, and the end goal odometer position.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 574 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_TRY_COMM, m_comm, DynaSpeedPosTuple_T::m_nPos, DynaSpeedPosTuple_T::m_nServoId, DynaSpeedPosTuple_T::m_nSpeed, and SyncMoveAtSpeedTo().

Referenced by GetLinkedMateId().

{
  DynaSpeedPosTuple_T tuplesSpeedPos[DYNA_ID_NUMOF];
  va_list             ap;
  int                 i;

  DYNA_TRY_COMM(m_comm);

  va_start(ap, uCount);

  for(i=0; i<uCount && i<DYNA_ID_NUMOF; ++i)
  {
    tuplesSpeedPos[i].m_nServoId  = va_arg(ap, int);
    tuplesSpeedPos[i].m_nSpeed    = va_arg(ap, int);
    tuplesSpeedPos[i].m_nPos      = va_arg(ap, int);
  }

  va_end(ap);

  return SyncMoveAtSpeedTo(tuplesSpeedPos, (uint_t)i);
}

int DynaChain::vSyncMoveTo ( uint_t  uCount,   ...  )

virtual

Synchronous move servos to new goal positions.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions.

The moves will proceed at the current goal speeds.

Operational Modes:

Servo mode (DYNA_MODE_SERVO).
Continuous mode (DYNA_MODE_CONTINUOUS) with 360 ° position data.

Parameters

uCount	Number of tuples.
...	A variable argument list of uCount 2-tuples of type (int,int) specifying the servo id and the goal end position (odometer ticks).

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 494 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DynaPosTuple_T::m_nPos, DynaPosTuple_T::m_nServoId, and SyncMoveTo().

Referenced by GetLinkedMateId().

{
  DynaPosTuple_T    tuplesPos[DYNA_ID_NUMOF];
  va_list           ap;
  int               i;

  va_start(ap, uCount);

  for(i=0; i<uCount && i<DYNA_ID_NUMOF; ++i)
  {
    tuplesPos[i].m_nServoId = va_arg(ap, int);
    tuplesPos[i].m_nPos     = va_arg(ap, int);
  }

  va_end(ap);

  return SyncMoveTo(tuplesPos, (uint_t)i);
}

int DynaChain::vSyncWriteGoalPos ( uint_t  uCount,   ...  )

virtual

Synchronous write new goal positions for servos.

If any servo is a linked master, the linked pair will moved synchronously to the appropriate end positions.

Control Table:

dyna_servo_memmap_goal_pos.
Dynamixel Position Common Values.

Parameters

uCount	Number of tuples.
...	A variable argument list of uCount 2-tuples of type (int,int) specifying the servo id and the goal end position (odometer ticks).

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 816 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_TRY_COMM, m_comm, DynaPosTuple_T::m_nPos, DynaPosTuple_T::m_nServoId, and SyncWriteGoalPos().

Referenced by GetLinkedMateId().

{
  DynaPosTuple_T  tuplesPos[DYNA_ID_NUMOF]; 
  va_list         ap;
  int             i;

  DYNA_TRY_COMM(m_comm);

  va_start(ap, uCount);

  for(i=0; i<uCount && i<DYNA_ID_NUMOF; ++i)
  {
    tuplesPos[i].m_nServoId = va_arg(ap, int);
    tuplesPos[i].m_nPos     = va_arg(ap, int);
  }

  va_end(ap);

  return SyncWriteGoalPos(tuplesPos, (uint_t)i);
}

int DynaChain::vSyncWriteGoalSpeed ( uint_t  uCount,   ...  )

virtual

Synchronous write new goal speed for for servos.

For servos in continuous mode, speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify rotation in the clockwise direction, stop, or rotation in the counterclockwise direction, respectively.

For servos in servo mode, the goal direction is not applicable, and therefore, the speeds are in the range [0,max]. The special value 0 is equivalent to the maximum rpm speed without servo speed control.

If any servo is a linked master, the linked pair will moved synchronously at the appropriate speeds and directions.

Parameters

uCount	Number of tuples.
...	A variable argument list of uCount 2-tuples of type (int,int) specifying the servo id and the goal speed.

Returns

On success, DYNA_OK is returned.
On error, the appropriate < 0 negated Dynamixel Error Code is returned.

Definition at line 919 of file DynaChain.cxx.

References DYNA_ID_NUMOF, DYNA_TRY_COMM, m_comm, DynaSpeedTuple_T::m_nServoId, DynaSpeedTuple_T::m_nSpeed, and SyncWriteGoalSpeed().

Referenced by GetLinkedMateId().

{
  DynaSpeedTuple_T  tuplesSpeed[DYNA_ID_NUMOF];
  va_list           ap;
  int               i;

  DYNA_TRY_COMM(m_comm);

  va_start(ap, uCount);

  for(i=0; i<uCount && i<DYNA_ID_NUMOF; ++i)
  {
    tuplesSpeed[i].m_nServoId = va_arg(ap, int);
    tuplesSpeed[i].m_nSpeed   = va_arg(ap, int);
  }

  va_end(ap);

  return SyncWriteGoalSpeed(tuplesSpeed, (uint_t)i);
}

---

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

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