DynaBgThread Class Reference

#include <DynaBgThread.h>

Public Types
enum	BgThreadState {   BgThreadStateZombie,   BgThreadStateReady,   BgThreadStateRunning,   BgThreadStatePaused,   BgThreadStateExit }
	Background thread states. More...

Public Member Functions
	DynaBgThread (double fHz=HZ_EXEC_DFT, double fTolerance=TOLERANCE_DFT)
	Default initializer constructor. More...
virtual	~DynaBgThread ()
	Desctructor.
virtual void	RegisterServoAgent (DynaServo *pServo)
	Register the Dynamixel servo for control and monitoring. More...
virtual void	RegisterChainAgent (DynaChain *pChain)
	Register the Dynamixel chain for control and monitoring. More...
virtual void	UnregisterAgent ()
	Unregister any chain or servo from control and monitoring. More...
virtual void	RegisterUserCallback (void(*fnUserCb)(void *), void *pUserArg)
	Register user-defined callback function. More...
virtual void	UnregisterUserCallback ()
	Unregister user-defined callback function.
virtual int	Run ()
	Run the background thread control and monitoring tasks. More...
virtual int	Stop ()
	Stop control and monitoring tasks. More...
virtual int	Pause ()
	Pause control and monitoring tasks. More...
virtual int	Resume ()
	Resume control and monitoring tasks. More...
BgThreadState	GetCurrentState ()
	Get current background thread state. More...
void	setHz (double fHz)
	Set new control and monitoring execution hertz rate. More...
double	getHz ()
	Get control and monitoring hetrz rate. More...
bool	isServoBeingControlled (int nServoId)
	Checks if servo is currently being controlled. More...

Static Public Member Functions
static int	WriteGoalPos (int nServoId, int nOdGoalPos, void *pUserArg)
	Start controlling servo to rotate it to the given goal position. More...
static int	WriteGoalSpeed (int nServoId, int nGoalSpeed, void *pUserArg)
static int	WriteGoalSpeedPos (int nServoId, int nGoalSpeed, int nGoalPos, void *pUserArg)

Static Public Attributes
static const double	HZ_EXEC_MIN = 1.0
	minimum exec hertz
static const double	HZ_EXEC_DFT = 50.0
	default exec hertz
static const long	T_EXEC_MIN = 100
	task exec period min (100usec)
static const double	TOLERANCE_DFT = 1.0
	default pos. tolerance (deg)

Protected Types
typedef map< int, DynaVServo >	MapVServo
	Virtual Servo Associative Map Type.

Protected Member Functions
void	lock ()
	Lock the background thread. More...
void	unlock ()
	Unlock the background thread. More...
void	changeState (DynaBgThread::BgThreadState eNewState)
	Change the background thread state. More...
void	readyWait ()
	Wait indefinitely in ready state. More...
void	timeWait (long lMicroSecs)
	Wait until state change or time out. More...
void	setTolerance (double fTolerance)
	Set position tolerance. More...
virtual DynaServo *	getRegisteredServo (int nServoId)
	Get registered servo. More...
virtual DynaVServo *	getNextVServo (MapVServo::iterator &iter, int &nServoId)
	Get the next virtual servo. More...
virtual DynaVServo *	getPrevVServo (MapVServo::iterator &iter, int &nServoId)
	Get the previous virtual servo. More...
virtual void	sched (long lPeriod)
	Schedule the Dynamixel background thread for the next task(s) to perform. More...
virtual void	exec ()
	Execute background tasks. More...
virtual void	execDynamics (DynaServo *pServo, DynaVServo *pVServo)
	Execute servo dynamics monitoring and control. More...
virtual void	execTorqueCtl (DynaServo *pServo, DynaVServo *pVServo)
	Execute servo torque control. More...
virtual void	execPosCtl (DynaServo *pServo, DynaVServo *pVServo)
	Execute servo position control. More...
virtual void	stopPosCtl (DynaServo *pServo, DynaVServo *pVServo)
	Stop servo position control. More...
virtual void	stopMotion (DynaServo *pServo)
	Stop servo motion. More...
virtual void	monitorDynamics (DynaServo *pServo, DynaVServo *pVServo)
	Execute servo dynamics monitoring background task. More...
virtual void	monitorHealth (DynaServo *pServo, DynaVServo *pVServo)
	Execute servo health monitoring background task. More...
void	createThread ()
	Create the background thread. More...
void	terminateThread ()
	Terminate the background thread. More...

Static Protected Member Functions
static void *	bgThread (void *pArg)
	Dynamixel background thread. More...

Protected Attributes
BgThreadState	m_eState
	thread state
pthread_mutex_t	m_mutexSync
	synchonization mutex
pthread_cond_t	m_condSync
	synchonization condition
pthread_t	m_thread
	pthread identifier
DynaServo *	m_pServo
	registered dynamixel servo
DynaChain *	m_pChain
	or registered dynamixel chain
MapVServo	m_mapVServo
	virtual servo associative map
uint_t	m_uNumServos
	number of servos to monitor
double	m_fTolerance
	position ± tolerance (degrees)
DynaAgent_T	m_agent
	servo/chain agent
void(*	m_fnUserCb )(void *)
	user callback function
void *	m_pUserArg
	user callback argument
double	m_fHz
	thread execution hertz
long	m_TExec
	full execution cycle μs period
struct timeval	m_tvSched
	working scheduler time stamp
long	m_TSched
	scheduler μs period
long	m_dTSched
	scheduler delta time
