DynaServoGeneric Class Reference

Generic Dynamixel Servo Base Class. More...

#include <DynaServoGeneric.h>

Inheritance diagram for DynaServoGeneric:

Public Member Functions
	DynaServoGeneric (DynaComm &comm)
	Bare-bones initialization constructor. More...
	DynaServoGeneric (DynaComm &comm, int nServoId, uint_t uModelNum=DYNA_MODEL_NUM, uint_t uFwVer=DYNA_FWVER_NA)
	Initialization constructor. More...
virtual	~DynaServoGeneric ()
	Destructor.
virtual float	CvtRawTempToC (uint uTemp)
	Convert raw temperature coding to degrees Celsius. More...
virtual float	CvtRawVoltToVolts (uint uVolts)
	Convert raw volts coding to volts. More...
virtual int	AgentWriteGoalPos (int nGoalOdPos)
	Agent write to the servo state memory to set the new goal position. More...
virtual int	AgentWriteGoalSpeed (int nGoalSpeed)
	Agent write to the servo state memory to set the new goal speed. More...
virtual int	AgentWriteGoalSpeedPos (int nGoalSpeed, int nGoalOdPos)
	Agent write to the servo state memory to set the new goal position and speed. More...
virtual int	MoveTo (int nGoalOdPos)
	Move to the goal postition. More...
virtual int	MoveAtSpeedTo (int nGoalSpeed, int nGoalOdPos)
	Move at speed to the goal postition. More...
virtual int	MoveAtSpeed (int nGoalSpeed)
	Move at speed. More...
virtual int	EStop ()
	Emergency stop servo. More...
virtual int	Stop ()
	Stop servo. More...
virtual int	Freeze ()
	Freeze servo at current position. More...
virtual int	Release ()
	Release servo from any applied torque. More...
virtual int	CfgReadRotationLimits (uint_t *pCwLim, uint_t *pCcwLim)
	Read from the servo configuration EEPROM the current rotation limits. More...
virtual int	CfgWriteRotationLimits (uint_t uCwLim, uint_t uCcwLim)
	Write to the servo configuration EEPROM the new rotation limits. More...
virtual int	CfgReadTemperatureLimit (uint_t *pTempLim)
	Read from the servo configuration EEPROM the current maximum temperature limit. More...
virtual int	CfgWriteTemperatureLimit (uint_t uTempLim)
	Write to the servo configuration EEPROM the new maximum temperature limit. More...
virtual int	CfgReadVoltageLimits (uint_t *pMinVoltLim, uint_t *pMaxVoltLim)
	Read from the servo configuration EEPROM the current voltage limits. More...
virtual int	CfgWriteVoltageLimits (uint_t uMinVoltLim, uint_t uMaxVoltLim)
	Write to the servo configuration EEPROM the new voltage limits. More...
virtual int	CfgReadMaxTorqueLimit (uint_t *pMaxTorqueLim)
	Read from the servo configuration EEPROM the current on power-up maximum torque limit. More...
virtual int	CfgWriteMaxTorqueLimit (uint_t uMaxTorqueLim)
	Write to the servo configuration EEPROM the new on power-up maximum torque limit. More...
virtual int	CfgReadAlarmShutdownMask (uint_t *pAlarmMask)
	Read from the servo configuration EEPROM the current servo shutdown on alarms mask. More...
virtual int	CfgWriteAlarmShutdownMask (uint_t uAlarmMask)
	Write to the servo configuration EEPROM the new servo shutdown on alarms mask. More...
virtual int	CfgReadServoMode (uint_t *pServoMode)
	Read from the servo configuration EEPROM to determine the servo operational mode. More...
virtual int	CfgWriteServoMode (uint_t uCwLim, uint_t uCcwLim)
	Write to the servo configuration EEPROM to set the servo operational mode. More...
virtual int	CfgWriteServoModeContinuous ()
	Write to the servo configuration EEPROM to set the servo operational mode to the full/continuous mode. More...
virtual int	ReadTorqueEnable (bool *pState)
	Read from the servo state memory the current torque enable value. More...
virtual int	WriteTorqueEnable (bool bState)
	Write to the servo state memory to set the new torque enable value. More...
virtual int	ReadLed (bool *pState)
	Read from the servo state memory the current LED on/off value. More...
virtual int	WriteLed (bool bState)
	Write to the servo state memory to turn on or off the servo LED. More...
virtual int	ReadControlMethod (DynaServoCtlMethod_T *pCtlMethod)
	Read from the servo state memory the current control method parameters. More...
virtual int	WriteControlMethod (DynaServoCtlMethod_T &ctlMethod)
	Write to the servo state memory the new control method parameters. More...
virtual int	ReadGoalPos (int *pGoalPos)
	Read from the servo state memory the current goal position. More...
virtual int	WriteGoalPos (int nGoalOdPos)
	Write to the servo state memory to set the new goal position. More...
virtual int	ReadGoalSpeed (int *pGoalSpeed)
	Read from the servo state memory the current goal speed and direction. More...
virtual int	WriteGoalSpeed (int nGoalSpeed)
	Write to the servo state memory the new goal speed and direction. More...
virtual int	ReadMaxTorqueLimit (uint_t *pMaxTorqueLim)
	Read from the servo state memory the current maximum torque limit. More...
virtual int	WriteMaxTorqueLimit (uint_t uMaxTorqueLim)
	Write to the servo state memory to set the new maximum torque limit. More...
virtual int	ReloadMaxTorqueLimit ()
	Reload the maximum torque limit from the configuration. More...
virtual int	ReadCurPos (int *pCurOdPos)
	Read from the servo state memory the current servo position. More...
virtual int	ReadCurSpeed (int *pCurSpeed)
	Read from the servo state memory the current speed and direction. More...
virtual int	ReadCurLoad (int *pCurLoad)
	Read from the servo state memory the current load. More...
virtual int	ReadDynamics (int *pCurPos, int *pCurSpeed, int *pCurLoad)
	Read from the servo state memory the current servo dynamics. More...
virtual int	ReadHealth (uint_t *pAlarms, int *pCurLoad, uint_t *pCurVolt, uint_t *pCurTemp)
	Read from the servo state memory the current servo health. More...
virtual int	ReadIsMoving (bool *pState)
	Read from the servo state memory to test if the servo is currently moving. More...
virtual int	Read (uint_t uAddr, uint_t *pVal)
virtual int	Write (uint_t uAddr, uint_t uVal)
virtual bool	Ping ()
	Ping this servo. More...
virtual int	Reset ()
	Reset this servo back to default values. More...
virtual int	SyncData ()
	Synchronize the shadowed configuration and state data to the servo control table. More...
virtual int	SyncCfg ()
	Synchronize the shadowed configuration to the servo control table EEPROM configuration. More...
virtual int	SyncState ()
	Synchronize the shadowed state data to the servo control table RAM state. More...
virtual void	Dump ()
	Dump contents of the servo EEPROM and RAM control tables.
Public Member Functions inherited from DynaServo
	DynaServo (DynaComm &comm, int nServoId, uint_t uModelNum, uint_t uFwVer)
	Default initialization constructor. More...
virtual	~DynaServo ()
	Destructor.
virtual uint_t	GetModelNumber () const
	Get servo model number. More...
virtual uint_t	GetFirmwareVersion () const
	Get servo firmware version. More...
virtual const char *	GetModelName () const
	Get servo model name string. More...
virtual uint_t	GetServoId () const
	Get servo id. More...
virtual uint_t	GetServoMode () const
	Get the servo operational mode. More...
virtual uint_t	Has360PosInfo () const
	Test if servo has 360 ° positioning information. More...
virtual const DynaServoSpec_T &	GetSpecification () const
	Get servo specification. More...
virtual const DynaServoCfg_T &	GetConfiguration () const
	Get servo configuration. More...
virtual const DynaServoState_T &	GetState () const
	Get servo state. More...
bool	IsMaster () const
	Test if this servo is a master. More...
bool	IsLinkedMaster () const
	Test if this servo is a linked master. More...
bool	IsUnlinked () const
	Test if this servo is unlinked. More...
virtual const DynaServoLink_T	GetLinkInfo () const
	Get linked information. More...
virtual void	Link (uint_t uLinkType, DynaServo *pServoMate, bool bRotReversed)
	Link this servo to another. More...
virtual void	Unlink ()
	Unlink this servo.
int	GetOdometer ()
	Get the current virtual odometer value. More...
int	IsOdometerEnabled ()
	Test if virtual odometer mapping is enabled. More...
int	GetOdometerZeroPt ()
	Get the virtual odometer zero point. More...
bool	IsOdometerReversed ()
	Test if the virtual odometer is reversed. More...
int	CalcOdometerAtEncMin ()
	Calculate the odometer value at the minimum (zero) encoder value. More...
int	CalcOdometerAtEncMax ()
	Calculate the odometer value at the maximum encoder value. More...
int	OdometerToEncoder (int nOdPos)
	Convert virtual odometer units to servo encoder units. More...
int	CalcSpeedDir (int nOdGoalPos)
	Calculate serve direction to goal odometer position. More...
virtual int	ResetOdometer (int nEncZeroPt, bool bIsReverse)
	Reset the servo's virtual odometer. More...
virtual int	UpdateOdometer (int nEncCurPos)
	Update the odometer from the current servo position and rotation direction. More...
virtual void	DisableOdometer ()
	Disable odometer mapping. More...
virtual uint_t	GetAlarms () const
	Get the current servo alarms. More...
virtual uint_t	GetCurPos () const
	Get the current servo position. More...
virtual int	GetCurSpeed () const
	Get the current servo speed. More...
virtual int	GetGoalSpeed () const
	Get the goal servo speed. More...
virtual int	GetCurLoad () const
	Get the current servo load. More...
virtual uint_t	GetCurTemp () const
	Get the current temperature. More...
virtual uint_t	GetCurVolt () const
	Get the current voltage. More...
virtual void	SetSoftTorqueThresholds (uint_t uOverTorqueTh, uint_t uClearTorqueTh)
	Set soft torque thresholds. More...
virtual void	GetSoftTorqueThresholds (uint_t &uOverTorqueTh, uint_t &uClearTorqueTh)
	Get soft torque thresholds. More...
virtual void	SetSoftTorqueOverCond (bool bNewCond)
	Set or clear servo in soft over torque condition. More...
virtual bool	HasSoftTorqueOverCond ()
	Test if servo is in a soft over torque condition. More...
virtual void	RegisterAgent (DynaAgent_T *pAgent, void *pAgentArg)
	Register servo proxy agent. More...
virtual void	UnregisterAgent ()
	Unregister servo proxy agent.
virtual bool	HasAgent ()
	Tests if servo has a registered agent. More...

Static Public Attributes
static const int	DYNA_MODEL_NUM = DYNA_MODEL_NUM_GENERIC

Protected Member Functions
void	Init ()
	Initialize servo class instance.
void	InitSpec ()
	Initialize servo fixed specification data.
void	InitCfg ()
	Initialize servo configuration data.
void	InitState ()
	Initialize servo state data.
virtual void	CheckData ()
	Check data for consitencies. More...
void	SetServoMode ()
	Set the servo mode given the servo capabilites and the current rotation limits. More...
virtual int	CalcMatesGoalPos (int nGoalOdPos, int *pGoalOdPosMate)
	Calculate the linked mate's goal position given this servo's goal position. More...
virtual int	CalcMatesGoalSpeed (int nGoalSpeed)
	Calculate the linked mate's speed speed given this servo's goal speed. More...
virtual int	ReadCtlMethodCompliance (DynaServoCtlMethod_T *pCtlMethod)
	Read from the servo state memory the current compliance control method parameters. More...
virtual int	WriteCtlMethodCompliance (DynaServoCtlMethod_T &ctlMethod)
	Write to the servo state memory the new compliance control method parameters. More...
virtual int	ReadCtlMethodPid (DynaServoCtlMethod_T *pCtlMethod)
	Read from the servo state memory the current PID control method parameters. More...
virtual int	WriteCtlMethodPid (DynaServoCtlMethod_T &ctlMethod)
	Write to the servo state memory the new PID control method parameters. More...
virtual bool	ChkComplianceSlope (uint_t uVal)
	Check validity of compliance slope discrete values. More...
virtual uint_t	PackGoalSpeed (int nGoalSpeed)
	Pack the goal speed into the control table value. More...
virtual int	UnpackGoalSpeed (uint_t uVal)
	Unpack goal speed from the control table value. More...
virtual int	UnpackCurSpeed (uint_t uVal)
	Unpack current speed from the control table value. More...
virtual int	UnpackCurLoad (uint_t uVal)
	Unpack current load estimate from the control table value. More...
Protected Member Functions inherited from DynaServo
void	Init (int nServoId, uint_t uModelNum, uint_t uFwVer)
	Initialize servo class instance. More...
void	DumpCtlTbl (const char *sTblName, const DynaCtlTblEntry_T tblInfo[], size_t uSize)
	Dump the servo control tabl values to stdout. More...

Additional Inherited Members
Static Public Member Functions inherited from DynaServo
static DynaServo *	New (DynaComm &comm, int nServoId)
	Archetype constructor to create a new Dynamixel servo instance. More...
static int	ReadModelNumber (DynaComm &comm, int nServoId, uint_t *pModelNum)
	Read the servo model number from the servo's EEPROM. More...
static int	ReadFirmwareVersion (DynaComm &comm, int nServoId, uint_t *pFwVer)
	Read the servo's firmware version from the servo's EEPROM. More...
static int	ReadServoId (DynaComm &comm, int nServoId, int *pServoId)
	Read the servo's id from the servo's EEPROM. More...
static int	WriteServoId (DynaComm &comm, int nServoId, int nNewServoId)
	Write the new servo id to the servo's EEPROM. More...
