DynaVServo Class Reference

Background Thread Virtual Servo Class. More...

#include <DynaBgThread.h>

Public Types
enum	VServoState {   StateIdle,   StateBegin,   StateControl }
	Servo dynamics position control state. More...

Public Member Functions
	DynaVServo ()
	Default constructor.
	DynaVServo (const DynaVServo &src)
	Copy constructor. More...
	~DynaVServo ()
	Destructor.
DynaVServo	operator= (const DynaVServo &rhs)
	Assignment operator. More...
void	setToleranceInTicks (DynaServo *pServo, double fTolerance)
	Set position tolerance in encoder ticks. More...
double	filterTorques (int nServoLoad)
	Apply a low-pass band filter on the sensed torques (loads). More...
int	getGoalDir (DynaServo *pServo)
	Get the current goal rotation direction. More...
void	lock ()
	Lock the virtual servo. More...
void	unlock ()
	Unlock the virtual servo. More...

Public Attributes
VServoState	m_eState
	virtual servo state
DynaPidPos	m_pidPos
	servo position PID
int	m_nTolerance
	position ± tolerance (ticks)
int	m_nOdGoalPos
	current goal position
int	m_nGoalSpeed
	current goal speed
bool	m_bOverTorqueCond
	is [not] in torque overload condition
bool	m_bOverTorqueCtl
	[not] in torque overload control
double	m_fTorqueOut
	low-pass filtered torque
std::deque< double >	m_histTorqueIn
	recent history of joint torques

Static Public Attributes
static const size_t	TORQUE_WIN_SIZE = 10
	Torque sliding window size for low-pass band filtering of input torques.
static const int	GOAL_SPEED_DFT = 100
	default virutal goal speed

Protected Attributes
pthread_mutex_t	m_mutexSync
	synchonization mutex

Detailed Description

Background Thread Virtual Servo Class.

Definition at line 87 of file DynaBgThread.h.

Member Enumeration Documentation

enum DynaVServo::VServoState

Servo dynamics position control state.

Enumerator
StateIdle	no active position control
StateBegin	start position control
StateControl	in poistion control

Definition at line 101 of file DynaBgThread.h.

  {
    StateIdle,      ///< no active position control
    StateBegin,     ///< start position control 
    StateControl    ///< in poistion control
  };

Constructor & Destructor Documentation

DynaVServo::DynaVServo

(

const DynaVServo &

src

)

Copy constructor.

Parameters

src	Source object.

Definition at line 205 of file DynaBgThread.cxx.

References m_bOverTorqueCond, m_bOverTorqueCtl, m_eState, m_fTorqueOut, m_histTorqueIn, m_mutexSync, m_nGoalSpeed, m_nOdGoalPos, m_nTolerance, and m_pidPos.

{
  m_eState          = src.m_eState;
  m_pidPos          = src.m_pidPos;
  m_nTolerance      = src.m_nTolerance;
  m_nOdGoalPos      = src.m_nOdGoalPos;
  m_nGoalSpeed      = src.m_nGoalSpeed;
  m_bOverTorqueCond = src.m_bOverTorqueCond;
  m_bOverTorqueCtl  = src.m_bOverTorqueCtl;
  m_fTorqueOut      = src.m_fTorqueOut;
  m_histTorqueIn    = src.m_histTorqueIn;

  pthread_mutex_init(&m_mutexSync, NULL);
}

Member Function Documentation

double DynaVServo::filterTorques

(

int

nServoLoad

)

Apply a low-pass band filter on the sensed torques (loads).

The torques are interpreted from the unitless load values read from the servo (and probably non-linear).

A simple moving average (SMA) of load values with a window size of TORQUE_WIN_SIZE is used as the filter.

Parameters

nServoLoad

New servo load value (raw unitless).

Returns

New torque average (raw unitless).

Definition at line 261 of file DynaBgThread.cxx.

References DynaBgThread::HZ_EXEC_DFT, DynaBgThread::HZ_EXEC_MIN, m_fTorqueOut, m_histTorqueIn, DynaBgThread::T_EXEC_MIN, DynaBgThread::TOLERANCE_DFT, and TORQUE_WIN_SIZE.

Referenced by DynaBgThread::monitorDynamics().