MapVServo::iterator	m_iterDynamics
	dynamics iterator
MapVServo::iterator	m_iterHealth
	health iterator
int	m_nHealthServoId
	current health monitoring servo id

Detailed Description

Dynamixel background thread.

The DynaBgThread class supports background control and monitoring of registered servos.

Definition at line 240 of file DynaBgThread.h.

Member Enumeration Documentation

enum DynaBgThread::BgThreadState

Background thread states.

Enumerator
BgThreadStateZombie	zombie instance - no thread exists
BgThreadStateReady	thread created and ready to run
BgThreadStateRunning	thread running
BgThreadStatePaused	thread paused
BgThreadStateExit	thread exiting

Definition at line 251 of file DynaBgThread.h.

  {
    BgThreadStateZombie,    ///< zombie instance - no thread exists
    BgThreadStateReady,     ///< thread created and ready to run
    BgThreadStateRunning,   ///< thread running
    BgThreadStatePaused,    ///< thread paused
    BgThreadStateExit       ///< thread exiting
  } BgThreadState;

Constructor & Destructor Documentation

DynaBgThread::DynaBgThread	(	double	fHz = HZ_EXEC_DFT,
		double	fTolerance = TOLERANCE_DFT
	)

Default initializer constructor.

Parameters

fHz	Execution hertz.
fTolerance	Position tolerance (degrees).

Definition at line 290 of file DynaBgThread.cxx.

References BG_TRACE_OPEN, DYNA_ID_NONE, and DynaVServo::m_mutexSync.

{
  BG_TRACE_OPEN();

  pthread_mutex_init(&m_mutexSync, NULL);
  pthread_cond_init(&m_condSync, NULL);

  m_pServo      = NULL;
  m_pChain      = NULL;
  m_uNumServos  = 0;

  setTolerance(fTolerance);

  m_agent.m_fnWriteGoalPos      = WriteGoalPos;
  m_agent.m_fnWriteGoalSpeed    = WriteGoalSpeed;
  m_agent.m_fnWriteGoalSpeedPos = WriteGoalSpeedPos;

  m_fnUserCb  = NULL;
  m_pUserArg  = NULL;

  m_iterDynamics      = m_mapVServo.end();
  m_iterHealth        = m_mapVServo.end();
  m_nHealthServoId    = DYNA_ID_NONE;

  setHz(fHz);

  // create background thread
  createThread();
}

Member Function Documentation

void * DynaBgThread::bgThread ( void *  pArg )

staticprotected

Dynamixel background thread.

Cannot have gui calls in this thread.

Parameters

pArg	Thread argument (point to DynaBgThread object).

Returns

Returns NULL on thread exit.

Definition at line 1187 of file DynaBgThread.cxx.

References exec(), m_eState, m_TSched, m_uNumServos, readyWait(), and sched().

Referenced by createThread().

{
  DynaBgThread  *pThis = (DynaBgThread *)pArg;
  int           rc;

  LOGDIAG3("Dynamixel background thread created.");

  //
  // Loop forever until exit
  //
  while( (pThis->m_eState != BgThreadStateExit) )
  {
    switch( pThis->m_eState )
    {
      case BgThreadStateReady:
      case BgThreadStatePaused:
        pThis->readyWait();
        LOGDIAG3("New state: %d: t_sched=%lu",
                  pThis->m_eState, pThis->m_TSched);
        break;
      case BgThreadStateRunning:
        if( pThis->m_uNumServos > 0 )
        {
          pThis->sched(pThis->m_TSched);
          if( pThis->m_eState == BgThreadStateRunning )
          {
            pThis->exec();
          }
        }
        else // nothing to do
        {
          // slowly
          pThis->sched(500000);
        }
        break;
      case BgThreadStateExit:
        break;
      default:
        LOGERROR("%d: Unexpected dynamixel background thread state.",
            pThis->m_eState);
        pThis->m_eState = BgThreadStateExit;
        break;
    }
  }

  pThis->m_eState = BgThreadStateZombie;

  LOGDIAG3("Dynamixel background thread exited.");

  return NULL;
}

void DynaBgThread::changeState ( DynaBgThread::BgThreadState  eNewState )

protected

Change the background thread state.

Parameters

eNewState

New state.

Context:

Calling thread or background thread.

Definition at line 769 of file DynaBgThread.cxx.

References DynaVServo::m_eState.

{
  m_eState = eNewState;
  pthread_cond_signal(&m_condSync);
}

void DynaBgThread::createThread ( )

protected

Create the background thread.

Context:

Calling thread.

Definition at line 1239 of file DynaBgThread.cxx.

References bgThread(), and DynaVServo::m_eState.

{
  int   rc;

  m_eState = BgThreadStateReady;

  rc = pthread_create(&m_thread, NULL, DynaBgThread::bgThread, (void *)this);
 
  if( rc != 0 )
  {
    LOGSYSERROR("pthread_create()");
    m_eState = BgThreadStateZombie;
  }
}

void DynaBgThread::exec ( )

protectedvirtual

Execute background tasks.

Context:

Background thread.

Definition at line 868 of file DynaBgThread.cxx.

Referenced by bgThread().