static int	ReadBaudRate (DynaComm &comm, int nServoId, int *pBaudRate)
	Read the servo's baud rate from the servo's EEPROM. More...
static int	WriteBaudRate (DynaComm &comm, int nServoId, int nNewBaudRate)
	Write the new baud rate to the servo's EEPROM. More...
static bool	Ping (DynaComm &comm, int nServoId)
	Ping the given servo. More...
static int	Reset (DynaComm &comm, int nServoId)
	Reset the given servo back to default values. More...
Protected Attributes inherited from DynaServo
DynaComm &	m_comm
	attached Dynamixel bus comm. object
int	m_nServoId
	servo id
DynaServoSpec_T	m_spec
	servo specification
DynaServoCfg_T	m_cfg
	servo shadowed EEPROM configuration
DynaServoState_T	m_state
	servo shadowed RAM state
CrossLink_T	m_link
	servo cross linkage
DynaAgent_T *	m_pAgent
	servo agent
void *	m_pAgentArg
	servo agent callback argument
int	m_nErrorCode
	class instance errored state

Detailed Description

Generic Dynamixel Servo Base Class.

The DynaServo base class provides the most commonly used interface functions for the various Dynamixel series servos.

Definition at line 75 of file DynaServoGeneric.h.

Constructor & Destructor Documentation

DynaServoGeneric::DynaServoGeneric ( DynaComm &  comm )

inline

Bare-bones initialization constructor.

May be used be derived classes to avoid undue communication and initializaton overhead.

Parameters

comm	Dynamixel bus communication instance.

Definition at line 90 of file DynaServoGeneric.h.

References DYNA_FWVER_NA, and ~DynaServoGeneric().

                                   : 
                  DynaServo(comm, DYNA_ID_NONE, DYNA_MODEL_NUM, DYNA_FWVER_NA)
  {
  }

DynaServoGeneric::DynaServoGeneric	(	DynaComm &	comm,
		int	nServoId,
		uint_t	uModelNum = DYNA_MODEL_NUM,
		uint_t	uFwVer = DYNA_FWVER_NA
	)

Initialization constructor.

Parameters

comm	Dynamixel bus communication instance.
nServoId	Servo Id.
uModelNum	Servo model number.
uFwVer	Servo firmware version.

Definition at line 134 of file DynaServoGeneric.cxx.

References CheckData(), Init(), and SyncData().

                                                     : 
                  DynaServo(comm, nServoId, uModelNum, uFwVer)
{
  Init();
  SyncData();
  CheckData();
}

Member Function Documentation

int DynaServoGeneric::AgentWriteGoalPos ( int  nGoalOdPos )

virtual

Agent write to the servo state memory to set the new goal position.

If the operation requires a servo proxy agent, then a call is made to the registered agent, if any. Otherwise a direct write to the servo is made.

Control Table:

RAM Goal Position.
Dynamixel Position Common Values.

Parameters

nGoalPos

Goal position (odometer ticks).

Returns

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

Implements DynaServo.

Definition at line 155 of file DynaServoGeneric.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_AGENT, DYNA_TRY_SERVO_HAS_POS_CTL, DynaServo::m_comm, DynaAgent_T::m_fnWriteGoalPos, DynaServoOdometer_T::m_nOdGoalPos, DynaServo::m_nServoId, DynaServoState_T::m_od, DynaServo::m_pAgent, DynaServo::m_pAgentArg, and DynaServo::m_state.

Referenced by CvtRawVoltToVolts(), and MoveTo().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);
  DYNA_TRY_SERVO_HAS_AGENT(this);

  m_state.m_od.m_nOdGoalPos = nGoalOdPos;
  rc = m_pAgent->m_fnWriteGoalPos(m_nServoId, nGoalOdPos, m_pAgentArg);

  return rc;
}

int DynaServoGeneric::AgentWriteGoalSpeed ( int  nGoalSpeed )

virtual

Agent write to the servo state memory to set the new goal speed.

If the operation requires a servo proxy agent, then a call is made to the registered agent, if any. Otherwise a direct write to the servo is made.

Control Table:

RAM Goal Speed and Direction.
Dynamixel Angular Speed Common Values.

Parameters

nGoalSpeed

Goal speed (raw).

Returns

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

Implements DynaServo.

Definition at line 170 of file DynaServoGeneric.cxx.

References DYNA_ECODE_BAD_VAL, DYNA_SPEED_MAX_RAW, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_AGENT, DYNA_TRY_SERVO_HAS_POS_CTL, iabs(), DynaServo::m_comm, DynaAgent_T::m_fnWriteGoalSpeed, DynaServo::m_nServoId, DynaServo::m_pAgent, and DynaServo::m_pAgentArg.

Referenced by CvtRawVoltToVolts(), and Stop().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);
  DYNA_TRY_SERVO_HAS_AGENT(this);

  DYNA_TRY_EXPR( (iabs(nGoalSpeed) <= DYNA_SPEED_MAX_RAW),
                  DYNA_ECODE_BAD_VAL,
                  "Goal speed %d: Out of range.", nGoalSpeed);

  rc = m_pAgent->m_fnWriteGoalSpeed(m_nServoId, nGoalSpeed, m_pAgentArg);

  return rc;
}

int DynaServoGeneric::AgentWriteGoalSpeedPos ( int  nGoalSpeed, int  nGoalOdPos  )

virtual

Agent write to the servo state memory to set the new goal position and speed.

If the operation requires a servo proxy agent, then a call is made to the registered agent, if any. Otherwise a direct write to the servo is made.

Control Table:

RAM Goal Position.
Dynamixel Position Common Values. RAM Goal Speed and Direction.
Dynamixel Angular Speed Common Values.

Parameters

nGoalOdPos	Goal position (odometer ticks).
nGoalSpeed	Goal speed (raw).

Returns

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

Implements DynaServo.

Definition at line 188 of file DynaServoGeneric.cxx.

References DYNA_ECODE_BAD_VAL, DYNA_SPEED_MAX_RAW, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_AGENT, DYNA_TRY_SERVO_HAS_POS_CTL, iabs(), DynaServo::m_comm, DynaAgent_T::m_fnWriteGoalSpeedPos, DynaServoOdometer_T::m_nOdGoalPos, DynaServo::m_nServoId, DynaServoState_T::m_od, DynaServo::m_pAgent, DynaServo::m_pAgentArg, and DynaServo::m_state.

Referenced by CvtRawVoltToVolts(), and MoveAtSpeedTo().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);
  DYNA_TRY_SERVO_HAS_AGENT(this);

  DYNA_TRY_EXPR( (iabs(nGoalSpeed) <= DYNA_SPEED_MAX_RAW),
                  DYNA_ECODE_BAD_VAL,
                  "Goal speed %d: Out of range.", nGoalSpeed);

  m_state.m_od.m_nOdGoalPos = nGoalOdPos;

  rc = m_pAgent->m_fnWriteGoalSpeedPos(m_nServoId,
                                       nGoalSpeed, nGoalOdPos,
                                       m_pAgentArg);

  return rc;
}

int DynaServoGeneric::CalcMatesGoalPos ( int  nGoalOdPos, int *  pGoalOdPosMate  )

protectedvirtual

Calculate the linked mate's goal position given this servo's goal position.

Parameters

	nGoalOdPos	Goal position (odometer ticks).
[out]	pGoalOdPosMate	Mate's calculated goal position (odometer ticks).

Returns

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

Implements DynaServo.

Definition at line 1725 of file DynaServoGeneric.cxx.

References DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_EXPR, DYNA_TRY_RC, DynaServo::GetOdometer(), DynaServo::CrossLink_T::m_bRotReversed, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServoSpec_T::m_uRawPosMax, DynaServoSpec_T::m_uRawPosMin, DynaServo::OdometerToEncoder(), and ReadCurPos().

Referenced by CvtRawVoltToVolts(), and WriteGoalPos().

{
  DynaServo  *pServoMate;
  int         nEncPos;
  int         nGoalOdPosMate;
  uint_t      uVal;
  int         nVal;
  int         delta;
  int         rc;

  // linked mate
  pServoMate = m_link.m_pServoMate;

  // read latest current positions for master and mate
  rc = ReadCurPos(&nVal);

  DYNA_TRY_RC(rc, "Servo %d: Failed.", m_nServoId);

  // mates rotate in reverse directions to each other 
  if( m_link.m_bRotReversed )
  {
    delta = GetOdometer() - nGoalOdPos;

    nGoalOdPosMate = pServoMate->GetOdometer() + delta;

    nEncPos = OdometerToEncoder(nGoalOdPosMate);

    DYNA_TRY_EXPR(((nEncPos >= m_spec.m_uRawPosMin) &&
                   (nEncPos <= m_spec.m_uRawPosMax)),
        DYNA_ECODE_BAD_VAL,
        "Mate servo %d: Goal odometer position %d (encoder=%d): Out of range.",
        m_nServoId, nGoalOdPosMate, nEncPos);
  }

  // same rotation direction
  else
  {
    nGoalOdPosMate = nGoalOdPos;
  }

  if( rc == DYNA_OK )
  {
    *pGoalOdPosMate = nGoalOdPosMate;
  }

  return rc;
}

virtual int DynaServoGeneric::CalcMatesGoalSpeed ( int  nGoalSpeed )

inlineprotectedvirtual

Calculate the linked mate's speed speed given this servo's goal speed.

Returns

Mate's goal speed.

Implements DynaServo.

Definition at line 1043 of file DynaServoGeneric.h.

References ChkComplianceSlope(), DynaServo::CrossLink_T::m_bRotReversed, DynaServo::m_link, PackGoalSpeed(), ReadCtlMethodCompliance(), ReadCtlMethodPid(), UnpackCurLoad(), UnpackCurSpeed(), UnpackGoalSpeed(), WriteCtlMethodCompliance(), and WriteCtlMethodPid().

Referenced by WriteGoalSpeed().

  {
    return m_link.m_bRotReversed? -nGoalSpeed: nGoalSpeed;
  }

int DynaServoGeneric::CfgReadAlarmShutdownMask ( uint_t *  pAlarmMask )

virtual

Read from the servo configuration EEPROM the current servo shutdown on alarms mask.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

EEPROM Alarm Shutdown.

Parameters

[out]

pAlarmMask

Shutdown alarm mask.

Returns

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

Implements DynaServo.

Definition at line 702 of file DynaServoGeneric.cxx.

References DYNA_ADDR_ALARM_SHUTDOWN, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uAlarmShutdown, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncCfg().

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_ALARM_SHUTDOWN, pAlarmMask);

  if( rc == DYNA_OK )
  {
    m_cfg.m_uAlarmShutdown = *pAlarmMask;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadAlarmShutdownMask(&uVal);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgReadMaxTorqueLimit ( uint_t *  pMaxTorqueLim )

virtual

Read from the servo configuration EEPROM the current on power-up maximum torque limit.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

EEPROM On Power-Up Maximum Torque Limit.
Dynamixel Torque Common Values.

Parameters

[out]

pMaxTorqueLim

Maximum torque limit.

Returns

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

Implements DynaServo.

Definition at line 634 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uLimTorqueMax, and DynaComm::Read16().

Referenced by CvtRawVoltToVolts(), and SyncCfg().

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB,
                      pMaxTorqueLim);

  if( rc == DYNA_OK )
  {
    m_cfg.m_uLimTorqueMax = *pMaxTorqueLim;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadMaxTorqueLimit(&uVal);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgReadRotationLimits ( uint_t *  pCwLim, uint_t *  pCcwLim  )

virtual

Read from the servo configuration EEPROM the current rotation limits.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

EEPROM Clockwise Angle Limit.
EEPROM Counterclockwise Angle Limit.
Dynamixel Position Common Values.

Parameters

[out]	pCwLim	Clockwise limit.
[out]	pCcwLim	Counterclockwise limit.

Returns

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

Implements DynaServo.

Definition at line 361 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_CCW_LSB, DYNA_ADDR_LIM_CW_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uLimCcw, DynaServoCfg_T::m_uLimCw, DynaComm::Read16(), and SetServoMode().

Referenced by CfgReadServoMode(), CvtRawVoltToVolts(), and SyncCfg().

{
  uint_t  uVal1, uVal2;   // working values
  int     rc;             // return code

  DYNA_TRY_COMM(m_comm);

  // Clockwise limit
  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_LIM_CW_LSB, pCwLim);

  // Counter-clockwise limit
  if( rc == DYNA_OK )
  {
    rc = m_comm.Read16(m_nServoId, DYNA_ADDR_LIM_CCW_LSB, pCcwLim);
  }

  // Update configuration and state.
  if( rc == DYNA_OK )
  {
    m_cfg.m_uLimCw  = *pCwLim;
    m_cfg.m_uLimCcw = *pCcwLim;

    SetServoMode();

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadRotationLimits(&uVal1, &uVal2);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgReadServoMode ( uint_t *  pServoMode )

virtual

Read from the servo configuration EEPROM to determine the servo operational mode.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

Dynamixel Operational Modes.
EEPROM Clockwise Angle Limit.
EEPROM Counterclockwise Angle Limit.

Parameters

[out]

pServoMode

Servo mode.

Returns

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

Implements DynaServo.

Definition at line 765 of file DynaServoGeneric.cxx.

References CfgReadRotationLimits(), DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::CrossLink_T::m_pServoMate, and DynaServoCfg_T::m_uServoMode.

Referenced by CvtRawVoltToVolts().

{
  uint_t  uCwLim;     // clockwise limit
  uint_t  uCcwLim;    // counterclockwise limit
  int     rc;         // return limit

  DYNA_TRY_COMM(m_comm);

  // mode set here
  rc = CfgReadRotationLimits(&uCwLim, &uCcwLim);

  *pServoMode = m_cfg.m_uServoMode;

  if( rc == DYNA_OK )
  {
    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadRotationLimits(&uCwLim, &uCcwLim);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgReadTemperatureLimit ( uint_t *  pTempLim )

virtual

Read from the servo configuration EEPROM the current maximum temperature limit.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

EEPROM Temperature Limit.

Parameters

[out]

pTempLim

Temperature limit.

Returns

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

Implements DynaServo.

Definition at line 469 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TEMP_MAX, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uLimTemp, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncCfg().

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_LIM_TEMP_MAX, pTempLim);

  if( rc == DYNA_OK )
  {
    m_cfg.m_uLimTemp = *pTempLim;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadTemperatureLimit(&uVal);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgReadVoltageLimits ( uint_t *  pMinVoltLim, uint_t *  pMaxVoltLim  )

virtual

Read from the servo configuration EEPROM the current voltage limits.

If this servo is the master in a linked pair of servos, both servos' configuration data are read.

Control Table:

EEPROM Minimum Voltage Limit.
EEPROM Maximum Voltage Limit.

Parameters

[out]	pMinVoltLim	Minimum voltage limit.
[out]	pMaxVoltLim	Maximum voltage limit.

Returns

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

Implements DynaServo.

Definition at line 535 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_VOLT_MAX, DYNA_ADDR_LIM_VOLT_MIN, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uLimVoltMax, DynaServoCfg_T::m_uLimVoltMin, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncCfg().

{
  uint_t  uVal1, uVal2;   // working values
  int     rc;             // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_LIM_VOLT_MIN, pMinVoltLim);

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_LIM_VOLT_MAX, pMaxVoltLim);
  }

  if( rc == DYNA_OK )
  {
    m_cfg.m_uLimVoltMin = *pMinVoltLim;
    m_cfg.m_uLimVoltMax = *pMaxVoltLim;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->CfgReadVoltageLimits(&uVal1, &uVal2);
    }
  }

  return rc;
}

int DynaServoGeneric::CfgWriteAlarmShutdownMask ( uint_t  uAlarmMask )

virtual

Write to the servo configuration EEPROM the new servo shutdown on alarms mask.

Control Table:

EEPROM Alarm Shutdown.

Parameters

uAlarmMask

Shutdown alarm mask.

Returns

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

Implements DynaServo.

Definition at line 724 of file DynaServoGeneric.cxx.

References DYNA_ADDR_ALARM_SHUTDOWN, DYNA_ADDR_ALARM_SHUTDOWN_MASK, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServoCfg_T::m_uAlarmShutdown, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  uAlarmMask &= DYNA_ADDR_ALARM_SHUTDOWN_MASK;

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_ALARM_SHUTDOWN, 1, 2,
                        m_nServoId, uAlarmMask,
                        m_link.m_pServoMate->GetServoId(), uAlarmMask);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uAlarmShutdown = uAlarmMask;
      m_link.m_pServoMate->m_cfg.m_uAlarmShutdown = uAlarmMask;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_ALARM_SHUTDOWN, uAlarmMask);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uAlarmShutdown = uAlarmMask;
    }
  }

  return rc;
}