{
  double  fTorque0;
  double  fTorqueK;

  fTorque0 = (double)nServoLoad / (double)TORQUE_WIN_SIZE;
  fTorqueK = m_histTorqueIn.back();
  m_histTorqueIn.pop_back();
  m_histTorqueIn.push_front(fTorque0);

  m_fTorqueOut = m_fTorqueOut - fTorqueK + fTorque0;

  return m_fTorqueOut;
}

int DynaVServo::getGoalDir ( DynaServo *  pServo )

inline

Get the current goal rotation direction.

The direct is deterimed by the difference of current odometer value and the goal position.

Normally counter-clockwise is positive, clockwise is negative, but the sence can be reversed in the odometer configuration.

Parameters

pServer

Pointer to servo.

Returns

1 or -1.

Definition at line 186 of file DynaBgThread.h.

References DynaServo::GetOdometer(), and DynaServo::IsOdometerReversed().

Referenced by DynaBgThread::execDynamics().

  {
    int nSign = pServo->IsOdometerReversed()? -1: 1;

    return (m_nOdGoalPos-pServo->GetOdometer()) >= 0? nSign: -1*nSign;
  }

void DynaVServo::lock ( )

inline

Lock the virtual servo.

The calling thread will block while waiting for the mutex to become available. Once locked, the background thread will block when accessing this vitural servo.

The lock()/unlock() primitives provide a safe mechanism to modify the registered vServo data.

Context:

Any.

Definition at line 206 of file DynaBgThread.h.

Referenced by DynaBgThread::execDynamics(), DynaBgThread::readyWait(), DynaBgThread::timeWait(), and DynaBgThread::WriteGoalPos().

  {
    pthread_mutex_lock(&m_mutexSync);
  }

DynaVServo DynaVServo::operator=

(

const DynaVServo &

rhs

)

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

This object.

Definition at line 220 of file DynaBgThread.cxx.

References m_bOverTorqueCond, m_bOverTorqueCtl, m_eState, m_fTorqueOut, m_histTorqueIn, m_nGoalSpeed, m_nOdGoalPos, m_nTolerance, and m_pidPos.

{
  m_eState          = rhs.m_eState;
  m_pidPos          = rhs.m_pidPos;
  m_nTolerance      = rhs.m_nTolerance;
  m_nOdGoalPos      = rhs.m_nOdGoalPos;
  m_nGoalSpeed      = rhs.m_nGoalSpeed;
  m_bOverTorqueCond = rhs.m_bOverTorqueCond;
  m_bOverTorqueCtl  = rhs.m_bOverTorqueCtl;
  m_fTorqueOut      = rhs.m_fTorqueOut;
  m_histTorqueIn    = rhs.m_histTorqueIn;

  return *this;
}

void DynaVServo::setToleranceInTicks	(	DynaServo *	pServo,
		double	fTolerance
	)

Set position tolerance in encoder ticks.

Parameters

pServo	Pointer to servo.
fTolerance	Tolerance (degrees).

Definition at line 235 of file DynaBgThread.cxx.

References DYNA_MODE_CONTINUOUS, DynaServo::GetServoMode(), DynaServo::GetSpecification(), DynaServoSpec_T::m_fAngleMax, DynaServoSpec_T::m_fAngleMin, m_nTolerance, and DynaServoSpec_T::m_uRawPosModulo.

Referenced by DynaBgThread::RegisterChainAgent(), and DynaBgThread::RegisterServoAgent().

{
  double    fTicks;
  double    fAngleMin;
  double    fAngleMax;
  double    fDegrees;

  //
  // Servo rotation range.
  //
  if( pServo->GetServoMode() == DYNA_MODE_CONTINUOUS )
  {
    fDegrees = 360.0;
  }
  else
  {
    fAngleMin = pServo->GetSpecification().m_fAngleMin;
    fAngleMax = pServo->GetSpecification().m_fAngleMax;
    fDegrees  = fAngleMax - fAngleMin;
  }
  
  fTicks = (double)pServo->GetSpecification().m_uRawPosModulo;

  m_nTolerance = (int)(fTicks * fTolerance / fDegrees);
}

void DynaVServo::unlock ( )

inline

Unlock the virtual servo.

The background thread will be available to run when accessing this vitural servo.

Context:

Any.

Definition at line 220 of file DynaBgThread.h.

Referenced by DynaBgThread::execDynamics(), DynaBgThread::readyWait(), DynaBgThread::timeWait(), and DynaBgThread::WriteGoalPos().

  {
    pthread_mutex_unlock(&m_mutexSync);
  }

---

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

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