{
  int         nServoId;
  DynaVServo *pVServo;
  DynaServo  *pServo;

  //
  // Get the next virtual servo.
  //
  if( (pVServo = getNextVServo(m_iterDynamics, nServoId)) == NULL )
  {
    return;
  }

  //
  // Get the associated registered servo.
  //
  if( (pServo = getRegisteredServo(nServoId)) == NULL )
  {
    // can't find servo - erase entry from vServo map
    m_mapVServo.erase(nServoId);
    m_uNumServos = (uint_t)m_mapVServo.size();
    return;
  }

  //
  // Execute servo dynamics monitoring and control.
  //
  execDynamics(pServo, pVServo);

  //
  // Monitor servo health.
  //
  if( nServoId == m_nHealthServoId )
  {
    monitorHealth(pServo, pVServo);
    getPrevVServo(m_iterHealth, m_nHealthServoId);
  }

  //
  // User callback
  //
  if( m_fnUserCb != NULL )
  {
    m_fnUserCb(m_pUserArg);
  }
}

void DynaBgThread::execDynamics ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Execute servo dynamics monitoring and control.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 916 of file DynaBgThread.cxx.

References DYNA_GET_DIR, DYNA_SPEED_CONT_STOP, DynaVServo::getGoalDir(), DynaServo::GetState(), DynaVServo::lock(), DynaVServo::m_bOverTorqueCond, DynaVServo::m_bOverTorqueCtl, DynaServoState_T::m_bTorqueEnabled, DynaVServo::m_eState, DynaVServo::m_fTorqueOut, DynaVServo::m_nGoalSpeed, DynaVServo::StateIdle, and DynaVServo::unlock().

{
  pVServo->lock();

  //
  // Read relavent servo state dynamics.
  //
  monitorDynamics(pServo, pVServo);

  //
  // Servo in torque overload condition, ease motor drive if possible.
  //
  if( pServo->GetState().m_bTorqueEnabled &&
      (pVServo->m_bOverTorqueCond || pVServo->m_bOverTorqueCtl) )
  {
    execTorqueCtl(pServo, pVServo);
  }

  //
  // Servo not driven or state is idle.
  //
  if( !pServo->GetState().m_bTorqueEnabled ||
      (pVServo->m_eState == DynaVServo::StateIdle) )
  {
    // do nothing
  }

  //
  // Stop servo.
  //
  else if( pVServo->m_nGoalSpeed == DYNA_SPEED_CONT_STOP )
  {
    stopPosCtl(pServo, pVServo);
  }

  //
  // Servo in torque overload condition and goal direction is in the same
  // direction as the load, which would exasperate the condition.
  //
  else if( pVServo->m_bOverTorqueCond &&
       (pVServo->getGoalDir(pServo) != DYNA_GET_DIR(pVServo->m_fTorqueOut)) )
  {
    //stopPosCtl(pServo, pVServo);
  }

  //
  // PID control servo to goal position.
  //
  else
  {
    execPosCtl(pServo, pVServo);
  }

  pVServo->unlock();
}

void DynaBgThread::execPosCtl ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Execute servo position control.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 1030 of file DynaBgThread.cxx.

References BG_TRACE, DynaPid::Control(), DYNA_MODE_SERVO, DYNA_OK, DYNA_SPEED_CONT_STOP, DynaPid::GetError(), DynaServo::GetOdometer(), DynaServo::GetServoId(), DynaServo::GetServoMode(), iabs(), DynaVServo::m_eState, DynaVServo::m_nGoalSpeed, DynaVServo::m_nOdGoalPos, DynaVServo::m_nTolerance, DynaVServo::m_pidPos, and DynaVServo::StateIdle.

{
  int                   nOdGoalPos; // goal position
  int                   nGoalSpeed; // goal speed
  int                   nCurPos;    // current position
  int                   nCurSpeed;  // current speed
  int                   nErr;       // position difference error
  double                dt;         // delta time

  //
  // Once goal position is set, servo mode servos move on there own.
  //
  if( pServo->GetServoMode() == DYNA_MODE_SERVO )
  {
    nGoalSpeed  = pVServo->m_nGoalSpeed;
    nOdGoalPos  = pVServo->m_nOdGoalPos;

    // stop
    if( iabs(nGoalSpeed) == 0 )
    {
      stopPosCtl(pServo, pVServo);
    }

    pVServo->m_eState = DynaVServo::StateIdle;

    return;
  }

  else
  {
    nCurPos = pServo->GetOdometer();

    dt = (double)m_dTSched/1000000.0;

    nGoalSpeed  = (int)pVServo->m_pidPos.Control((double)nCurPos, dt);
    nErr        = (int)pVServo->m_pidPos.GetError();

    if( (iabs(nErr) < pVServo->m_nTolerance) ||
        (nGoalSpeed == DYNA_SPEED_CONT_STOP) )
    {
      stopPosCtl(pServo, pVServo);
    }
    else if( pServo->WriteGoalSpeed(nGoalSpeed) == DYNA_OK )
    {
      BG_TRACE(BG_TRACE_FILTER_POSCTL, pServo->GetServoId(), "MOVE",
                "goalspeed=%d, cur_od_pos=%d, goal_od_pos=%d, diff=%d\n",
                nGoalSpeed,
                nCurPos,
                pVServo->m_nOdGoalPos,
                nErr);
    }
  }
}

void DynaBgThread::execTorqueCtl ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Execute servo torque control.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 972 of file DynaBgThread.cxx.

References BG_TRACE, DYNA_GET_DIR, DYNA_MODE_CONTINUOUS, DYNA_MODE_SERVO, DYNA_SPEED_CONT_STOP, DynaServo::GetOdometer(), DynaServo::GetServoId(), DynaServo::GetServoMode(), DynaServo::GetSoftTorqueThresholds(), DynaVServo::GOAL_SPEED_DFT, DynaVServo::m_bOverTorqueCtl, and DynaVServo::m_fTorqueOut.