int DynaServoGeneric::CfgWriteMaxTorqueLimit ( uint_t  uMaxTorqueLim )

virtual

Write to the servo configuration EEPROM the new on power-up maximum torque limit.

If this servo is the master in a linked pair of servos, both servos' configuration data are written.

Control Table:

EEPROM On Power-Up Maximum Torque Limit.
Dynamixel Torque Common Values.

Parameters

uMaxTorqueLim

Maximum torque limit.

Returns

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

Implements DynaServo.

Definition at line 657 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServoCfg_T::m_uLimTorqueMax, DynaServoSpec_T::m_uRawTorqueMax, DynaServoSpec_T::m_uRawTorqueMin, DynaComm::vSyncWrite(), and DynaComm::Write16().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR(((uMaxTorqueLim >= m_spec.m_uRawTorqueMin) && 
                 (uMaxTorqueLim <= m_spec.m_uRawTorqueMax)), 
      DYNA_ECODE_BAD_VAL,
      "Maximum torque limit %u: Out of range.", uMaxTorqueLim);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB, 2, 2,
                        m_nServoId, uMaxTorqueLim,
                        m_link.m_pServoMate->GetServoId(), uMaxTorqueLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimTorqueMax = uMaxTorqueLim;
      m_link.m_pServoMate->m_cfg.m_uLimTorqueMax = uMaxTorqueLim;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write16(m_nServoId, DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB,
                      uMaxTorqueLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimTorqueMax = uMaxTorqueLim;
    }
  }

  return rc;
}

int DynaServoGeneric::CfgWriteRotationLimits ( uint_t  uCwLim, uint_t  uCcwLim  )

virtual

Write to the servo configuration EEPROM the new rotation limits.

If this servo is the master in a linked pair of servos, both servos' configuration data are written.

Control Table:

EEPROM Clockwise Angle Limit. EEPROM Counterclockwise Angle Limit.
Dynamixel Position Common Values.

Parameters

uCwLim	Clockwise limit.
uCcwLim	Counterclockwise limit.

Returns

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

Implements DynaServo.

Definition at line 394 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_CCW_LSB, DYNA_ADDR_LIM_CW_LSB, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServoCfg_T::m_uLimCcw, DynaServoCfg_T::m_uLimCw, DynaServoSpec_T::m_uRawPosMax, DynaServoSpec_T::m_uRawPosMin, SetServoMode(), DynaComm::vSyncWrite(), and DynaComm::Write16().

Referenced by CfgWriteServoMode(), CfgWriteServoModeContinuous(), and CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR(((uCwLim >= m_spec.m_uRawPosMin) &&
                 (uCwLim <= m_spec.m_uRawPosMax)),
      DYNA_ECODE_BAD_VAL,
      "Clockwise limit %u: Out of range.", uCwLim);

  DYNA_TRY_EXPR(((uCcwLim >= m_spec.m_uRawPosMin) &&
                 (uCcwLim <= m_spec.m_uRawPosMax)),
      DYNA_ECODE_BAD_VAL,
      "Counterclockwise limit %u: Out of range.", uCcwLim);

  DYNA_TRY_EXPR((uCcwLim >= uCwLim), DYNA_ECODE_BAD_VAL,
      "Counterclockwise limit %u: Smaller than clockwise limit %u.",
      uCcwLim, uCwLim);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    // Clockwise limit
    rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_CW_LSB, 2, 2,
                          m_nServoId, uCwLim,
                          m_link.m_pServoMate->GetServoId(), uCwLim);

    // Counter-clockwise limit
    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimCw = uCwLim;
      m_link.m_pServoMate->m_cfg.m_uLimCw = uCwLim;

      rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_CCW_LSB, 2, 2,
                          m_nServoId, uCcwLim,
                          m_link.m_pServoMate->GetServoId(), uCcwLim);

      if( rc == DYNA_OK )
      {
        m_cfg.m_uLimCcw = uCcwLim;
        m_link.m_pServoMate->m_cfg.m_uLimCcw = uCcwLim;
      }
    }
  }

  //
  // Solitary servo
  //
  else
  {
    // Clockwise limit
    rc = m_comm.Write16(m_nServoId, DYNA_ADDR_LIM_CW_LSB, uCwLim);

    // Counter-clockwise limit
    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimCw = uCwLim;
      rc = m_comm.Write16(m_nServoId, DYNA_ADDR_LIM_CCW_LSB, uCcwLim);
    }

    // Update configuration and state.
    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimCcw = uCcwLim;
      SetServoMode();
    }
  }

  return rc;
}

int DynaServoGeneric::CfgWriteServoMode ( uint_t  uCwLim, uint_t  uCcwLim  )

virtual

Write to the servo configuration EEPROM to set the servo operational mode.

If this servo is the master in a linked pair of servos, both servos' configuration data are written.

Control Table:

Dynamixel Operational Modes.
EEPROM Clockwise Angle Limit.
EEPROM Counterclockwise Angle Limit.

Parameters

uCwLim	Clockwise limit.
uCcwLim	Counterclockwise limit.

Returns

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

Implements DynaServo.

Definition at line 789 of file DynaServoGeneric.cxx.

References CfgWriteRotationLimits(), DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, and DynaServo::m_comm.

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  return CfgWriteRotationLimits(uCwLim, uCcwLim);
}

int DynaServoGeneric::CfgWriteServoModeContinuous ( )

virtual

Write to the servo configuration EEPROM to set the servo operational mode to the full/continuous mode.

If this servo is the master in a linked pair of servos, both servos' configuration data are written.

Control Table:

Dynamixel Operational Modes.
EEPROM Clockwise Angle Limit.
EEPROM Counterclockwise Angle Limit.

Returns

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

Implements DynaServo.

Definition at line 797 of file DynaServoGeneric.cxx.

References CfgWriteRotationLimits(), DYNA_CCW_POS_CONT_MODE, DYNA_CW_POS_CONT_MODE, DYNA_MODE_CONTINUOUS, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_MODE, and DynaServo::m_comm.

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_MODE(this, DYNA_MODE_CONTINUOUS);

  return CfgWriteRotationLimits(DYNA_CW_POS_CONT_MODE, DYNA_CCW_POS_CONT_MODE);
}

int DynaServoGeneric::CfgWriteTemperatureLimit ( uint_t  uTempLim )

virtual

Write to the servo configuration EEPROM the new maximum temperature limit.

Control Table:

EEPROM Temperature Limit.

Parameters

uTempLim

Temperature limit.

Returns

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

Implements DynaServo.

Definition at line 491 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TEMP_MAX, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServoCfg_T::m_uLimTemp, DynaServoSpec_T::m_uRawTempMax, DynaServoSpec_T::m_uRawTempMin, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR(((uTempLim >= m_spec.m_uRawTempMin) &&
                 (uTempLim <= m_spec.m_uRawTempMax)), 
      DYNA_ECODE_BAD_VAL,
      "Temperature limit %u: Out of range.", uTempLim);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_TEMP_MAX, 1, 2,
                        m_nServoId, uTempLim,
                        m_link.m_pServoMate->GetServoId(), uTempLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimTemp = uTempLim;
      m_link.m_pServoMate->m_cfg.m_uLimTemp = uTempLim;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_LIM_TEMP_MAX, uTempLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimTemp = uTempLim;
    }
  }

  return rc;
}

int DynaServoGeneric::CfgWriteVoltageLimits ( uint_t  uMinVoltLim, uint_t  uMaxVoltLim  )

virtual

Write to the servo configuration EEPROM the new voltage limits.

If this servo is the master in a linked pair of servos, both servos' configuration data are written.

Control Table:

EEPROM Minimum Voltage Limit.
EEPROM Maximum Voltage Limit.

Parameters

uMinVoltLim	Minimum voltage limit.
uMaxVoltLim	Maximum voltage limit.

Returns

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

Implements DynaServo.

Definition at line 564 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_VOLT_MAX, DYNA_ADDR_LIM_VOLT_MIN, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServoCfg_T::m_uLimVoltMax, DynaServoCfg_T::m_uLimVoltMin, DynaServoSpec_T::m_uRawVoltMax, DynaServoSpec_T::m_uRawVoltMin, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR(((uMinVoltLim >= m_spec.m_uRawVoltMin) && 
                 (uMinVoltLim <= m_spec.m_uRawVoltMax)), 
      DYNA_ECODE_BAD_VAL,
      "Minimum voltage limit %u: Out of range.", uMinVoltLim);

  DYNA_TRY_EXPR(((uMaxVoltLim >= m_spec.m_uRawVoltMin) && 
                 (uMaxVoltLim <= m_spec.m_uRawVoltMax)), 
      DYNA_ECODE_BAD_VAL,
      "Maximum voltage limit %u: Out of range.", uMaxVoltLim);

  DYNA_TRY_EXPR((uMinVoltLim <= uMaxVoltLim), DYNA_ECODE_BAD_VAL,
      "Maximum voltage limit %u: Smaller than minimum voltage limit %u.",
      uMaxVoltLim, uMinVoltLim);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_VOLT_MIN, 1, 2,
                        m_nServoId, uMinVoltLim,
                        m_link.m_pServoMate->GetServoId(), uMinVoltLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimVoltMin = uMinVoltLim;
      m_link.m_pServoMate->m_cfg.m_uLimVoltMin = uMinVoltLim;

      rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_VOLT_MAX, 1, 2,
                        m_nServoId, uMaxVoltLim,
                        m_link.m_pServoMate->GetServoId(), uMaxVoltLim);
    }

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimVoltMax = uMaxVoltLim;
      m_link.m_pServoMate->m_cfg.m_uLimVoltMax = uMaxVoltLim;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_LIM_VOLT_MIN, uMinVoltLim);

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimVoltMin = uMinVoltLim;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_LIM_VOLT_MAX, uMaxVoltLim);
    }

    if( rc == DYNA_OK )
    {
      m_cfg.m_uLimVoltMax = uMaxVoltLim;
    }
  }

  return rc;
}

void DynaServoGeneric::CheckData ( )

protectedvirtual

Check data for consitencies.

Todo:

TODO check synchronized data for consistencies

Definition at line 1720 of file DynaServoGeneric.cxx.

Referenced by CvtRawVoltToVolts(), DynaServoAX12::DynaServoAX12(), DynaServoEX106P::DynaServoEX106P(), DynaServoGeneric(), DynaServoMX106::DynaServoMX106(), DynaServoMX12W::DynaServoMX12W(), DynaServoMX28::DynaServoMX28(), DynaServoMX64::DynaServoMX64(), DynaServoRX10::DynaServoRX10(), DynaServoRX24F::DynaServoRX24F(), DynaServoRX28::DynaServoRX28(), DynaServoRX64::DynaServoRX64(), and Reset().