{
  double      fTorqueAvg;     // filtered load
  uint_t      uOverTorqueTh;  // set over torque condition threshold
  uint_t      uClearTorqueTh; // clear over torque condition threshold
  int         nCurPos;        // current position
  int         sign;           // torque release direction sign 
  int         nGoalSpeed;     // goal speed
  int         nGoalPos;       // goal position

  LOGWARN("BGThread: Executing torque control for servo %d, torque=%.2lf.",
      pServo->GetServoId(), pVServo->m_fTorqueOut);

  pServo->GetSoftTorqueThresholds(uOverTorqueTh, uClearTorqueTh);

  fTorqueAvg = pVServo->m_fTorqueOut;

  if( (uint_t)fabs(fTorqueAvg) > uOverTorqueTh )
  {
    sign = DYNA_GET_DIR(fTorqueAvg);

    nGoalSpeed = DynaVServo::GOAL_SPEED_DFT;

    nCurPos   = pServo->GetOdometer();
    nGoalPos  = nCurPos + sign * 5;

    switch( pServo->GetServoMode() )
    {
      case DYNA_MODE_CONTINUOUS:
        nGoalSpeed = nGoalSpeed * sign;
        pServo->WriteGoalSpeed(nGoalSpeed);
        usleep(1000); // RDK hack
        stopMotion(pServo);
        break;

      case DYNA_MODE_SERVO:
      default:
        pServo->WriteGoalSpeed(nGoalSpeed);
        pServo->WriteGoalPos(nGoalPos);
        break;
    }

    pVServo->m_bOverTorqueCtl = true;

    BG_TRACE(BG_TRACE_FILTER_TORQCTL, pServo->GetServoId(), "TORQUECTL",
      "cur_load=%d, filtered_load=%.2lf, cur_od_pos=%d, "
      "goal_speed=%d, goal_od_pos=%d\n",
              nCurLoad, fTorqueAvg, nCurPos, nGoalSpeed, nGoalPos);
  }

  // stop servo, but don't touch current goals
  else
  {
    pVServo->m_bOverTorqueCtl = false;
    pServo->WriteGoalSpeed(DYNA_SPEED_CONT_STOP);
  }
}

BgThreadState DynaBgThread::GetCurrentState ( )

inline

Get current background thread state.

Returns

DynaBgThread::BgThreadState

Definition at line 415 of file DynaBgThread.h.

Referenced by DynaShellCmdCreate::Exec(), DynaShellCmdDestroy::Exec(), DynaShellCmdScan::Exec(), DynaShellCmdBgtStart::Exec(), DynaShellCmdBgtStop::Exec(), and DynaShellCmdBgtGetState::Exec().

  {
    return m_eState;
  }

double DynaBgThread::getHz ( )

inline

Get control and monitoring hetrz rate.

Returns

Execution hertz.

Definition at line 457 of file DynaBgThread.h.

Referenced by DynaShellCmdBgtStart::Exec(), and DynaShellCmdBgtGetState::Exec().

  {
    return m_fHz;
  }

virtual DynaVServo* DynaBgThread::getNextVServo ( MapVServo::iterator &  iter, int &  nServoId  )

inlineprotectedvirtual

Get the next virtual servo.

Parameters

[in,out]	iter	Iterator at current position.
[out]	nServoId	Servo id.

Returns

Returns pointer to virtual servo on success. NULL on failure.

Definition at line 608 of file DynaBgThread.h.

  {
    if( m_mapVServo.size() == 0 )
    {
      return NULL;
    }

    ++iter;

    if( iter == m_mapVServo.end() )
    {
      iter = m_mapVServo.begin();
    }

    nServoId = iter->first;
    return &(iter->second);
  }

virtual DynaVServo* DynaBgThread::getPrevVServo ( MapVServo::iterator &  iter, int &  nServoId  )

inlineprotectedvirtual

Get the previous virtual servo.

Parameters

[in,out]	iter	Iterator at current position.
[out]	nServoId	Servo id.

Returns

Returns pointer to virtual servo on success. NULL on failure.

Definition at line 634 of file DynaBgThread.h.

  {
    if( m_mapVServo.size() == 0 )
    {
      return NULL;
    }

    if( iter == m_mapVServo.begin() )
    {
      iter = m_mapVServo.end();
    }

    --iter;

    nServoId = iter->first;
    return &(iter->second);
  }

DynaServo * DynaBgThread::getRegisteredServo ( int  nServoId )

protectedvirtual

Get registered servo.

Parameters

nServoId

Servo id.

Returns

Returns pointer to servo on success. NULL on failure.

Definition at line 810 of file DynaBgThread.cxx.

References DynaServo::GetServoId().

Referenced by WriteGoalPos().

{
  if( m_pChain != NULL )
  {
    return m_pChain->GetServo(nServoId);
  }
  else if( m_pServo != NULL )
  {
    return m_pServo->GetServoId() == nServoId? m_pServo: NULL;
  }
  else
  {
    return NULL;
  }
}

bool DynaBgThread::isServoBeingControlled ( int  nServoId )

inline

Checks if servo is currently being controlled.

Parameters

nServoId

Servo Id.

Returns

Returns true or false.

Definition at line 469 of file DynaBgThread.h.

References DynaVServo::StateControl.

  {
    MapVServo::iterator pos;

    if( (pos = m_mapVServo.find(nServoId)) != m_mapVServo.end() )
    {
      return pos->second.m_eState == DynaVServo::StateControl?  true: false;
    }
    else
    {
      return false;
    }
  }

void DynaBgThread::lock ( )

inlineprotected

Lock the background thread.

The calling thread will block while waiting for the mutex to become available. Once locked, the background thread will block.

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

Context:

Any.

Definition at line 533 of file DynaBgThread.h.

  {
    pthread_mutex_lock(&m_mutexSync);
  }

void DynaBgThread::monitorDynamics ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Execute servo dynamics monitoring background task.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 1129 of file DynaBgThread.cxx.

References BG_TRACE, DynaVServo::filterTorques(), DynaServo::GetServoId(), DynaServo::GetSoftTorqueThresholds(), DynaServo::HasSoftTorqueOverCond(), DynaVServo::m_bOverTorqueCond, DynaVServo::m_fTorqueOut, and DynaServo::SetSoftTorqueOverCond().

{
  int         nCurPos;
  int         nCurSpeed;
  int         nCurLoad;
  uint_t      uTorqueAvg;
  uint_t      uOverTorqueTh;
  uint_t      uClearTorqueTh;
  bool        bCurOverCond;

  // read the servo dynamics.
  if( pServo->ReadDynamics(&nCurPos, &nCurSpeed, &nCurLoad) >= 0 )
  {
    uTorqueAvg = (uint_t)fabs(pVServo->filterTorques(nCurLoad));

    pServo->GetSoftTorqueThresholds(uOverTorqueTh, uClearTorqueTh);

    bCurOverCond =  pServo->HasSoftTorqueOverCond();

    // set over torque limit condition
    if( uTorqueAvg > uOverTorqueTh )
    {
      pVServo->m_bOverTorqueCond = true;
      pServo->SetSoftTorqueOverCond(true);
    }

    // clear over torque limit condition
    else if( (uTorqueAvg < uClearTorqueTh) && bCurOverCond )
    {
      pVServo->m_bOverTorqueCond = false;
      pServo->SetSoftTorqueOverCond(false);
    }

    BG_TRACE(BG_TRACE_FILTER_DYNA, pServo->GetServoId(), "DYNAMICS",
        "pos=%d, speed=%d, cur_load=%d, filtered_load=%.2lf, overtorque=%s\n",
        nCurPos, nCurSpeed, nCurLoad, pVServo->m_fTorqueOut,
        (pServo->HasSoftTorqueOverCond()? "true": "false"));
  }
}

void DynaBgThread::monitorHealth ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Execute servo health monitoring background task.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 1169 of file DynaBgThread.cxx.

References BG_TRACE, and DynaServo::GetServoId().

{
  uint_t      uAlarms;
  int         nCurLoad;
  uint_t      uCurVolt;
  uint_t      uCurTemp;

  LOGDIAG4("BGThread: Monitoring health for servo %d.", pServo->GetServoId());

  pServo->ReadHealth(&uAlarms, &nCurLoad, &uCurVolt, &uCurTemp);

  //pVServo->filterTorques(nCurLoad);

  BG_TRACE(BG_TRACE_FILTER_HEALTH, pServo->GetServoId(), "HEALTH",
        "load=%d, voltage=%u, temperature=%u, alarms=0x%02x\n",
        nCurLoad, uCurVolt, uCurTemp, uAlarms);
}

int DynaBgThread::Pause ( )

virtual

Pause control and monitoring tasks.

The Pause()/Resume() actions provide a safe mechanism to modify the registered dynamixel chain and/or alter the time periods. When the thread is resumed, the time periods are automatically recalculated since the mix of servos in the chain could have changed.

Valid Current States:

BgThreadStateRunning BgThreadStatePaused

New State:

BgThreadStatePaused

Context:

Calling thread.

Definition at line 490 of file DynaBgThread.cxx.

References DYNA_ECODE_GEN, DYNA_LOG_ERROR, DYNA_OK, and DynaVServo::m_eState.

{
  int   rc;

  switch( m_eState )
  {
    case BgThreadStateRunning:
    case BgThreadStatePaused:
      changeState(BgThreadStatePaused);
      LOGDIAG3("Background thread paused.");
      rc = DYNA_OK;
      break;
    case BgThreadStateZombie:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "No background thread.");
      break;
    case BgThreadStateReady:
    case BgThreadStateExit:
    default:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "Background thread in invalid state %d.", m_eState);
      break;
  } 

  return rc;
}

void DynaBgThread::readyWait ( )

protected

Wait indefinitely in ready state.

Context:

Calling thread or background thread.

Definition at line 775 of file DynaBgThread.cxx.

References DynaVServo::lock(), DynaVServo::m_eState, DynaVServo::m_mutexSync, and DynaVServo::unlock().

Referenced by bgThread().

{
  lock();
  while( m_eState == BgThreadStateReady )
  {
    pthread_cond_wait(&m_condSync, &m_mutexSync);
  }
  unlock();
}

void DynaBgThread::RegisterChainAgent ( DynaChain *  pChain )

virtual

Register the Dynamixel chain for control and monitoring.

The chain will replace the currently monitored servo or chain.

If the thread is running, it should be paused or locked prior to calling this function.

The time periods are automatically recalculated.

Parameters

pChain

Dynamixel chain. May be NULL to deregister.

Definition at line 367 of file DynaBgThread.cxx.

References DYNA_ID_NONE, DynaChain::GetNumberInChain(), DynaServo::RegisterAgent(), and DynaVServo::setToleranceInTicks().