{
  /*! \todo TODO check synchronized data for consistencies */
}

bool DynaServoGeneric::ChkComplianceSlope ( uint_t  uVal )

protectedvirtual

Check validity of compliance slope discrete values.

Parameters

uVal	Slope value.

Returns

Returns true if value is valid, false otherwise.

Definition at line 2074 of file DynaServoGeneric.cxx.

References DYNA_COMP_SLOPE_TORQUE_1, DYNA_COMP_SLOPE_TORQUE_2, DYNA_COMP_SLOPE_TORQUE_3, DYNA_COMP_SLOPE_TORQUE_4, DYNA_COMP_SLOPE_TORQUE_5, DYNA_COMP_SLOPE_TORQUE_6, and DYNA_COMP_SLOPE_TORQUE_7.

Referenced by CalcMatesGoalSpeed(), and WriteCtlMethodCompliance().

{
  switch( uVal )
  {
    case DYNA_COMP_SLOPE_TORQUE_1:
    case DYNA_COMP_SLOPE_TORQUE_2:
    case DYNA_COMP_SLOPE_TORQUE_3:
    case DYNA_COMP_SLOPE_TORQUE_4:
    case DYNA_COMP_SLOPE_TORQUE_5:
    case DYNA_COMP_SLOPE_TORQUE_6:
    case DYNA_COMP_SLOPE_TORQUE_7:
      return true;
    default:
      return false;
  }
}

virtual float DynaServoGeneric::CvtRawTempToC ( uint  uTemp )

inlinevirtual

Convert raw temperature coding to degrees Celsius.

Parameters

uTemp

Raw temperature value.

Returns

Celsius.

Implements DynaServo.

Definition at line 127 of file DynaServoGeneric.h.

  {
    return (float)uTemp;    // 1-1 mapping
  }

virtual float DynaServoGeneric::CvtRawVoltToVolts ( uint  uVolts )

inlinevirtual

Convert raw volts coding to volts.

Parameters

uTemp

Raw volts value.

Returns

Volatage.

Implements DynaServo.

Definition at line 139 of file DynaServoGeneric.h.

References AgentWriteGoalPos(), AgentWriteGoalSpeed(), AgentWriteGoalSpeedPos(), CalcMatesGoalPos(), CfgReadAlarmShutdownMask(), CfgReadMaxTorqueLimit(), CfgReadRotationLimits(), CfgReadServoMode(), CfgReadTemperatureLimit(), CfgReadVoltageLimits(), CfgWriteAlarmShutdownMask(), CfgWriteMaxTorqueLimit(), CfgWriteRotationLimits(), CfgWriteServoMode(), CfgWriteServoModeContinuous(), CfgWriteTemperatureLimit(), CfgWriteVoltageLimits(), CheckData(), Dump(), EStop(), Freeze(), Init(), InitCfg(), InitSpec(), InitState(), MoveAtSpeed(), MoveAtSpeedTo(), MoveTo(), Ping(), Read(), ReadControlMethod(), ReadCurLoad(), ReadCurPos(), ReadCurSpeed(), ReadDynamics(), ReadGoalPos(), ReadGoalSpeed(), ReadHealth(), ReadIsMoving(), ReadLed(), ReadMaxTorqueLimit(), ReadTorqueEnable(), Release(), ReloadMaxTorqueLimit(), Reset(), SetServoMode(), Stop(), SyncCfg(), SyncData(), SyncState(), Write(), WriteControlMethod(), WriteGoalPos(), WriteGoalSpeed(), WriteLed(), WriteMaxTorqueLimit(), and WriteTorqueEnable().

  {
    return (float)uVolts / 10.0;
  }

int DynaServoGeneric::EStop ( )

virtual

Emergency stop servo.

All torque is removed.

If this servo is the master in a linked pair of servos, both servos will be emergency stopped.

Returns

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

Implements DynaServo.

Definition at line 264 of file DynaServoGeneric.cxx.

References DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::m_comm, Stop(), and WriteTorqueEnable().

Referenced by CvtRawVoltToVolts().

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

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  Stop();

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

int DynaServoGeneric::Freeze ( )

virtual

Freeze servo at current position.

Torque is applied.

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

Returns

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

Implements DynaServo.

Definition at line 326 of file DynaServoGeneric.cxx.

References DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::m_comm, Stop(), and WriteTorqueEnable().

Referenced by CvtRawVoltToVolts().

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

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  rc = WriteTorqueEnable(true);

  rc_tmp = Stop();

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

  return rc;
}

int DynaServoGeneric::MoveAtSpeed ( int  nGoalSpeed )

virtual

Move at speed.

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

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

nGoalSpeed

Goal speed (raw).

Returns

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

Implements DynaServo.

Definition at line 255 of file DynaServoGeneric.cxx.

References DYNA_MODE_CONTINUOUS, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_IN_MODE, DynaServo::m_comm, and WriteGoalSpeed().

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_IN_MODE(this, DYNA_MODE_CONTINUOUS);

  return WriteGoalSpeed(nGoalSpeed);
}

int DynaServoGeneric::MoveAtSpeedTo ( 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 goal rotation in the clockwise direction, stop, or goal 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. The absolute value of the goal speed is used.

If the servo is in continuous mode, but supports 360 ° positioning data, 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

nGoalSpeed	Goal speed (raw).
nGoalOdPos	Goal position (odometer ticks).

Returns

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

Implements DynaServo.

Definition at line 235 of file DynaServoGeneric.cxx.

References AgentWriteGoalSpeedPos(), DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_POS_CTL, DynaServo::HasAgent(), DynaServo::m_comm, WriteGoalPos(), and WriteGoalSpeed().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);

  if( HasAgent() )
  {
    rc = AgentWriteGoalSpeedPos(nGoalSpeed, nGoalOdPos);
  }
  else if( (rc = WriteGoalSpeed(nGoalSpeed)) == DYNA_OK )
  {
    rc = WriteGoalPos(nGoalOdPos);
  }

  return rc;
}

int DynaServoGeneric::MoveTo ( 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

nGoalOdPos

Goal position (odometer ticks).

Returns

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

Implements DynaServo.

Definition at line 215 of file DynaServoGeneric.cxx.

References AgentWriteGoalPos(), DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DYNA_TRY_SERVO_HAS_POS_CTL, DynaServo::HasAgent(), DynaServo::m_comm, and WriteGoalPos().

Referenced by CvtRawVoltToVolts().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);

  if( HasAgent() )
  {
    rc = AgentWriteGoalPos(nGoalOdPos);
  }
  else
  {
    rc = WriteGoalPos(nGoalOdPos);
  }

  return rc;
}

uint_t DynaServoGeneric::PackGoalSpeed ( int  nGoalSpeed )

protectedvirtual

Pack the goal speed into the control table value.

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.

Parameters

nGoalSpeed

Goal speed.

Returns

Returns packed value.

Implements DynaServo.

Definition at line 2091 of file DynaServoGeneric.cxx.

References DYNA_GOAL_SPEED_DIR_CCW, DYNA_GOAL_SPEED_DIR_CW, DYNA_GOAL_SPEED_MAG_MASK, DYNA_MODE_CONTINUOUS, iabs(), DynaServo::m_cfg, and DynaServoCfg_T::m_uServoMode.

Referenced by CalcMatesGoalSpeed(), and WriteGoalSpeed().

{
  if( m_cfg.m_uServoMode == DYNA_MODE_CONTINUOUS )
  {
    return (uint_t)( (iabs(nGoalSpeed) & DYNA_GOAL_SPEED_MAG_MASK) | 
             (nGoalSpeed<0? DYNA_GOAL_SPEED_DIR_CW: DYNA_GOAL_SPEED_DIR_CCW) );
  }
  else
  {
    return (uint_t)iabs(nGoalSpeed);
  }
}

bool DynaServoGeneric::Ping ( )

virtual

Ping this servo.

Returns

Returns true if the servo responded, else false.

Implements DynaServo.

Definition at line 1493 of file DynaServoGeneric.cxx.

References DYNA_TRY_COMM, DynaServo::m_comm, DynaServo::m_nServoId, and DynaComm::Ping().

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  return m_comm.Ping(m_nServoId);
}

int DynaServoGeneric::Read ( uint_t  uAddr, uint_t *  pVal  )

virtual

Read a raw value from the servo EEPROM/RAM control table.

Warning

The shadowed configuration and state data are not update.
Linked servos are not kept in sync.

Parameters

	uAddr	Control table address.
[out]	pVal	Read raw value.

doc_return_std

Implements DynaServo.

Reimplemented in DynaServoEX106P.

Definition at line 1401 of file DynaServoGeneric.cxx.

References DYNA_ADDR_ALARM_LED, DYNA_ADDR_ALARM_SHUTDOWN, DYNA_ADDR_BAUD_RATE, DYNA_ADDR_CCW_COMP_MARGIN, DYNA_ADDR_CCW_COMP_SLOPE, DYNA_ADDR_CUR_LOAD_LSB, DYNA_ADDR_CUR_POS_LSB, DYNA_ADDR_CUR_SPEED_LSB, DYNA_ADDR_CUR_TEMP_C, DYNA_ADDR_CUR_VOLT, DYNA_ADDR_CW_COMP_MARGIN, DYNA_ADDR_CW_COMP_SLOPE, DYNA_ADDR_EEPROM_LOCK, DYNA_ADDR_FWVER, DYNA_ADDR_GOAL_POS_LSB, DYNA_ADDR_GOAL_SPEED_LSB, DYNA_ADDR_ID, DYNA_ADDR_IS_MOVING, DYNA_ADDR_LED, DYNA_ADDR_LIM_CCW_LSB, DYNA_ADDR_LIM_CW_LSB, DYNA_ADDR_LIM_TEMP_MAX, DYNA_ADDR_LIM_TORQUE_MAX_LSB, DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB, DYNA_ADDR_LIM_VOLT_MAX, DYNA_ADDR_LIM_VOLT_MIN, DYNA_ADDR_MODEL_NUM_LSB, DYNA_ADDR_PUNCH_LSB, DYNA_ADDR_REG_INSTR, DYNA_ADDR_SRL, DYNA_ADDR_T_RET_DELAY, DYNA_ADDR_TORQUE_EN, DYNA_TRY_ADDR, DYNA_TRY_COMM, DynaServo::m_comm, DynaServo::m_nServoId, DynaComm::Read16(), and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and DynaServoEX106P::Read().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_ADDR(uAddr);

  switch( uAddr )
  {
    case DYNA_ADDR_MODEL_NUM_LSB:
    case DYNA_ADDR_LIM_CW_LSB:
    case DYNA_ADDR_LIM_CCW_LSB:
    case DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB:
    case DYNA_ADDR_GOAL_POS_LSB:
    case DYNA_ADDR_GOAL_SPEED_LSB:
    case DYNA_ADDR_LIM_TORQUE_MAX_LSB:
    case DYNA_ADDR_CUR_POS_LSB:
    case DYNA_ADDR_CUR_SPEED_LSB:
    case DYNA_ADDR_CUR_LOAD_LSB:
      return m_comm.Read16(m_nServoId, uAddr, pVal);

    case DYNA_ADDR_FWVER:
    case DYNA_ADDR_ID:
    case DYNA_ADDR_BAUD_RATE:
    case DYNA_ADDR_T_RET_DELAY:
    case DYNA_ADDR_LIM_TEMP_MAX:
    case DYNA_ADDR_LIM_VOLT_MIN:
    case DYNA_ADDR_LIM_VOLT_MAX:
    case DYNA_ADDR_SRL:
    case DYNA_ADDR_ALARM_LED:
    case DYNA_ADDR_ALARM_SHUTDOWN:
    case DYNA_ADDR_TORQUE_EN:
    case DYNA_ADDR_LED:
    case DYNA_ADDR_CW_COMP_MARGIN:
    case DYNA_ADDR_CCW_COMP_MARGIN:
    case DYNA_ADDR_CW_COMP_SLOPE:
    case DYNA_ADDR_CCW_COMP_SLOPE:
    case DYNA_ADDR_CUR_VOLT:
    case DYNA_ADDR_CUR_TEMP_C:
    case DYNA_ADDR_REG_INSTR:
    case DYNA_ADDR_IS_MOVING:
    case DYNA_ADDR_EEPROM_LOCK:
    case DYNA_ADDR_PUNCH_LSB:
    default:
      return m_comm.Read8(m_nServoId, uAddr, pVal);
  }
}

int DynaServoGeneric::ReadControlMethod ( DynaServoCtlMethod_T *  pCtlMethod )

virtual

Read from the servo state memory the current control method parameters.

The control method used is determined by the servo's specification.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

Dynamixel Internal Position Control Methods.
RAM Clockwise Compliance Margin.
RAM Counterclockwise Compliance Margin.
RAM Clockwise Compliance Slope.
RAM Counterclockwise Compliance Slope.
RAM PID P Gain.
RAM PID I Gain.
RAM PID D Gain.

Parameters

[out]

pCtlMethod

Control method parameters.

Returns

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

Implements DynaServo.

Definition at line 940 of file DynaServoGeneric.cxx.

References DYNA_CTL_METHOD_COMPLIANCE, DYNA_CTL_METHOD_PID, DYNA_ECODE_INTERNAL, DYNA_LOG_ERROR, DYNA_TRY_COMM, DynaServo::m_comm, DynaServo::m_nServoId, DynaServo::m_spec, DynaServoSpec_T::m_uCtlMethodUsed, ReadCtlMethodCompliance(), and ReadCtlMethodPid().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);

  switch( m_spec.m_uCtlMethodUsed )
  {
    case DYNA_CTL_METHOD_COMPLIANCE:
      return ReadCtlMethodCompliance(pCtlMethod);
    case DYNA_CTL_METHOD_PID:
      return ReadCtlMethodPid(pCtlMethod);
    default:
      rc = -DYNA_ECODE_INTERNAL;
      DYNA_LOG_ERROR(rc, "Servo %d: Unknown control method %u.",
          m_nServoId, m_spec.m_uCtlMethodUsed);
      return rc;
  }
}