Referenced by DynaShellCmdCreate::Exec(), DynaShellCmdScan::Exec(), and MainInitArgs().

{
  int         nServoId;
  DynaServo  *pServo;
  int         iter;
  DynaVServo  vServo;

  UnregisterAgent();

  if( pChain != NULL )
  {
    m_pServo      = NULL;
    m_pChain      = pChain;
    m_uNumServos  = pChain->GetNumberInChain();

    for(nServoId = m_pChain->IterStart(&iter);
        nServoId != DYNA_ID_NONE;
        nServoId = m_pChain->IterNext(&iter))
    {
      if( (pServo = m_pChain->GetServo(nServoId)) != NULL )
      {
        m_mapVServo[nServoId] = vServo;
        m_mapVServo[nServoId].setToleranceInTicks(pServo, m_fTolerance);

        pServo->RegisterAgent(&m_agent, this);
      }
    }
  }

  setHz(m_fHz);

  m_iterDynamics      = m_mapVServo.begin();
  m_iterHealth        = m_mapVServo.begin();
  m_nHealthServoId    = m_iterHealth->first;

  LOGDIAG3("Registered chain: t_sched=%lu", m_TSched);
}

void DynaBgThread::RegisterServoAgent ( DynaServo *  pServo )

virtual

Register the Dynamixel servo for control and monitoring.

The servo will replace the currently monitored servo or chain.

If the thread is running, it should be paused or locked prior to calling this function.

The time periods are automatically recalculated.

Parameters

pServo

Dynamixel servo. May be NULL to deregister.

Definition at line 336 of file DynaBgThread.cxx.

References DynaServo::GetServoId(), DynaServo::RegisterAgent(), and DynaVServo::setToleranceInTicks().

{
  int         nServoId;
  DynaVServo  vServo;

  UnregisterAgent();

  if( pServo != NULL )
  {
    m_pServo      = pServo;
    m_pChain      = NULL;
    m_uNumServos  = 1;

    nServoId = pServo->GetServoId();

    m_mapVServo[nServoId] = vServo;

    m_mapVServo[nServoId].setToleranceInTicks(pServo, m_fTolerance);

    pServo->RegisterAgent(&m_agent, this);
  }

  setHz(m_fHz);

  m_iterDynamics      = m_mapVServo.begin();
  m_iterHealth        = m_mapVServo.begin();
  m_nHealthServoId    = m_iterHealth->first;

  LOGDIAG3("Registered servo: t_sched=%lu", m_TSched);
}

virtual void DynaBgThread::RegisterUserCallback ( void(*)(void *)  fnUserCb, void *  pUserArg  )

inlinevirtual

Register user-defined callback function.

Parameters

fnUserCb	User callback function.
pUserArg	User callback argument.

Definition at line 320 of file DynaBgThread.h.

  {
    m_fnUserCb = fnUserCb;
    m_pUserArg = pUserArg;
  }

int DynaBgThread::Resume ( )

virtual

Resume control and monitoring tasks.

Valid Current States:

BgThreadStatePaused

New State:

BgThreadStateRunning

Context:

Calling thread.

Definition at line 517 of file DynaBgThread.cxx.

References DYNA_ECODE_GEN, DYNA_LOG_ERROR, DYNA_OK, and DynaVServo::m_eState.

{
  int         rc;             // return code

  switch( m_eState )
  {
    case BgThreadStatePaused:
      gettimeofday(&m_tvSched, NULL);
      changeState(BgThreadStateRunning);
      LOGDIAG3("Background thread resumed.");
      rc = DYNA_OK;
      break;
    case BgThreadStateZombie:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "No background thread.");
      break;
    case BgThreadStateReady:
    case BgThreadStateRunning:
    case BgThreadStateExit:
    default:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "Background thread in invalid state %d.", m_eState);
      break;
  } 

  return rc;
}

int DynaBgThread::Run ( )

virtual

Run the background thread control and monitoring tasks.

Valid Current States:

BgThreadStateReady

New State:

BgThreadStateRunning

Context:

Calling thread.

Definition at line 435 of file DynaBgThread.cxx.

References DYNA_ECODE_GEN, DYNA_LOG_ERROR, DYNA_OK, and DynaVServo::m_eState.

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

{
  int         rc;   // return code

  switch( m_eState )
  {
    case BgThreadStateReady:
      gettimeofday(&m_tvSched, NULL);
      changeState(BgThreadStateRunning);
      LOGDIAG3("Background thread running.");
      rc = DYNA_OK;
      break;
    case BgThreadStateZombie:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "No background thread.");
      break;
    case BgThreadStateRunning:
    case BgThreadStatePaused:
    case BgThreadStateExit:
    default:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "Background thread in invalid state %d.", m_eState);
      break;
  } 

  return rc;
}

void DynaBgThread::sched ( long  lPeriod )

protectedvirtual

Schedule the Dynamixel background thread for the next task(s) to perform.

Context:

Background thread.

Definition at line 826 of file DynaBgThread.cxx.

References dt_usec().

Referenced by bgThread().