int DynaServoGeneric::ReadCtlMethodCompliance ( DynaServoCtlMethod_T *  pCtlMethod )

protectedvirtual

Read from the servo state memory the current compliance control method parameters.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

Dynamixel Internal Position Control Methods.
RAM Clockwise Compliance Margin.
RAM Counterclockwise Compliance Margin.
RAM Clockwise Compliance Slope.
RAM Counterclockwise Compliance Slope.

Parameters

[out]

pCtlMethod

Control method parameters.

Returns

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

Definition at line 1773 of file DynaServoGeneric.cxx.

References DYNA_ADDR_CCW_COMP_MARGIN, DYNA_ADDR_CCW_COMP_SLOPE, DYNA_ADDR_CW_COMP_MARGIN, DYNA_ADDR_CW_COMP_SLOPE, DYNA_CTL_METHOD_COMPLIANCE, DYNA_OK, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServoCtlMethod_T::m_comp, DynaServoState_T::m_ctlMethod, DynaServo::m_link, DynaServo::m_nServoId, DynaServoCtlMethod_T::m_params, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoCtlMethod_T::m_uCtlMethod, and DynaComm::Read8().

Referenced by CalcMatesGoalSpeed(), and ReadControlMethod().

{
  DynaServoCtlMethod_T *pVal; // working value
  int                   rc;   // return code

  pCtlMethod->m_uCtlMethod = DYNA_CTL_METHOD_COMPLIANCE;

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_CW_COMP_MARGIN,
                            &(pCtlMethod->m_params.m_comp.m_uCwMargin));

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_CCW_COMP_MARGIN,
                            &(pCtlMethod->m_params.m_comp.m_uCcwMargin));
  }

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_CW_COMP_SLOPE,
                            &(pCtlMethod->m_params.m_comp.m_uCwSlope));
  }

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_CCW_COMP_SLOPE,
                            &(pCtlMethod->m_params.m_comp.m_uCcwSlope));
  }

  if( rc == DYNA_OK )
  {
    m_state.m_ctlMethod = *pCtlMethod;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadControlMethod(pVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadCtlMethodPid ( DynaServoCtlMethod_T *  pCtlMethod )

protectedvirtual

Read from the servo state memory the current PID control method parameters.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

Dynamixel Internal Position Control Methods.
RAM PID P Gain.
RAM PID I Gain.
RAM PID D Gain.

Parameters

[out]

pCtlMethod

Control method parameters.

Returns

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

Definition at line 1939 of file DynaServoGeneric.cxx.

References DYNA_ADDR_D_GAIN, DYNA_ADDR_I_GAIN, DYNA_ADDR_P_GAIN, DYNA_CTL_METHOD_PID, DYNA_OK, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServoState_T::m_ctlMethod, DynaServo::m_link, DynaServo::m_nServoId, DynaServoCtlMethod_T::m_params, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoCtlMethod_T::m_uCtlMethod, and DynaComm::Read8().

Referenced by CalcMatesGoalSpeed(), and ReadControlMethod().

{
  DynaServoCtlMethod_T *pVal; // working value
  int                   rc;   // return code

  pCtlMethod->m_uCtlMethod = DYNA_CTL_METHOD_PID;

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_P_GAIN,
                            &(pCtlMethod->m_params.m_pid.m_uPGain));

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_I_GAIN,
                            &(pCtlMethod->m_params.m_pid.m_uIGain));
  }

  if( rc == DYNA_OK )
  {
    rc = m_comm.Read8(m_nServoId, DYNA_ADDR_D_GAIN,
                            &(pCtlMethod->m_params.m_pid.m_uDGain));
  }

  if( rc == DYNA_OK )
  {
    m_state.m_ctlMethod = *pCtlMethod;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadControlMethod(pVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadCurLoad ( int *  pCurLoad )

virtual

Read from the servo state memory the current load.

Loads are in the range [-max,max] where the values < 0, 0, and > 0 specify torques loads in clockwise direction, no torque load, or torque loads in the counterclockwise direction, respectively. Load is approximately 0.1% of maximum.

Control Table:

RAM Current Load.

Parameters

[out]

pCurLoad

Current load.

Returns

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

Implements DynaServo.

Definition at line 1275 of file DynaServoGeneric.cxx.

References DYNA_ADDR_CUR_LOAD_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServoState_T::m_nCurLoad, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaComm::Read16(), and UnpackCurLoad().

Referenced by CvtRawVoltToVolts(), ReadDynamics(), and ReadHealth().

{
  uint_t  uVal;   // working value
  int     nVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_CUR_LOAD_LSB, &uVal);
  
  if( rc == DYNA_OK )
  {
    m_state.m_nCurLoad = UnpackCurLoad(uVal);
    *pCurLoad = m_state.m_nCurLoad;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadCurLoad(&nVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadCurPos ( int *  pCurOdPos )

virtual

Read from the servo state memory the current servo position.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Current Position.
Dynamixel Position Common Values.

Parameters

[out]

pCurOdPos

Odometer current position (ticks).

Returns

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

Implements DynaServo.

Definition at line 1222 of file DynaServoGeneric.cxx.

References DYNA_ADDR_CUR_POS_LSB, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_SERVO_HAS_POS_CTL, DynaServo::GetOdometer(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoState_T::m_uCurPos, DynaComm::Read16(), and DynaServo::UpdateOdometer().

Referenced by CalcMatesGoalPos(), CvtRawVoltToVolts(), ReadDynamics(), Stop(), and SyncState().

{
  uint_t  uVal;   // working value
  int     nVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_SERVO_HAS_POS_CTL(this);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_CUR_POS_LSB, &uVal);

  if( rc == DYNA_OK )
  {
    m_state.m_uCurPos = uVal;
    
    UpdateOdometer((int)uVal);

    *pCurOdPos = GetOdometer();

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadCurPos(&nVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadCurSpeed ( int *  pCurSpeed )

virtual

Read from the servo state memory the current speed and direction.

Speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify the servo is rotating in the clockwise direction, the servo is stopped, or the servo is rotating in the counterclockwise direction, respectively.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Current Speed and Direction.
Dynamixel Angular Speed Common Values.
Dynamixel Direction Common Values.

Parameters

[out]

pCurSpeed

Current speed.

Returns

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

Implements DynaServo.

Definition at line 1250 of file DynaServoGeneric.cxx.

References DYNA_ADDR_CUR_SPEED_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServoState_T::m_nCurSpeed, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaComm::Read16(), and UnpackCurSpeed().

Referenced by CvtRawVoltToVolts(), ReadDynamics(), and SyncState().

{
  uint_t  uVal;   // working value
  int     nVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_CUR_SPEED_LSB, &uVal);

  if( rc == DYNA_OK )
  {

    m_state.m_nCurSpeed = UnpackCurSpeed(uVal);
    *pCurSpeed = m_state.m_nCurSpeed;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadCurSpeed(&nVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadDynamics ( int *  pCurPos, int *  pCurSpeed, int *  pCurLoad  )

virtual

Read from the servo state memory the current servo dynamics.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Current Position.
RAM Current Speed and Direction.
RAM Current Load.

Parameters

[out]	pCurPos	Current position.
[out]	pCurSpeed	Current speed.
[out]	pCurLoad	Current load.

Returns

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

Implements DynaServo.

Definition at line 1299 of file DynaServoGeneric.cxx.

References DYNA_OK, DYNA_TRY_COMM, DynaServo::m_comm, ReadCurLoad(), ReadCurPos(), and ReadCurSpeed().

Referenced by CvtRawVoltToVolts().

{
  int     rc, rc_tmp;                   // return codes

  DYNA_TRY_COMM(m_comm);

  rc = ReadCurPos(pCurPos);

  rc_tmp = ReadCurSpeed(pCurSpeed);

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

  rc_tmp = ReadCurLoad(pCurLoad);

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

  return rc;
}

int DynaServoGeneric::ReadGoalPos ( int *  pGoalPos )

virtual

Read from the servo state memory the current goal position.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Goal Position.
Dynamixel Position Common Values.

Parameters

[out]

pGoalPos

Servo Mode: Encoder goal position. Continuous Mode: Odometer goal position.

Returns

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

Implements DynaServo.

Definition at line 981 of file DynaServoGeneric.cxx.

References DYNA_ADDR_GOAL_POS_LSB, DYNA_MODE_CONTINUOUS, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServoOdometer_T::m_nOdGoalPos, DynaServo::m_nServoId, DynaServoState_T::m_od, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoState_T::m_uGoalPos, DynaServoCfg_T::m_uServoMode, and DynaComm::Read16().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;       // working value
  int     nValMate;   // working mate's value
  int     rc;         // return code

  DYNA_TRY_COMM(m_comm);

  if( m_cfg.m_uServoMode == DYNA_MODE_CONTINUOUS )
  {
    *pGoalPos = m_state.m_od.m_nOdGoalPos;
    rc = DYNA_OK;
  }
  else
  {
    rc = m_comm.Read16(m_nServoId, DYNA_ADDR_GOAL_POS_LSB, &uVal);

    if( rc == DYNA_OK )
    {
      m_state.m_uGoalPos = uVal;
      *pGoalPos = uVal;

      if( IsLinkedMaster() )
      {
        rc = m_link.m_pServoMate->ReadGoalPos(&nValMate);
      }
    }
  }

  return rc;
}

int DynaServoGeneric::ReadGoalSpeed ( int *  pGoalSpeed )

virtual

Read from the servo state memory the current goal speed and direction.

For servos in continuous mode, speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify goal rotation in the clockwise direction, stop, or goal 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' state data are read.

Control Table:

RAM Goal Speed and Direction.
Dynamixel Angular Speed Common Values.
Dynamixel Direction Common Values.

Parameters

[out]

pGoalSpeed

Goal speed.

Returns

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

Implements DynaServo.

Definition at line 1072 of file DynaServoGeneric.cxx.

References DYNA_ADDR_GOAL_SPEED_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServoState_T::m_nGoalSpeed, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaComm::Read16(), and UnpackGoalSpeed().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;     // working value
  int     nVal;     // working value
  int     rc;       // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_GOAL_SPEED_LSB, &uVal);

  if( rc == DYNA_OK )
  {
    m_state.m_nGoalSpeed = UnpackGoalSpeed(uVal);
    *pGoalSpeed = m_state.m_nGoalSpeed;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadGoalSpeed(&nVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadHealth ( uint_t *  pAlarms, int *  pCurLoad, uint_t *  pCurVolt, uint_t *  pCurTemp  )

virtual

Read from the servo state memory the current servo health.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Current Load.
RAM Current Voltage.
RAM Current Temperature.

Parameters

[out]	pAlarms	Servo alarms.
[out]	pCurLoad	Current load.
[out]	pCurVolt	Current voltage.
[out]	pCurTemp	Current temperature.

Returns

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

Implements DynaServo.

Definition at line 1326 of file DynaServoGeneric.cxx.

References DYNA_ADDR_CUR_TEMP_C, DYNA_ADDR_CUR_VOLT, DYNA_OK, DYNA_TRY_COMM, DynaComm::GetAlarms(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoState_T::m_uAlarms, DynaServoState_T::m_uCurTemp, DynaServoState_T::m_uCurVolt, DynaComm::Read8(), and ReadCurLoad().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal1;            // working value
  int     nVal2;            // woking value
  uint_t  uVal3, uVal4;     // working values
  int     rc, rc_tmp;       // return codes

  DYNA_TRY_COMM(m_comm);

  rc = ReadCurLoad(pCurLoad);
  
  rc_tmp = m_comm.Read8(m_nServoId, DYNA_ADDR_CUR_VOLT, pCurVolt);

  if( rc_tmp == DYNA_OK )
  {
    m_state.m_uCurVolt = *pCurVolt;
  }
  else if( rc == DYNA_OK )
  {
    rc = rc_tmp;
  }

  rc_tmp = m_comm.Read8(m_nServoId, DYNA_ADDR_CUR_TEMP_C, pCurTemp);

  if( rc_tmp == DYNA_OK )
  {
    m_state.m_uCurTemp = *pCurTemp;
  }
  else if( rc == DYNA_OK )
  {
    rc = rc_tmp;
  }

  if( rc == DYNA_OK )
  {
    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadHealth(&uVal1, &nVal2, &uVal3, &uVal4);
    }
  }

  m_state.m_uAlarms = m_comm.GetAlarms(); 
  *pAlarms = m_state.m_uAlarms;

  return rc;
}

int DynaServoGeneric::ReadIsMoving ( bool *  pState )

virtual

Read from the servo state memory to test if the servo is currently moving.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Servo Is Moving.

Parameters

[out]

pState

Servo is [not] moving (true/false) state.

Returns

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

Implements DynaServo.

Definition at line 1376 of file DynaServoGeneric.cxx.

References DYNA_ADDR_IS_MOVING, DYNA_IS_NOT_MOVING, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServoState_T::m_bIsMoving, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;   // working value
  bool    bVal;   // working boolean
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_IS_MOVING, &uVal);

  if( rc == DYNA_OK )
  {
    *pState = uVal==DYNA_IS_NOT_MOVING? false: true;

    m_state.m_bIsMoving = *pState;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadIsMoving(&bVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadLed ( bool *  pState )

virtual

Read from the servo state memory the current LED on/off value.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM LED On/Off.

Parameters

[out]

pState

On (true) or off (false) state.

Returns

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

Implements DynaServo.

Definition at line 873 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LED, DYNA_LED_OFF, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServoState_T::m_bLed, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;   // working value
  bool    bVal;   // working boolean
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_LED, &uVal);

  if( rc == DYNA_OK )
  {
    *pState = uVal==DYNA_LED_OFF? false: true;

    m_state.m_bLed = *pState;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadLed(&bVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadMaxTorqueLimit ( uint_t *  pMaxTorqueLim )

virtual

Read from the servo state memory the current maximum torque limit.

The servo will initially set this value to the value in the RAM Maximum Torque Limit configuration. The servo will set this value to 0 on an alarm shutdown event.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Maximum Torque Limit.
Dynamixel Torque Common Values.

Parameters

[out]

pMaxTorqueLim

Maximum torque limit.

Returns

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

Implements DynaServo.

Definition at line 1147 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TORQUE_MAX_LSB, DYNA_OK, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaServoState_T::m_uLimTorqueMax, and DynaComm::Read16().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read16(m_nServoId, DYNA_ADDR_LIM_TORQUE_MAX_LSB, pMaxTorqueLim);

  if( rc == DYNA_OK )
  {
    m_state.m_uLimTorqueMax = *pMaxTorqueLim;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadMaxTorqueLimit(&uVal);
    }
  }

  return rc;
}

int DynaServoGeneric::ReadTorqueEnable ( bool *  pState )

virtual

Read from the servo state memory the current torque enable value.

If this servo is the master in a linked pair of servos, both servos' state data are read.

Control Table:

RAM Torque Enable.

Parameters

[out]

pState

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.

Implements DynaServo.

Definition at line 806 of file DynaServoGeneric.cxx.

References DYNA_ADDR_TORQUE_EN, DYNA_OK, DYNA_TORQUE_EN_OFF, DYNA_TRY_COMM, DynaServo::IsLinkedMaster(), DynaServoState_T::m_bTorqueEnabled, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), and SyncState().

{
  uint_t  uVal;   // working value
  bool    bVal;   // working boolean
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);

  rc = m_comm.Read8(m_nServoId, DYNA_ADDR_TORQUE_EN, &uVal);

  if( rc == DYNA_OK )
  {
    *pState = uVal==DYNA_TORQUE_EN_OFF? false: true;

    m_state.m_bTorqueEnabled = *pState;

    if( IsLinkedMaster() )
    {
      rc = m_link.m_pServoMate->ReadTorqueEnable(&bVal);
    }
  }

  return rc;
}

int DynaServoGeneric::Release ( )

virtual

Release servo from any applied torque.

Servo is free to rotate.

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

Returns

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

Implements DynaServo.

Definition at line 346 of file DynaServoGeneric.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::m_comm, Stop(), and WriteTorqueEnable().

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  Stop();

  return WriteTorqueEnable(false);
}

int DynaServoGeneric::ReloadMaxTorqueLimit ( )

virtual

Reload the maximum torque limit from the configuration.

If this servo is the master in a linked pair of servos, both servos' state data are reloaded.

Control Table:

EEPROM On Power-Up Maximum Torque Limit.
RAM Maximum Torque Limit.

Returns

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

Implements DynaServo.

Definition at line 1214 of file DynaServoGeneric.cxx.

References DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::m_cfg, DynaServo::m_comm, DynaServoCfg_T::m_uLimTorqueMax, and WriteMaxTorqueLimit().

Referenced by CvtRawVoltToVolts().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  return WriteMaxTorqueLimit(m_cfg.m_uLimTorqueMax);
}

int DynaServoGeneric::Reset ( )

virtual

Reset this servo back to default values.

Warning

All configuration is lost.

Implements DynaServo.

Definition at line 1499 of file DynaServoGeneric.cxx.

References CheckData(), DYNA_OK, DYNA_TRY_COMM, DynaServo::m_comm, DynaServo::m_nServoId, DynaComm::Reset(), and SyncData().

Referenced by CvtRawVoltToVolts().

{
  int   rc;

  DYNA_TRY_COMM(m_comm);

  if( (rc = m_comm.Reset(m_nServoId)) == DYNA_OK )
  {
    SyncData();
    CheckData();
  }
}

void DynaServoGeneric::SetServoMode ( )

protected

Set the servo mode given the servo capabilites and the current rotation limits.

See Dynamixel Operational Modes.

Definition at line 2149 of file DynaServoGeneric.cxx.

References DYNA_CCW_POS_CONT_MODE, DYNA_CW_POS_CONT_MODE, DYNA_MODE_CONTINUOUS, DYNA_MODE_SERVO, DynaServo::m_cfg, DynaServo::m_spec, DynaServoCfg_T::m_uLimCcw, DynaServoCfg_T::m_uLimCw, DynaServoCfg_T::m_uServoMode, and DynaServoSpec_T::m_uSupportedModes.

Referenced by CfgReadRotationLimits(), CfgWriteRotationLimits(), and CvtRawVoltToVolts().

{
  if( (m_spec.m_uSupportedModes & DYNA_MODE_CONTINUOUS) &&
      (m_cfg.m_uLimCw  == DYNA_CW_POS_CONT_MODE) &&
      (m_cfg.m_uLimCcw == DYNA_CCW_POS_CONT_MODE) )
  {
    m_cfg.m_uServoMode = DYNA_MODE_CONTINUOUS;
  }

  else
  {
    m_cfg.m_uServoMode = DYNA_MODE_SERVO;
  }
}

int DynaServoGeneric::Stop ( )

virtual

Stop servo.

Current torque levels are kept.

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

Returns

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

Implements DynaServo.

Definition at line 285 of file DynaServoGeneric.cxx.

References AgentWriteGoalSpeed(), DYNA_MODE_CONTINUOUS, DYNA_MODE_SERVO, DYNA_OK, DYNA_SPEED_CONT_STOP, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::GetServoMode(), DynaServo::HasAgent(), DynaServo::m_comm, ReadCurPos(), WriteGoalPos(), and WriteGoalSpeed().

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

{
  int     nCurPos;    // current position
  int     rc;         // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  if( HasAgent() )
  {
    rc = AgentWriteGoalSpeed(DYNA_SPEED_CONT_STOP);
  }

  else
  {
    switch( GetServoMode() )
    {
      //
      // In continuous mode: Set the goal speed to zero.
      //
      case DYNA_MODE_CONTINUOUS:
        rc = WriteGoalSpeed(DYNA_SPEED_CONT_STOP);
        break;

      //
      // In servo mode: Set the goal position to the current position (a 0 speed
      // value sets the speed to the default maximum).
      //
      case DYNA_MODE_SERVO:
      default:
        if( (rc = ReadCurPos(&nCurPos)) == DYNA_OK )
        {
          rc = WriteGoalPos(nCurPos);
        }
        break;
    }
  }

  return rc;
}

int DynaServoGeneric::SyncCfg ( )

virtual

Synchronize the shadowed configuration to the servo control table EEPROM configuration.

Returns

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

Implements DynaServo.

Reimplemented in DynaServoEX106P.

Definition at line 1522 of file DynaServoGeneric.cxx.

References CfgReadAlarmShutdownMask(), CfgReadMaxTorqueLimit(), CfgReadRotationLimits(), CfgReadTemperatureLimit(), CfgReadVoltageLimits(), DYNA_ADDR_ALARM_LED, DYNA_ADDR_SRL, DYNA_ADDR_T_RET_DELAY, DYNA_OK, DYNA_TRY_COMM, DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_nServoId, DynaServoCfg_T::m_uAlarmLed, DynaServoCfg_T::m_uSrl, DynaServoCfg_T::m_uTRetDelay, and DynaComm::Read8().

Referenced by CvtRawVoltToVolts(), DynaServoEX106P::SyncCfg(), and SyncData().

{
  uint_t  uVal1;
  uint_t  uVal2;

  DYNA_TRY_COMM(m_comm);

  if( m_comm.Read8(m_nServoId, DYNA_ADDR_T_RET_DELAY, &uVal1) == DYNA_OK )
  {
    m_cfg.m_uTRetDelay = uVal1;
  }

  CfgReadRotationLimits(&uVal1, &uVal2);
  CfgReadTemperatureLimit(&uVal1);
  CfgReadVoltageLimits(&uVal1, &uVal2);
  CfgReadMaxTorqueLimit(&uVal1);

  if( m_comm.Read8(m_nServoId, DYNA_ADDR_SRL, &uVal1) == DYNA_OK )
  {
    m_cfg.m_uSrl = uVal1;
  }

  if( m_comm.Read8(m_nServoId, DYNA_ADDR_ALARM_LED, &uVal1) == DYNA_OK )
  {
    m_cfg.m_uAlarmLed = uVal1;
  }

  CfgReadAlarmShutdownMask(&uVal1);

  return DYNA_OK;
}

int DynaServoGeneric::SyncData ( )

virtual

Synchronize the shadowed configuration and state data to the servo control table.

Returns

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

Implements DynaServo.

Definition at line 1512 of file DynaServoGeneric.cxx.

References SyncCfg(), and SyncState().

Referenced by CvtRawVoltToVolts(), DynaServoAX12::DynaServoAX12(), DynaServoEX106P::DynaServoEX106P(), DynaServoGeneric(), DynaServoMX106::DynaServoMX106(), DynaServoMX12W::DynaServoMX12W(), DynaServoMX28::DynaServoMX28(), DynaServoMX64::DynaServoMX64(), DynaServoRX10::DynaServoRX10(), DynaServoRX24F::DynaServoRX24F(), DynaServoRX28::DynaServoRX28(), DynaServoRX64::DynaServoRX64(), and Reset().

{
  int   rc;

  rc = SyncCfg();
  rc = SyncState();

  return rc;
}

int DynaServoGeneric::SyncState ( )

virtual

Synchronize the shadowed state data to the servo control table RAM state.

Returns

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

Implements DynaServo.

Reimplemented in DynaServoEX106P.

Definition at line 1554 of file DynaServoGeneric.cxx.

References DYNA_LOG_SERVO_ALARMS, DYNA_OK, DYNA_TRY_COMM, DynaServo::m_comm, DynaServoState_T::m_ctlMethod, DynaServo::m_nServoId, DynaServo::m_state, DynaServoState_T::m_uAlarms, ReadControlMethod(), ReadCurPos(), ReadCurSpeed(), ReadGoalPos(), ReadGoalSpeed(), ReadHealth(), ReadIsMoving(), ReadLed(), ReadMaxTorqueLimit(), ReadTorqueEnable(), and DynaServo::ResetOdometer().

Referenced by CvtRawVoltToVolts(), SyncData(), and DynaServoEX106P::SyncState().

{
  bool    bState;
  uint_t  uVal1, uVal3, uVal4, uVal5;
  int     nVal2;
  
  DYNA_TRY_COMM(m_comm);

  ReadTorqueEnable(&bState);
  ReadLed(&bState);
  ReadControlMethod(&m_state.m_ctlMethod);
  ReadGoalPos(&nVal2);
  ReadGoalSpeed(&nVal2);
  ReadMaxTorqueLimit(&uVal1);
  ReadCurPos(&nVal2);
  ReadCurSpeed(&nVal2);
  ReadHealth(&uVal1, &nVal2, &uVal3, &uVal4);
  ReadIsMoving(&bState);

  // reset odometer to align with encoder (odometer is also enabled)
  ResetOdometer(0, false);
  
  // log any servo alarms
  DYNA_LOG_SERVO_ALARMS(m_nServoId, m_state.m_uAlarms);

  return DYNA_OK;
}

int DynaServoGeneric::UnpackCurLoad ( uint_t  uVal )

protectedvirtual

Unpack current load estimate from the control table value.

Loads are in the range [-max,max] where the values < 0, 0, and > 0 specify torques loads in clockwise direction, no torque load, or torque loads in the counterclockwise direction, respectively.

Parameters

uVal	Packed field.

Returns

Current load.

Implements DynaServo.

Definition at line 2135 of file DynaServoGeneric.cxx.

References DYNA_CUR_LOAD_DIR_CW, DYNA_CUR_LOAD_DIR_MASK, and DYNA_CUR_LOAD_MAG_MASK.

Referenced by CalcMatesGoalSpeed(), and ReadCurLoad().

{
  int nLoad;

  nLoad = (int)(uVal & DYNA_CUR_LOAD_MAG_MASK);

  if( (uVal & DYNA_CUR_LOAD_DIR_MASK) == DYNA_CUR_LOAD_DIR_CW )
  {
    nLoad = -nLoad;
  }

  return nLoad;
}

int DynaServoGeneric::UnpackCurSpeed ( uint_t  uVal )

protectedvirtual

Unpack current speed from the control table value.

Speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify the servo is rotating in the clockwise direction, the servo is stopped, or the servo is rotating in the counterclockwise direction, respectively.

Parameters

uVal	Packed field.

Returns

Current speed.

Implements DynaServo.

Definition at line 2121 of file DynaServoGeneric.cxx.

References DYNA_CUR_SPEED_DIR_CW, DYNA_CUR_SPEED_DIR_MASK, and DYNA_CUR_SPEED_MAG_MASK.

Referenced by CalcMatesGoalSpeed(), and ReadCurSpeed().

{
  int nSpeed;

  nSpeed = (int)(uVal & DYNA_CUR_SPEED_MAG_MASK);

  if( (uVal & DYNA_CUR_SPEED_DIR_MASK) == DYNA_CUR_SPEED_DIR_CW )
  {
    nSpeed = -nSpeed;
  }

  return nSpeed;
}

int DynaServoGeneric::UnpackGoalSpeed ( uint_t  uVal )

protectedvirtual

Unpack goal speed from the control table value.

For servos in continuous mode, speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify the servo is rotating in the clockwise direction, the servo is stopped, or the servo is rotating 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.

Parameters

uVal	Packed field.

Returns

Goal speed.

Implements DynaServo.

Definition at line 2104 of file DynaServoGeneric.cxx.

References DYNA_GOAL_SPEED_DIR_CW, DYNA_GOAL_SPEED_DIR_MASK, DYNA_GOAL_SPEED_MAG_MASK, DYNA_MODE_CONTINUOUS, DynaServo::m_cfg, and DynaServoCfg_T::m_uServoMode.

Referenced by CalcMatesGoalSpeed(), and ReadGoalSpeed().

{
  int nSpeed;

  nSpeed = (int)(uVal & DYNA_GOAL_SPEED_MAG_MASK);

  if( m_cfg.m_uServoMode == DYNA_MODE_CONTINUOUS )
  {
    if( (uVal & DYNA_GOAL_SPEED_DIR_MASK) == DYNA_GOAL_SPEED_DIR_CW )
    {
      nSpeed = -nSpeed;
    }
  }

  return nSpeed;
}

int DynaServoGeneric::Write ( uint_t  uAddr, uint_t  uVal  )

virtual

Write a raw value to the servo EEPROM/RAM control table.

Warning

The shadowed configuration and state data are not updated and will hence be out of sync.

Any linked master-slave servos may get out of sync and may result in physical damage.

Parameters

uAddr	Control table address.
uVal	Raw value to write.

doc_return_std

Implements DynaServo.

Reimplemented in DynaServoEX106P.

Definition at line 1447 of file DynaServoGeneric.cxx.

References DYNA_ADDR_ALARM_LED, DYNA_ADDR_ALARM_SHUTDOWN, DYNA_ADDR_BAUD_RATE, DYNA_ADDR_CCW_COMP_MARGIN, DYNA_ADDR_CCW_COMP_SLOPE, DYNA_ADDR_CUR_LOAD_LSB, DYNA_ADDR_CUR_POS_LSB, DYNA_ADDR_CUR_SPEED_LSB, DYNA_ADDR_CUR_TEMP_C, DYNA_ADDR_CUR_VOLT, DYNA_ADDR_CW_COMP_MARGIN, DYNA_ADDR_CW_COMP_SLOPE, DYNA_ADDR_EEPROM_LOCK, DYNA_ADDR_FWVER, DYNA_ADDR_GOAL_POS_LSB, DYNA_ADDR_GOAL_SPEED_LSB, DYNA_ADDR_ID, DYNA_ADDR_IS_MOVING, DYNA_ADDR_LED, DYNA_ADDR_LIM_CCW_LSB, DYNA_ADDR_LIM_CW_LSB, DYNA_ADDR_LIM_TEMP_MAX, DYNA_ADDR_LIM_TORQUE_MAX_LSB, DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB, DYNA_ADDR_LIM_VOLT_MAX, DYNA_ADDR_LIM_VOLT_MIN, DYNA_ADDR_MODEL_NUM_LSB, DYNA_ADDR_PUNCH_LSB, DYNA_ADDR_REG_INSTR, DYNA_ADDR_SRL, DYNA_ADDR_T_RET_DELAY, DYNA_ADDR_TORQUE_EN, DYNA_TRY_ADDR, DYNA_TRY_COMM, DynaServo::m_comm, DynaServo::m_nServoId, DynaComm::Write16(), and DynaComm::Write8().

Referenced by CvtRawVoltToVolts(), and DynaServoEX106P::Write().

{
  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_ADDR(uAddr);

  switch( uAddr )
  {
    case DYNA_ADDR_MODEL_NUM_LSB:
    case DYNA_ADDR_LIM_CW_LSB:
    case DYNA_ADDR_LIM_CCW_LSB:
    case DYNA_ADDR_LIM_TORQUE_MAX_ON_LSB:
    case DYNA_ADDR_GOAL_POS_LSB:
    case DYNA_ADDR_GOAL_SPEED_LSB:
    case DYNA_ADDR_LIM_TORQUE_MAX_LSB:
    case DYNA_ADDR_CUR_POS_LSB:
    case DYNA_ADDR_CUR_SPEED_LSB:
    case DYNA_ADDR_CUR_LOAD_LSB:
      return m_comm.Write16(m_nServoId, uAddr, uVal);

    case DYNA_ADDR_FWVER:
    case DYNA_ADDR_ID:
    case DYNA_ADDR_BAUD_RATE:
    case DYNA_ADDR_T_RET_DELAY:
    case DYNA_ADDR_LIM_TEMP_MAX:
    case DYNA_ADDR_LIM_VOLT_MIN:
    case DYNA_ADDR_LIM_VOLT_MAX:
    case DYNA_ADDR_SRL:
    case DYNA_ADDR_ALARM_LED:
    case DYNA_ADDR_ALARM_SHUTDOWN:
    case DYNA_ADDR_TORQUE_EN:
    case DYNA_ADDR_LED:
    case DYNA_ADDR_CW_COMP_MARGIN:
    case DYNA_ADDR_CCW_COMP_MARGIN:
    case DYNA_ADDR_CW_COMP_SLOPE:
    case DYNA_ADDR_CCW_COMP_SLOPE:
    case DYNA_ADDR_CUR_VOLT:
    case DYNA_ADDR_CUR_TEMP_C:
    case DYNA_ADDR_REG_INSTR:
    case DYNA_ADDR_IS_MOVING:
    case DYNA_ADDR_EEPROM_LOCK:
    case DYNA_ADDR_PUNCH_LSB:
    default:
      return m_comm.Write8(m_nServoId, uAddr, uVal);
  }
}

int DynaServoGeneric::WriteControlMethod ( DynaServoCtlMethod_T &  ctlMethod )

virtual

Write to the servo state memory the new control method parameters.

The control method used is determined by the servo's specification.

If this servo is the master in a linked pair of servos, both servos' state data are written.

Control Table:

Dynamixel Internal Position Control Methods.
RAM Clockwise Compliance Margin.
RAM Counterclockwise Compliance Margin.
RAM Clockwise Compliance Slope.
RAM Counterclockwise Compliance Slope.
RAM PID P Gain.
RAM PID I Gain.
RAM PID D Gain.

Parameters

ctlMethod

Control method parameters.

Returns

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

Implements DynaServo.

Definition at line 960 of file DynaServoGeneric.cxx.

References DYNA_CTL_METHOD_COMPLIANCE, DYNA_CTL_METHOD_PID, DYNA_ECODE_INTERNAL, DYNA_LOG_ERROR, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::m_comm, DynaServo::m_nServoId, DynaServo::m_spec, DynaServoSpec_T::m_uCtlMethodUsed, WriteCtlMethodCompliance(), and WriteCtlMethodPid().

Referenced by CvtRawVoltToVolts().

{
  int   rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  switch( m_spec.m_uCtlMethodUsed )
  {
    case DYNA_CTL_METHOD_COMPLIANCE:
      return WriteCtlMethodCompliance(ctlMethod);
    case DYNA_CTL_METHOD_PID:
      return WriteCtlMethodPid(ctlMethod);
    default:
      rc = -DYNA_ECODE_INTERNAL;
      DYNA_LOG_ERROR(rc, "Servo %d: Unknown control method %u.",
          m_nServoId, m_spec.m_uCtlMethodUsed);
      return rc;
  }
}

int DynaServoGeneric::WriteCtlMethodCompliance ( DynaServoCtlMethod_T &  ctlMethod )

protectedvirtual

Write to the servo state memory the new compliance control method parameters.

If this servo is the master in a linked pair of servos, both servos' state data are written.

Control Table:

Dynamixel Internal Position Control Methods.
RAM Clockwise Compliance Margin.
RAM Counterclockwise Compliance Margin.
RAM Clockwise Compliance Slope.
RAM Counterclockwise Compliance Slope.

Parameters

ctlMethod

Control method parameters.

Returns

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

Definition at line 1814 of file DynaServoGeneric.cxx.

References ChkComplianceSlope(), DYNA_ADDR_CCW_COMP_MARGIN, DYNA_ADDR_CCW_COMP_SLOPE, DYNA_ADDR_CW_COMP_MARGIN, DYNA_ADDR_CW_COMP_SLOPE, DYNA_COMP_MARGIN_MAX_RAW, DYNA_COMP_MARGIN_MIN_RAW, DYNA_ECODE_BAD_VAL, DYNA_ECODE_NOT_SUPP, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServoCtlMethod_T::m_comp, DynaServoState_T::m_ctlMethod, DynaServo::m_link, DynaServo::m_nServoId, DynaServoCtlMethod_T::m_params, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServo::m_state, DynaServoCtlMethod_T::m_uCtlMethod, DynaServoSpec_T::m_uCtlMethodUsed, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CalcMatesGoalSpeed(), and WriteControlMethod().

{
  uint_t      uCwMargin;    // new cw margin
  uint_t      uCcwMargin;   // new ccw margin
  uint_t      uCwSlope;     // new cw slope
  uint_t      uCcwSlope;    // new ccw slope
  DynaServo  *pServoMate;   // servo mate
  int         rc;           // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  uCwMargin   = ctlMethod.m_params.m_comp.m_uCwMargin;
  uCcwMargin  = ctlMethod.m_params.m_comp.m_uCcwMargin;
  uCwSlope    = ctlMethod.m_params.m_comp.m_uCwSlope;
  uCcwSlope   = ctlMethod.m_params.m_comp.m_uCcwSlope;

  DYNA_TRY_EXPR((ctlMethod.m_uCtlMethod == m_spec.m_uCtlMethodUsed),
        DYNA_ECODE_NOT_SUPP,
        "Control method %u.", ctlMethod.m_uCtlMethod);

  DYNA_TRY_EXPR(((uCwMargin >= DYNA_COMP_MARGIN_MIN_RAW) &&
                 (uCwMargin <= DYNA_COMP_MARGIN_MAX_RAW)),
        DYNA_ECODE_BAD_VAL,
        "CW compliance margin %u: Out of range.",
        uCwMargin);

  DYNA_TRY_EXPR(((uCcwMargin >= DYNA_COMP_MARGIN_MIN_RAW) &&
                 (uCcwMargin <= DYNA_COMP_MARGIN_MAX_RAW)),
        DYNA_ECODE_BAD_VAL,
        "CCW compliance margin %u: Out of range.",
        uCcwMargin);

  DYNA_TRY_EXPR(ChkComplianceSlope(uCwSlope),
        DYNA_ECODE_BAD_VAL,
        "CW compliance slope 0x%02x: Unknown value.",
        uCwSlope);

  DYNA_TRY_EXPR(ChkComplianceSlope(uCcwSlope),
        DYNA_ECODE_BAD_VAL,
        "CCW compliance slope 0x%02x: Unknown value.",
        uCcwSlope);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    pServoMate = m_link.m_pServoMate;

    rc = m_comm.vSyncWrite(DYNA_ADDR_CW_COMP_MARGIN, 1, 2,
                            m_nServoId, uCwMargin,
                            m_link.m_pServoMate->GetServoId(), uCwMargin);

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCwMargin = uCwMargin;
      pServoMate->m_state.m_ctlMethod.m_params.m_comp.m_uCwMargin = uCwMargin;

      rc = m_comm.vSyncWrite(DYNA_ADDR_CCW_COMP_MARGIN, 1, 2,
                            m_nServoId, uCcwMargin,
                            m_link.m_pServoMate->GetServoId(), uCcwMargin);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCcwMargin = uCcwMargin;
      pServoMate->m_state.m_ctlMethod.m_params.m_comp.m_uCcwMargin = uCcwMargin;

      rc = m_comm.vSyncWrite(DYNA_ADDR_CW_COMP_SLOPE, 1, 2,
                            m_nServoId, uCwSlope,
                            m_link.m_pServoMate->GetServoId(), uCwSlope);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCwSlope = uCwSlope;
      pServoMate->m_state.m_ctlMethod.m_params.m_comp.m_uCwSlope = uCwSlope;

      rc = m_comm.vSyncWrite(DYNA_ADDR_CCW_COMP_SLOPE, 1, 2,
                            m_nServoId, uCcwSlope,
                            m_link.m_pServoMate->GetServoId(), uCcwSlope);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCcwSlope = uCcwSlope;
      pServoMate->m_state.m_ctlMethod.m_params.m_comp.m_uCcwSlope = uCcwSlope;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_CW_COMP_MARGIN, uCwMargin);

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCwMargin = uCwMargin;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_CCW_COMP_MARGIN, uCcwMargin);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCcwMargin = uCcwMargin;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_CW_COMP_SLOPE, uCwSlope);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCwSlope = uCwSlope;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_CCW_COMP_SLOPE, uCcwSlope);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_comp.m_uCcwSlope = uCcwSlope;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteCtlMethodPid ( DynaServoCtlMethod_T &  ctlMethod )

protectedvirtual

Write to the servo state memory the new PID control method parameters.

If this servo is the master in a linked pair of servos, both servos' state data are written.

Control Table:

Dynamixel Internal Position Control Methods.
RAM PID P Gain.
RAM PID I Gain.
RAM PID D Gain.

Parameters

ctlMethod

Control method parameters.

Returns

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

Definition at line 1974 of file DynaServoGeneric.cxx.

References DYNA_ADDR_D_GAIN, DYNA_ADDR_I_GAIN, DYNA_ADDR_P_GAIN, DYNA_D_GAIN_MAX_RAW, DYNA_D_GAIN_MIN_RAW, DYNA_ECODE_BAD_VAL, DYNA_ECODE_NOT_SUPP, DYNA_I_GAIN_MAX_RAW, DYNA_I_GAIN_MIN_RAW, DYNA_OK, DYNA_P_GAIN_MAX_RAW, DYNA_P_GAIN_MIN_RAW, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServoState_T::m_ctlMethod, DynaServo::m_link, DynaServo::m_nServoId, DynaServoCtlMethod_T::m_params, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServo::m_state, DynaServoCtlMethod_T::m_uCtlMethod, DynaServoSpec_T::m_uCtlMethodUsed, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CalcMatesGoalSpeed(), and WriteControlMethod().

{
  uint_t      uPGain;       // new p gain
  uint_t      uIGain;       // new i gain
  uint_t      uDGain;       // new d gain
  DynaServo  *pServoMate;   // servo mate
  int         rc;           // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  uPGain  = ctlMethod.m_params.m_pid.m_uPGain;
  uIGain  = ctlMethod.m_params.m_pid.m_uIGain;
  uDGain  = ctlMethod.m_params.m_pid.m_uDGain;

  DYNA_TRY_EXPR((ctlMethod.m_uCtlMethod == m_spec.m_uCtlMethodUsed),
        DYNA_ECODE_NOT_SUPP,
        "Control method %u.", ctlMethod.m_uCtlMethod);

  DYNA_TRY_EXPR(((uPGain >= DYNA_P_GAIN_MIN_RAW) &&
                 (uPGain <= DYNA_P_GAIN_MAX_RAW)),
        DYNA_ECODE_BAD_VAL,
        "P-Gain %u: Out of range.", uPGain);

  DYNA_TRY_EXPR(((uIGain >= DYNA_I_GAIN_MIN_RAW) &&
                 (uIGain <= DYNA_I_GAIN_MAX_RAW)),
        DYNA_ECODE_BAD_VAL,
        "I-Gain %u: Out of range.", uIGain);

  DYNA_TRY_EXPR(((uDGain >= DYNA_D_GAIN_MIN_RAW) &&
                 (uDGain <= DYNA_D_GAIN_MAX_RAW)),
        DYNA_ECODE_BAD_VAL,
        "D-Gain %u: Out of range.", uDGain);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    pServoMate = m_link.m_pServoMate;

    rc = m_comm.vSyncWrite(DYNA_ADDR_P_GAIN, 1, 2,
                            m_nServoId, uPGain,
                            m_link.m_pServoMate->GetServoId(), uPGain);

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uPGain = uPGain;
      pServoMate->m_state.m_ctlMethod.m_params.m_pid.m_uPGain = uPGain;

      rc = m_comm.vSyncWrite(DYNA_ADDR_I_GAIN, 1, 2,
                            m_nServoId, uIGain,
                            m_link.m_pServoMate->GetServoId(), uIGain);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uIGain = uIGain;
      pServoMate->m_state.m_ctlMethod.m_params.m_pid.m_uIGain = uIGain;

      rc = m_comm.vSyncWrite(DYNA_ADDR_D_GAIN, 1, 2,
                            m_nServoId, uDGain,
                            m_link.m_pServoMate->GetServoId(), uDGain);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uDGain = uDGain;
      pServoMate->m_state.m_ctlMethod.m_params.m_pid.m_uDGain = uDGain;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_P_GAIN, uPGain);

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uPGain = uPGain;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_I_GAIN, uIGain);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uIGain = uIGain;
      rc = m_comm.Write8(m_nServoId, DYNA_ADDR_D_GAIN, uDGain);
    }

    if( rc == DYNA_OK )
    {
      m_state.m_ctlMethod.m_params.m_pid.m_uDGain = uDGain;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteGoalPos ( int  nGoalOdPos )

virtual

Write to the servo state memory to set the new goal position.

If the servo [pair] are in servo mode, rotation will occur until the position(s) are achieved.

If this servo is the master in a linked pair of servos, both servos' state written.

Control Table:

RAM Goal Position.
Dynamixel Position Common Values.

Parameters

nGoalOdPos

Odometer goal position (odometer ticks). In continuous mode, writing the goal position has no effect on movement.

Returns

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

Implements DynaServo.

Definition at line 1013 of file DynaServoGeneric.cxx.

References CalcMatesGoalPos(), DYNA_ADDR_GOAL_POS_LSB, DYNA_ECODE_BAD_VAL, DYNA_MODE_SERVO, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_cfg, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServo::m_state, DynaServoState_T::m_uGoalPos, DynaServoSpec_T::m_uRawPosMax, DynaServoSpec_T::m_uRawPosMin, DynaServoCfg_T::m_uServoMode, DynaServo::OdometerToEncoder(), DynaComm::vSyncWrite(), and DynaComm::Write16().

Referenced by CvtRawVoltToVolts(), MoveAtSpeedTo(), MoveTo(), and Stop().

{
  int       nEncPos;          // encoder position
  int       rc;               // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  nEncPos = OdometerToEncoder(nGoalOdPos);
  
  if( m_cfg.m_uServoMode == DYNA_MODE_SERVO )
  {
    DYNA_TRY_EXPR(((nEncPos >= m_spec.m_uRawPosMin) &&
                   (nEncPos <= m_spec.m_uRawPosMax)),
      DYNA_ECODE_BAD_VAL,
      "Servo %d: Goal odometer position %d (encoder=%d): Out of range.",
      m_nServoId, nGoalOdPos, nEncPos);
  }

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    int   nGoalOdPosMate;   // mate's calculated goal odometer position
    int   nEncGoalPosMate;  // mate's calculated goal encoder position

    if( (rc = CalcMatesGoalPos(nGoalOdPos, &nGoalOdPosMate)) == DYNA_OK )
    {
      nEncGoalPosMate = m_link.m_pServoMate->OdometerToEncoder(nGoalOdPosMate);
      rc = m_comm.vSyncWrite(DYNA_ADDR_GOAL_POS_LSB, 2, 2,
                          m_nServoId, (uint_t)nEncPos,
                          m_link.m_pServoMate->GetServoId(), // RDK FIX ME
                          (uint_t)nEncGoalPosMate);

      if( rc == DYNA_OK )
      {
        m_state.m_uGoalPos = (uint_t)nEncPos;
        m_link.m_pServoMate->m_state.m_uGoalPos = (uint_t)nEncGoalPosMate;
      }
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write16(m_nServoId, DYNA_ADDR_GOAL_POS_LSB, (uint_t)nEncPos);

    if( rc == DYNA_OK )
    {
      m_state.m_uGoalPos = (uint_t)nEncPos;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteGoalSpeed ( int  nGoalSpeed )

virtual

Write to the servo state memory the new goal speed and direction.

For servos in continuous mode, speeds are in the range [-max,max] where the values < 0, 0, and > 0 specify goal rotation in the clockwise direction, stop, or goal 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, rotation will occur indefinitely in the direction specified and at the given speed.

Control Table:

RAM Goal Speed and Direction.
Dynamixel Angular Speed Common Values.
Dynamixel Direction Common Values.

Parameters

nGoalSpeed

Goal speed.

Returns

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

Implements DynaServo.

Definition at line 1096 of file DynaServoGeneric.cxx.

References CalcMatesGoalSpeed(), DYNA_ADDR_GOAL_SPEED_LSB, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_SPEED_MAX_RAW, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), iabs(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServoState_T::m_nGoalSpeed, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, PackGoalSpeed(), DynaComm::vSyncWrite(), and DynaComm::Write16().

Referenced by CvtRawVoltToVolts(), MoveAtSpeed(), MoveAtSpeedTo(), and Stop().

{
  int     nGoalSpeedMate;   // mate's calculated speed
  uint_t  uVal1, uVal2;     // working values
  int     rc;               // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR( (iabs(nGoalSpeed) <= DYNA_SPEED_MAX_RAW),
                  DYNA_ECODE_BAD_VAL,
                  "Goal speed %d: Out of range.", nGoalSpeed);

  uVal1 = PackGoalSpeed(nGoalSpeed);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    nGoalSpeedMate = CalcMatesGoalSpeed(nGoalSpeed);

    uVal2 = PackGoalSpeed(nGoalSpeedMate);

    rc = m_comm.vSyncWrite(DYNA_ADDR_GOAL_SPEED_LSB, 2, 2,
                        m_nServoId, uVal1,
                        m_link.m_pServoMate->GetServoId(), uVal2);

    if( rc == DYNA_OK )
    {
      m_state.m_nGoalSpeed = nGoalSpeed;
      m_link.m_pServoMate->m_state.m_nGoalSpeed = nGoalSpeedMate;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write16(m_nServoId, DYNA_ADDR_GOAL_SPEED_LSB, uVal1);

    if( rc == DYNA_OK )
    {
      m_state.m_nGoalSpeed = nGoalSpeed;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteLed ( bool  bState )

virtual

Write to the servo state memory to turn on or off the servo LED.

If this servo is the master in a linked pair of servos, both servos' state written.

Control Table:

RAM LED On/Off.

Parameters

bState

On (true) or off (false) state.

Returns

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

Implements DynaServo.

Definition at line 898 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LED, DYNA_LED_OFF, DYNA_LED_ON, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServoState_T::m_bLed, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaComm::vSyncWrite(), and DynaComm::Write8().

Referenced by CvtRawVoltToVolts().

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  uVal = bState? DYNA_LED_ON: DYNA_LED_OFF;

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_LED, 1, 2,
                        m_nServoId, uVal,
                        m_link.m_pServoMate->GetServoId(), uVal);

    if( rc == DYNA_OK )
    {
      m_state.m_bLed = bState;
      m_link.m_pServoMate->m_state.m_bLed = bState;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_LED, uVal);

    if( rc == DYNA_OK )
    {
      m_state.m_bLed = bState;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteMaxTorqueLimit ( uint_t  uMaxTorqueLim )

virtual

Write to the servo state memory to set the new maximum torque limit.

If this servo is the master in a linked pair of servos, both servos' state written.

Control Table:

RAM Maximum Torque Limit.
Dynamixel Torque Common Values.

Parameters

uMaxTorqueLim

Maximum torque limit.

Returns

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

Implements DynaServo.

Definition at line 1169 of file DynaServoGeneric.cxx.

References DYNA_ADDR_LIM_TORQUE_MAX_LSB, DYNA_ECODE_BAD_VAL, DYNA_OK, DYNA_TRY_COMM, DYNA_TRY_EXPR, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_spec, DynaServo::m_state, DynaServoState_T::m_uLimTorqueMax, DynaServoSpec_T::m_uRawTorqueMax, DynaServoSpec_T::m_uRawTorqueMin, DynaComm::vSyncWrite(), and DynaComm::Write16().

Referenced by CvtRawVoltToVolts(), and ReloadMaxTorqueLimit().

{
  int     rc;   // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  DYNA_TRY_EXPR(((uMaxTorqueLim >= m_spec.m_uRawTorqueMin) && 
                 (uMaxTorqueLim <= m_spec.m_uRawTorqueMax)), 
      DYNA_ECODE_BAD_VAL,
      "Maximum torque limit %u: Out of range.", uMaxTorqueLim);

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_LIM_TORQUE_MAX_LSB, 2, 2,
                        m_nServoId, uMaxTorqueLim,
                        m_link.m_pServoMate->GetServoId(), uMaxTorqueLim);

    if( rc == DYNA_OK )
    {
      m_state.m_uLimTorqueMax = uMaxTorqueLim;
      m_link.m_pServoMate->m_state.m_uLimTorqueMax = uMaxTorqueLim;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write16(m_nServoId, DYNA_ADDR_LIM_TORQUE_MAX_LSB,
                                    uMaxTorqueLim);

    if( rc == DYNA_OK )
    {
      m_state.m_uLimTorqueMax = uMaxTorqueLim;
    }
  }

  return rc;
}

int DynaServoGeneric::WriteTorqueEnable ( bool  bState )

virtual

Write to the servo state memory to set the new torque enable value.

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

If this servo is the master in a linked pair of servos, both servos' state written.

Control Table:

RAM Torque Enable.

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.

Implements DynaServo.

Definition at line 831 of file DynaServoGeneric.cxx.

References DYNA_ADDR_TORQUE_EN, DYNA_OK, DYNA_TORQUE_EN_OFF, DYNA_TORQUE_EN_ON, DYNA_TRY_COMM, DYNA_TRY_IS_MASTER, DynaServo::GetServoId(), DynaServo::IsLinkedMaster(), DynaServoState_T::m_bTorqueEnabled, DynaServo::m_comm, DynaServo::m_link, DynaServo::m_nServoId, DynaServo::CrossLink_T::m_pServoMate, DynaServo::m_state, DynaComm::vSyncWrite(), and DynaComm::Write8().

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

{
  uint_t  uVal;   // working value
  int     rc;     // return code

  DYNA_TRY_COMM(m_comm);
  DYNA_TRY_IS_MASTER(this);

  uVal = bState? DYNA_TORQUE_EN_ON: DYNA_TORQUE_EN_OFF;

  //
  // Linked pair
  //
  if( IsLinkedMaster() )
  {
    rc = m_comm.vSyncWrite(DYNA_ADDR_TORQUE_EN, 1, 2,
                        m_nServoId, uVal,
                        m_link.m_pServoMate->GetServoId(),uVal);

    if( rc == DYNA_OK )
    {
      m_state.m_bTorqueEnabled = bState;
      m_link.m_pServoMate->m_state.m_bTorqueEnabled = bState;
    }
  }

  //
  // Solitary servo
  //
  else
  {
    rc = m_comm.Write8(m_nServoId, DYNA_ADDR_TORQUE_EN, uVal);

    if( rc == DYNA_OK )
    {
      m_state.m_bTorqueEnabled = bState;
    }
  }

  return rc;
}

Member Data Documentation

const int DynaServoGeneric::DYNA_MODEL_NUM = DYNA_MODEL_NUM_GENERIC

static

Model number

Definition at line 79 of file DynaServoGeneric.h.

Referenced by DynaServo::New().

---

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

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