{
  static bool     bLogWarnings = false; // true to help tune 

  struct timeval  tvNow;        // now
  long            dt;           // delta time (microseconds)
  long            dtRemaining;  // remaining delta time to next scheduled event

  // now
  gettimeofday(&tvNow, NULL);

  // delta time between calls to this scheduler
  dt = dt_usec(tvNow, m_tvSched);

  // time remaining till next background thread event
  dtRemaining = lPeriod - dt;

  // wait
  if( dtRemaining > 0 )
  {
    timeWait(dtRemaining);
    gettimeofday(&tvNow, NULL);
    m_dTSched = dt_usec(tvNow, m_tvSched);
  }

  // slipped task period - don't wait
  else
  {
    // only log if really bad
    dtRemaining = fabs(dtRemaining);
    if( bLogWarnings && (dtRemaining >= lPeriod) )
    {
      LOGWARN("BGThread: Scheduled task slipped by %.2fmsec for servo %d",
        dtRemaining/1000.0, m_iterDynamics->first);
    }
    m_dTSched  = dt;
  }

  // save now
  m_tvSched = tvNow;
}

void DynaBgThread::setHz

(

double

fHz

)

Set new control and monitoring execution hertz rate.

Parameters

fHz	New execution hertz.

Definition at line 738 of file DynaBgThread.cxx.

Referenced by DynaShellCmdBgtStart::Exec().

{
  if( fHz < HZ_EXEC_MIN )
  {
    fHz = HZ_EXEC_MIN;
  }

  m_fHz   = fHz;
  m_TExec = (long)(1.0/m_fHz * 1000000.0);

  if( m_mapVServo.size() > 0 )
  {
    m_TSched = m_TExec / m_mapVServo.size();
  }
  else
  {
    m_TSched = 5000000;
  }
  if( m_TSched  < T_EXEC_MIN )
  {
    m_TSched = T_EXEC_MIN;
  }

  LOGDIAG3("BGThread: Hz: %.3lf, Scheduled period: %ldusecs.", m_fHz, m_TSched);
}

void DynaBgThread::setTolerance ( double  fTolerance )

inlineprotected

Set position tolerance.

The servo is considered at a goal position when it is within ± of the tolerance.

Definition at line 586 of file DynaBgThread.h.

  {
    m_fTolerance = fTolerance > 0.0? fTolerance: 0.0;
  }

int DynaBgThread::Stop ( )

virtual

Stop control and monitoring tasks.

Any servos being actively position controlled will be erased from the position control map.

Valid Current States:

BgThreadStateReady BgThreadStateRunning BgThreadStatePaused

New State:

BgThreadStateReady

Context:

Calling thread.

Definition at line 463 of file DynaBgThread.cxx.

References DYNA_ECODE_GEN, DYNA_LOG_ERROR, DYNA_OK, and DynaVServo::m_eState.

Referenced by DynaShellCmdCreate::Exec(), DynaShellCmdDestroy::Exec(), DynaShellCmdScan::Exec(), DynaShellCmdBgtStop::Exec(), and DynaShell::~DynaShell().

{
  int   rc;

  switch( m_eState )
  {
    case BgThreadStateReady:
    case BgThreadStateRunning:
    case BgThreadStatePaused:
      changeState(BgThreadStateReady);
      LOGDIAG3("Background thread stopped.");
      rc = DYNA_OK;
      break;
    case BgThreadStateZombie:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "No background thread.");
      break;
    case BgThreadStateExit:
    default:
      rc = -DYNA_ECODE_GEN;
      DYNA_LOG_ERROR(rc, "Background thread in invalid state %d.", m_eState);
      break;
  } 

  return rc;
}

void DynaBgThread::stopMotion ( DynaServo *  pServo )

protectedvirtual

Stop servo motion.

Parameters

pServo

Pointer to registered servo.

Definition at line 1101 of file DynaBgThread.cxx.

References DYNA_MODE_CONTINUOUS, DYNA_MODE_SERVO, DYNA_OK, DYNA_SPEED_CONT_STOP, and DynaServo::GetServoMode().

{
  int nCurOdPos;

  // stop servo
  switch( pServo->GetServoMode() )
  {
    //
    // In continuous mode: Set the goal speed to zero.
    //
    case DYNA_MODE_CONTINUOUS:
      pServo->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( pServo->ReadCurPos(&nCurOdPos) == DYNA_OK )
      {
        pServo->WriteGoalPos(nCurOdPos);
      }
      break;
  }
}

void DynaBgThread::stopPosCtl ( DynaServo *  pServo, DynaVServo *  pVServo  )

protectedvirtual

Stop servo position control.

Parameters

pServo	Pointer to registered servo.
pVServo	Pointer to associated virtual servo.

Context:

Background thread.

Definition at line 1084 of file DynaBgThread.cxx.

References BG_TRACE, DynaServo::GetOdometer(), DynaServo::GetServoId(), DynaVServo::m_eState, DynaVServo::m_nOdGoalPos, and DynaVServo::StateIdle.

Referenced by WriteGoalPos().

{
  int   nGoalOdPos;

  // stop servo movement
  stopMotion(pServo);

  nGoalOdPos            = pVServo->m_nOdGoalPos;
  pVServo->m_nOdGoalPos = pServo->GetOdometer();
  pVServo->m_eState     = DynaVServo::StateIdle;

  BG_TRACE(BG_TRACE_FILTER_POSCTL,
      pServo->GetServoId(), "STOP",
      "cur_od_pos=%d, goal_od_pos=%d\n",
      pVServo->m_nOdGoalPos, nOdGoalPos);
}

void DynaBgThread::terminateThread ( )

protected

Terminate the background thread.

This function does not return until the thread actually terminates.

Context:

Calling thread.

Definition at line 1254 of file DynaBgThread.cxx.

{
  changeState(BgThreadStateExit);
  pthread_join(m_thread, NULL);
}

void DynaBgThread::timeWait ( long  lMicroSecs )

protected

Wait until state change or time out.

Parameters

lMicroSec

Maximum wait duration (microseconds).

Context:

Calling thread or background thread.

Definition at line 785 of file DynaBgThread.cxx.

References DynaVServo::lock(), DynaVServo::m_mutexSync, and DynaVServo::unlock().

{
  struct timespec tsTimeout;
  long int        lSecs;

  if( lMicroSecs <= 0 )
  {
    return;
  }

  clock_gettime(CLOCK_REALTIME, &tsTimeout);

  tsTimeout.tv_nsec = lMicroSecs * 1000;
  if( tsTimeout.tv_nsec > 1000000000 )
  {
    tsTimeout.tv_sec  += 1000000000;
    tsTimeout.tv_nsec -= 1000000000;
  }

  // wait with timeout
  lock();
  pthread_cond_timedwait(&m_condSync, &m_mutexSync, &tsTimeout);
  unlock();
}

void DynaBgThread::unlock ( )

inlineprotected

Unlock the background thread.

The background thread will be available to run.

Context:

Any.

Definition at line 547 of file DynaBgThread.h.

  {
    pthread_mutex_unlock(&m_mutexSync);
  }

void DynaBgThread::UnregisterAgent ( )

virtual

Unregister any chain or servo from control and monitoring.

If the thread is running, it should be paused or locked prior to calling this function.

The time periods are automatically recalculated.

Parameters

pChain

Dynamixel chain. May be NULL to deregister.

Definition at line 405 of file DynaBgThread.cxx.

References DYNA_ID_NONE, and DynaServo::UnregisterAgent().

Referenced by DynaShellCmdCreate::Exec(), DynaShellCmdDestroy::Exec(), and DynaShellCmdScan::Exec().

{
  int         nServoId;
  DynaServo  *pServo;
  int         iter;

  if( m_pServo != NULL )
  {
    m_pServo->UnregisterAgent();
  }
  else if( m_pChain != NULL )
  {
    for(nServoId = m_pChain->IterStart(&iter);
        nServoId != DYNA_ID_NONE;
        nServoId = m_pChain->IterNext(&iter))
    {
      if( (pServo = m_pChain->GetServo(nServoId)) != NULL )
      {
        pServo->UnregisterAgent();
      }
    }
  }

  m_pServo      = NULL;
  m_pChain      = NULL;
  m_uNumServos  = 0;

  m_mapVServo.clear();
}

int DynaBgThread::WriteGoalPos ( int  nServoId, int  nOdGoalPos, void *  pUserArg  )

static

Start controlling servo to rotate it to the given goal position.

When the servo attains the given goal position within tolerance, the servo will be stopped. The servo speed to the goal position is controlled externally.

The servo is expected to be in continuous mode and to support 306 ° position information.

Parameters

nServoId	Servo id.
uGoalPos	Goal position in odometer virtual raw units.
pUserArg	Pointer to this class instance.

Returns

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

Definition at line 545 of file DynaBgThread.cxx.

References BG_TRACE, DYNA_ECODE_NO_SERVO, DYNA_MODE_CONTINUOUS, DYNA_MODE_SERVO, DYNA_OK, DYNA_SPEED_CONT_STOP, DynaServo::GetCurSpeed(), DynaServo::GetOdometer(), getRegisteredServo(), DynaServo::GetServoMode(), DynaServo::IsOdometerReversed(), DynaVServo::lock(), DynaVServo::m_eState, m_mapVServo, DynaVServo::m_nGoalSpeed, DynaVServo::m_nOdGoalPos, DynaVServo::m_nTolerance, DynaVServo::m_pidPos, DynaPidPos::SpecifySetPoint(), DynaVServo::StateControl, stopPosCtl(), and DynaVServo::unlock().

{
  DynaBgThread         *pThis = (DynaBgThread *)pUserArg;
  MapVServo::iterator   iter;
  DynaServo            *pServo;
  DynaVServo           *pVServo;
  int                   nOdCurPos;
  int                   nGoalSpeed;
  int                   rc;

  pServo = pThis->getRegisteredServo(nServoId);

  if( pServo == NULL )
  {
    rc = -DYNA_ECODE_NO_SERVO;
  }

  iter = pThis->m_mapVServo.find(nServoId);

  if( iter == pThis->m_mapVServo.end() )
  {
    rc = -DYNA_ECODE_NO_SERVO;
  }
  
  else
  {
    pVServo = &(iter->second);
    pVServo->lock();
    nOdCurPos = pServo->GetOdometer();
    pVServo->m_eState = DynaVServo::StateControl;
    pVServo->m_nOdGoalPos = nOdGoalPos;
    nGoalSpeed = pVServo->m_nGoalSpeed;

    pVServo->m_pidPos.SpecifySetPoint(nOdCurPos,
                                      nOdGoalPos,
                                      nGoalSpeed,
                                      pServo->GetCurSpeed(),
                                      pServo->IsOdometerReversed(),
                                      true);

    pVServo->unlock();

    BG_TRACE(BG_TRACE_FILTER_POSCTL|BG_TRACE_FILTER_TORQCTL, nServoId,
          "WriteGoalPos",
          "speedlimit=%d, goalpos=%d, curpos=%d, tol=%d.\n",
          nGoalSpeed,
          nOdGoalPos,
          nOdCurPos,
          pVServo->m_nTolerance);

    rc = DYNA_OK;
  }

  return rc;
}

---

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

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