kuon::KuonRobot Class Reference

Kuon robotic manipulator plus accesories class. More...

#include <kuonRobot.h>

Public Types
enum	AsyncTaskId { AsyncTaskIdNone }
	Asynchronous task id. More...
typedef std::map< int, KuonRobotJoint >	MapRobotJoints
	Map of robot joints. More...
typedef std::map< std::string, int >	IMapRobotJoints
	Indirect map of robot joints. More...

Public Member Functions
	KuonRobot (bool bNoExec=false)
	Default initialization constructor. More...
virtual	~KuonRobot ()
	Destructor.
int	connect (const std::string &strDevMotorCtlr0=KuonDevMotorCtlr0, const std::string &strDevMotorCtlr1=KuonDevMotorCtlr1, int nBaudRateMotorCtlr=KuonBaudRateMotorCtlr)
	Connect to Kuon. More...
int	disconnect ()
	Disconnect from Kuon. More...
int	estop ()
	Emergency stop. More...
int	freeze ()
	Freeze arm and accessories at current position. More...
int	release ()
	Release arm and accessories. More...
int	clearAlarms ()
	Attempt to clear all motor alarms in all kinematic chains. More...
void	resetEStop ()
	Reset (clears) emergency stop condition. More...
float	setGovernor (float fGovernor)
	Set speed limit governor value. More...
float	incrementGovernor (float fDelta)
	Increment/decrement speed limit governor value. More...
int	setBrake (float fBrake)
	Set robot's auto-brake value. More...
int	setSlew (float fSlew)
	Set robot's power slew value. More...
int	setSpeed (double fSpeedLeft, double fSpeedRight, units_t units=units_norm)
	Set robot's left and right motor speeds. More...
int	move (KuonWheelTrajectoryPoint &trajectoryPoint)
	Move platform through trajectory point. More...
int	velocityToRawSpeed (double fVelocity, units_t units=units_norm)
	Convert velocity to raw motor speed value. More...
int	getRobotStatus (KuonRobotStatus &robotStatus)
	Get the robot current status. More...
int	getJointState (KuonJointStatePoint &jointStatePoint)
	Get the joint states of a kinematic chain. More...
int	getTrajectoryState (KuonWheelTrajectoryFeedback &trajFeedback)
	Get trajectory feedback. More...
void	setRobotMode (KuonRobotMode eRobotMode)
	Set robot's operational mode. More...
void	cancelAsyncTask ()
	Cancel any asynchronous task. More...
KuonAsyncTaskState	getAsyncState ()
	Get the current asynchronous task state. More...
int	getAsyncRc ()
	Get the last asynchronous task return code. More...
bool	isConnected ()
	Test if connected to Kuon hardware. More...
void	getVersion (int &nVerMajor, int &nVerMinor, int &nRevision)
	Get the Kuon robotic arm hardware version number. More...
std::string	getVersion ()
	Get the Kuon robotic arm hardware version string. More...
KuonDesc *	getKuonDesc ()
	Get the Kuon product description. More...
int	getProdId ()
	Convenience function to get this Kuon description's base product id. More...
std::string	getProdName ()
	Convenience function to get this Kuon description's base product name. More...
std::string	getFullProdBrief ()
	Get the Kuon full brief descirption. More...
int	isDescribed ()
	Test if robot is fully described via configuration XML. More...
int	isEStopped ()
	Test if robot is current emergency stopped. More...
int	areMotorsPowered ()
	Test if robot motor are currently being driven (powered). More...
bool	isInMotion ()
	Test if any joint in any of the kinematic chains is moving. More...
int	isAlarmed ()
	Test if robot is alarmed. More...
float	getGovernor ()
	Get current speed limit governor setting. More...
float	getBrake ()
	Get current brake setting. More...
float	getSlew ()
	Get current slew setting. More...
void	getVelocitySetPoints (double &fSpeedLeft, double &fSpeedRight, units_t units=units_norm)
	Get the current left and right side motor velocity set points. More...
KuonRobotJoint *	getBaseJointAt (int index)
	Get robotic joint in arm+ee kinematic chain at the given index. More...
KuonRobotJoint *	getBaseJoint (std::string &strName)
	Get robotic joint in arm+ee kinematic chain. More...
KuonRobotJoint *	getBaseJoint (int nMotorId)
	Get robotic joint in arm+ee kinematic chain. More...

Static Public Attributes
static const float	GovernorDft = 0.20
	speed limit governor start-up default
static const float	BrakeDft = 0.20
	brake start-up default
static const float	SlewDft = 0.10
	slew start-up default

Protected Member Functions
int	convertSpecs ()
	Convert specification(s) to operational parameters. More...
int	addRobotJoint (KuonSpecMotor_T *pSpecJoint, MapRobotJoints &kin, IMapRobotJoints &imap)
	Add a joint to robot's kinematic chain. More...
int	configForOperation ()
	Configure Kuon for normal operation. More...
int	createAsyncThread ()
	Create the asynchronous thread. More...

Static Protected Member Functions
static void *	asyncThread (void *pArg)
	Asynchronous operation thread. More...

Protected Attributes
bool	m_bNoExec
	do [not] execute physical movements
KuonDesc	m_descKuon
	Kuon description
KuonRobotMode	m_eRobotMode
	robot operating mode
bool	m_bIsEStopped
	arm is [not] emergency stopped
bool	m_bAlarmState
	robot is [not] alarmed
bool	m_bAreMotorsPowered
	arm motor are [not] driven
float	m_fGovernor
	speed limit governor setting
float	m_fBattery
	battery energy level
float	m_fBrake
	motor braking
float	m_fSlew
	power slewing
int	m_nGovernSpeedMin
	minimum governed speed limit
int	m_nGovernSpeedMax
	maximum governed speed limit
int	m_nSetPtSpeedLeft
	left motors raw speed set point
int	m_nSetPtSpeedRight
	right motors raw speed set point
int	m_fdMotorCtlr0
	motor controller 0 file descriptor
int	m_fdMotorCtlr1
	motor controller 1 file descriptor
MapRobotJoints	m_kinBase
	robot base kin
IMapRobotJoints	m_imapBase
	robot base indirect kin map
KuonWheelTrajectoryPoint	m_lastTrajBase
	last trajectory point for base
KuonAsyncTaskState	m_eAsyncTaskState
	asynchronous task state
int	m_rcAsyncTask
	last async task return code
AsyncTaskId	m_eAsyncTaskId
	asynchronous task id
void *	m_pAsyncTaskArg
	asynchronous argument
pthread_t	m_threadAsync
	async pthread identifier

Detailed Description

Kuon robotic manipulator plus accesories class.

Includes up to 3 kinematic chains.

Definition at line 83 of file kuonRobot.h.

Member Typedef Documentation

typedef std::map<std::string, int> kuon::KuonRobot::IMapRobotJoints

Indirect map of robot joints.

key:	joint name
mapped type:	motor id

Definition at line 111 of file kuonRobot.h.

typedef std::map<int, KuonRobotJoint> kuon::KuonRobot::MapRobotJoints

Map of robot joints.

key:	motor id
mapped type:	joint data

Note

Joint order is critical. Ascending motor ids keeps map in order, but if this cannot be guaranteed, then change strategy.

Definition at line 101 of file kuonRobot.h.

Member Enumeration Documentation

enum kuon::KuonRobot::AsyncTaskId

Asynchronous task id.

Enumerator
AsyncTaskIdNone	no task

Definition at line 116 of file kuonRobot.h.

    {
      AsyncTaskIdNone,        ///< no task

      // add others here, as needed
    };

Constructor & Destructor Documentation

KuonRobot::KuonRobot

(

bool

bNoExec = false

)

Default initialization constructor.

Parameters

bNoExec

Do [not] execute arm physical movements. All commands and responses are supported but the lower level arm movement commands will not be issued.

Definition at line 143 of file kuonRobot.cxx.

References kuon::KUON_OK, kuon::KuonAsyncTaskStateIdle, and kuon::KuonRobotModeAuto.

{
  // state
  m_bNoExec           = bNoExec;
  m_eRobotMode        = KuonRobotModeAuto;
  m_bIsEStopped       = false;
  m_bAlarmState       = false;
  m_fBattery          = 0.0;
  m_fBrake            = BrakeDft;
  m_fSlew             = SlewDft;
  m_nSetPtSpeedLeft   = 0;
  m_nSetPtSpeedRight  = 0;

  setGovernor(GovernorDft);

  // motor controllers i/f
  m_fdMotorCtlr0      = -1;   // front or rear (cannot tell with RS160D ctlrs)
  m_fdMotorCtlr1      = -1;   // front or rear (cannot tell with RS160D ctlrs)

  // asynchronous task control
  m_eAsyncTaskState   = KuonAsyncTaskStateIdle;
  m_rcAsyncTask       = KUON_OK;
  m_eAsyncTaskId      = AsyncTaskIdNone;
  m_pAsyncTaskArg     = NULL;
}

Member Function Documentation

int KuonRobot::addRobotJoint ( KuonSpecMotor_T *  pSpecJoint, MapRobotJoints &  kin, IMapRobotJoints &  imap  )

protected

Add a joint to robot's kinematic chain.

Parameters

[in]	pSpecJoint	Pointer to joint spcecification.
[in]	pSpecMotor	Pointer to motor spcecification.
[out]	kin	Modified kinematics chain of joints.
[out]	imap	Indirect map of kinematic chain.

Definition at line 681 of file kuonRobot.cxx.

References kuon::KUON_OK, kuon::KuonJointTypeContinuous, kuon::KuonRobotJoint::m_eJointType, kuon::KuonRobotJoint::m_fGearRatio, kuon::KuonSpecMotor_T::m_fGearRatio, kuon::KuonRobotJoint::m_fTicksPerMotorRad, kuon::KuonRobotJoint::m_fTicksPerWheelRad, kuon::KuonRobotJoint::m_fTireRadius, kuon::KuonSpecMotor_T::m_fTireRadius, kuon::KuonSpecMotor_T::m_nDir, kuon::KuonRobotJoint::m_nMotorCtlrId, kuon::KuonSpecMotor_T::m_nMotorCtlrId, kuon::KuonRobotJoint::m_nMotorDir, kuon::KuonRobotJoint::m_nMotorId, kuon::KuonSpecMotor_T::m_nMotorId, kuon::KuonRobotJoint::m_nMotorIndex, kuon::KuonSpecMotor_T::m_nMotorIndex, kuon::KuonRobotJoint::m_strName, and kuon::KuonSpecMotor_T::m_strName.

{
  int               nMotorId;     // master servo id
  KuonRobotJoint    joint;        // robotic joint

  // master servo id associated with joint
  nMotorId = pSpecMotor->m_nMotorId;

  //
  // Populate joint data.
  //
  joint.m_strName             = pSpecMotor->m_strName;
  joint.m_nMotorId            = nMotorId;
  joint.m_nMotorCtlrId        = pSpecMotor->m_nMotorCtlrId;
  joint.m_nMotorIndex         = pSpecMotor->m_nMotorIndex;
  joint.m_nMotorDir           = pSpecMotor->m_nDir;
  joint.m_eJointType          = KuonJointTypeContinuous;
  joint.m_fGearRatio          = pSpecMotor->m_fGearRatio;
  joint.m_fTireRadius         = pSpecMotor->m_fTireRadius;
  joint.m_fTicksPerMotorRad   = 0.0;
  joint.m_fTicksPerWheelRad   = 0.0;

  //
  // Sanity checks.
  //

  //
  // Add to kinematic chain.
  //
  kin[nMotorId]         = joint;      // kinematic chain
  imap[joint.m_strName] = nMotorId;   // indirect map by joint name

  return KUON_OK;
}

int kuon::KuonRobot::areMotorsPowered ( )

inline

Test if robot motor are currently being driven (powered).

Returns

Returns true or false.

Definition at line 509 of file kuonRobot.h.

References m_bAreMotorsPowered.

    {
      return m_bAreMotorsPowered;
    }

void * KuonRobot::asyncThread ( void *  pArg )

staticprotected

Asynchronous operation thread.

Parameters

pArg	Thread argument (point to KuonRobot object).

Returns

Returns NULL on thread exit.

Definition at line 790 of file kuonRobot.cxx.

References kuon::KUON_ECODE_BAD_VAL, kuon::KuonAsyncTaskStateIdle, m_eAsyncTaskId, m_eAsyncTaskState, m_pAsyncTaskArg, and m_rcAsyncTask.

{
  KuonRobot *pThis = (KuonRobot *)pArg;
  int       oldstate;
  int       rc;

  pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, &oldstate);
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldstate);
  //pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, &oldstate);

  LOGDIAG3("Async robot task thread created.");

  //
  // Execute asychronous task.
  //
  // For now, only calibrate task is supported asynchronously.
  //
  switch( pThis->m_eAsyncTaskId )
  {
    // Unknown task id.
    default:
      LOGERROR("Unknown async task id = %d.", (int)pThis->m_eAsyncTaskId);
      rc = -KUON_ECODE_BAD_VAL;
      break;
  }

  // freeze robot at current calibrated or aborted position.
  //pThis->freeze();  disable, so that goto zero pt in calibration finsishes

  pThis->m_eAsyncTaskId     = AsyncTaskIdNone;
  pThis->m_pAsyncTaskArg    = NULL;
  pThis->m_rcAsyncTask      = rc;
  pThis->m_eAsyncTaskState  = KuonAsyncTaskStateIdle;

  LOGDIAG3("Async robot task thread exited.");

  return NULL;
}

void KuonRobot::cancelAsyncTask

(

)

Cancel any asynchronous task.

Note

There may be a little delay between canceling an async task and the task actually stopping.

Definition at line 764 of file kuonRobot.cxx.

References kuon::KUON_ECODE_INTR, and kuon::KuonAsyncTaskStateIdle.

Referenced by setRobotMode().

{
  MapRobotJoints::iterator  iter;

  if( m_eAsyncTaskState != KuonAsyncTaskStateIdle )
  {
    // cancel thread
    pthread_cancel(m_threadAsync);
    pthread_join(m_threadAsync, NULL);

    // cleanup
    switch( m_eAsyncTaskId )
    {
      default:
        break;
    }

    // clear state
    m_eAsyncTaskId    = AsyncTaskIdNone;
    m_pAsyncTaskArg   = NULL;
    m_rcAsyncTask     = -KUON_ECODE_INTR;
    m_eAsyncTaskState = KuonAsyncTaskStateIdle;
    LOGDIAG3("Async task canceled.");
  }
}

int KuonRobot::clearAlarms

(

)

Attempt to clear all motor alarms in all kinematic chains.

Note

Not all alarms are clearable (e.g. temperature).

Definition at line 362 of file kuonRobot.cxx.

References kuon::KUON_OK, and KUON_TRY_CONN.

{
  KUON_TRY_CONN();

  return KUON_OK;
}

int KuonRobot::configForOperation ( )

protected

Configure Kuon for normal operation.

The arm, end effector, and accessory effectors are configured for normal operation.

Definition at line 718 of file kuonRobot.cxx.

References kuon::KUON_OK, and kuon::KuonRobotModeAuto.

{
  m_eRobotMode        = KuonRobotModeAuto;
  m_bIsEStopped       = false;
  m_bAlarmState       = false;
  m_fBattery          = 0.0;
  m_fBrake            = BrakeDft;
  m_fSlew             = SlewDft;
  m_nSetPtSpeedLeft   = 0;
  m_nSetPtSpeedRight  = 0;

  setGovernor(GovernorDft);
  setBrake(BrakeDft);
  setSlew(SlewDft);
  setSpeed(0.0, 0.0);

  return KUON_OK;
}

int KuonRobot::connect	(	const std::string &	strDevMotorCtlr0 = KuonDevMotorCtlr0,
		const std::string &	strDevMotorCtlr1 = KuonDevMotorCtlr1,
		int	nBaudRateMotorCtlr = KuonBaudRateMotorCtlr
	)

Connect to Kuon.

Parameters

strDevMotorCtlr0	Motor controller serial device name 0.
strDevMotorCtlr1	Motor controller serial device name 1.
nBaudRateMotorCtlr	Motor controllers baud rate.

Definition at line 174 of file kuonRobot.cxx.

References kuon::getRealDeviceName(), kuon::KUON_ECODE_BAD_OP, kuon::KUON_ECODE_MOT_CTLR, and kuon::KUON_OK.

{
  string  strDevName0;  // real device name 0
  string  strDevName1;  // real device name 1
  int     rc;           // return code

  //
  // Need a robot description before preceeding.
  //
  if( !m_descKuon.isDescribed() )
  {
    LOGERROR("Undefined Kuon description - "
             "don't know how to initialized properly.");
    return -KUON_ECODE_BAD_OP;
  }

  // get the real device names, not any symbolic links
  strDevName0 = getRealDeviceName(strDevMotorCtlr0);
  strDevName1 = getRealDeviceName(strDevMotorCtlr1);

  // RDK Hack
  // Possible race condition where controller is not fully powered on.
  // Wait a little bit.
  // RDK Hack
  long usec = 3000000;
  LOGDIAG2("Waiting for motor controllers to power-up");
  usleep(usec);
  // RDK Hack

  // Open motor controller 0.
  //
  if( RS160DOpenConnection(strDevName0.c_str(), &m_fdMotorCtlr0) < 0 )
  {
    LOGERROR("%s: Failed to open motor controller 0.", strDevName0.c_str());
    rc = -KUON_ECODE_MOT_CTLR;
  }

  //
  // Configure motor controller 0.
  //
  else if( RS160DSetToSerial(m_fdMotorCtlr0) < 0 )
  {
    LOGERROR("%s: Failed to configure motor controller 0.",
        strDevName0.c_str());
    rc = -KUON_ECODE_MOT_CTLR;
  }

  //
  // Open motor controller 1.
  //
  if( RS160DOpenConnection(strDevName1.c_str(), &m_fdMotorCtlr1) < 0 )
  {
    LOGERROR("%s: Failed to open motor controller 1.", strDevName1.c_str());
    rc = -KUON_ECODE_MOT_CTLR;
  }

  // 
  // Configure motor controller 1.
  //
  else if( RS160DSetToSerial(m_fdMotorCtlr1) < 0 )
  {
    LOGERROR("%s: Failed to configure motor controller 1.",
        strDevName1.c_str());
    rc = -KUON_ECODE_MOT_CTLR;
  }

  // 
  // Convert fixed specifications to operational parameters.
  //
  else if( (rc = convertSpecs()) < 0 )
  {
    LOGERROR("Failed to convert product specifications to "
             "operational parameters.");
  }

  //
  // Configure for operation.
  //
  else if( (rc = configForOperation()) < 0 )
  {
    LOGERROR("Failed to configure for operation.");
  }

  // success
  else
  {
    LOGDIAG1("Connected to Kuon with controllers = (%s, %s).",
      strDevMotorCtlr0.c_str(), strDevMotorCtlr1.c_str());

    rc = KUON_OK;
  }

  // undo
  if( rc < 0 )
  {
    disconnect();
  }

  return rc;
}

int KuonRobot::convertSpecs ( )

protected

Convert specification(s) to operational parameters.

The specifications must be defined and the connecton to the Kuon hardware must be open.

There are up to 3 kinematic chains, given the specifications and the hardware:

• arm + end effector
• equipment deck effector
• auxiliary effector

Note

A kinematic chain differs from the dynamixel chain. Kuon supports one dynamixel bus in which all motor hang. A dynamixel chain defines this communication bus. Motors on the bus can be associated with any of the above three kinematic chains.

Definition at line 648 of file kuonRobot.cxx.

References kuon::KuonSpec::getMotorSpecAt(), kuon::KuonDescBase::getNumMotors(), kuon::KUON_OK, kuon::KuonSpecMotor_T::m_nMotorId, and kuon::KuonDescBase::m_spec.

{
  KuonDescBase      *pDescBase;
  KuonSpecMotor_T   *pSpecMotor;
  KuonRobotJoint    *pMotor;
  int               i;
  int               rc;

  m_kinBase.clear();
  m_imapBase.clear();

  pDescBase = m_descKuon.getBaseDesc();

  //
  // Build up base kinematic chain.
  //
  for(i = 0; i < pDescBase->getNumMotors(); ++i)
  {
    // joint specification
    pSpecMotor = pDescBase->m_spec.getMotorSpecAt(i);

    // add robotic 'joint'
    if( (rc = addRobotJoint(pSpecMotor, m_kinBase, m_imapBase)) < 0 )
    {
      LOGERROR("Motor %d: Cannot add to kinematic chain.",
          pSpecMotor->m_nMotorId);
      return rc;
    }
  }
 
  return KUON_OK;
}

int KuonRobot::createAsyncThread ( )

protected

Create the asynchronous thread.

Context:

Calling thread.

Definition at line 737 of file kuonRobot.cxx.

References kuon::KUON_ECODE_SYS, kuon::KUON_OK, kuon::KuonAsyncTaskStateIdle, and kuon::KuonAsyncTaskStateWorking.

{
  int   rc;

  m_eAsyncTaskState = KuonAsyncTaskStateWorking;

  rc = pthread_create(&m_threadAsync, NULL, KuonRobot::asyncThread, (void*)this);
 
  if( rc == 0 )
  {
    rc = KUON_OK;
  }

  else
  {
    m_eAsyncTaskState = KuonAsyncTaskStateIdle;
    LOGSYSERROR("pthread_create()");
    m_rcAsyncTask   = -KUON_ECODE_SYS;
    m_eAsyncTaskId  = AsyncTaskIdNone;
    m_pAsyncTaskArg = NULL;
    rc = m_rcAsyncTask;
  }

  return rc;
}

int KuonRobot::disconnect

(

)

Disconnect from Kuon.

Definition at line 277 of file kuonRobot.cxx.

References kuon::KUON_OK.

{
  bool  bWasConn = false;

  if( isConnected() )
  {
    bWasConn = true;
    RS160DEStop(m_fdMotorCtlr0, m_fdMotorCtlr1);
  }

  if( m_fdMotorCtlr0 >= 0 )
  {
    RS160DClose(m_fdMotorCtlr0);
    m_fdMotorCtlr0 = -1;
  }

  if( m_fdMotorCtlr1 >= 0 )
  {
    RS160DClose(m_fdMotorCtlr1);
    m_fdMotorCtlr1 = -1;
  }

  if( bWasConn )
  {
    LOGDIAG1("Disconnected from Kuon.");
  }

  return KUON_OK;
}

int KuonRobot::estop

(

)

Emergency stop.

All motor will stop driving, so arm or accessories may fall.

Definition at line 307 of file kuonRobot.cxx.

References kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NO_EXEC.

{
  KUON_TRY_NO_EXEC();
  KUON_TRY_CONN();

  RS160DEStop(m_fdMotorCtlr0, m_fdMotorCtlr1);

  setBrake(1.0);

  m_bIsEStopped       = true;
  m_bAlarmState       = true;

  m_lastTrajBase.clear();

  LOGDIAG3("Kuon emergency stopped.");

  return KUON_OK;
}

int KuonRobot::freeze

(

)

Freeze arm and accessories at current position.

Motors are still being driven.

Definition at line 326 of file kuonRobot.cxx.

References kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NO_EXEC.

{
  KUON_TRY_NO_EXEC();
  KUON_TRY_CONN();

  m_lastTrajBase.clear();

  // stop 
  setSpeed(0.0, 0.0);

  // set 'parking' brake
  setBrake(1.0);

  LOGDIAG3("Kuon frozen at current position.");

  return KUON_OK;
}

int kuon::KuonRobot::getAsyncRc ( )

inline

Get the last asynchronous task return code.

Returns

Returns KUON_OK when task terminated successfully. Otherwise, returns < 0 Kuon Error Codes.

Definition at line 393 of file kuonRobot.h.

References m_rcAsyncTask.

    {
      return m_rcAsyncTask;
    }

KuonAsyncTaskState kuon::KuonRobot::getAsyncState ( )

inline

Get the current asynchronous task state.

Returns

KuonAsyncTaskState enum value.

Definition at line 381 of file kuonRobot.h.

References m_eAsyncTaskState.

    {
      return m_eAsyncTaskState;
    }

KuonRobotJoint* kuon::KuonRobot::getBaseJoint ( std::string &  strName )

inline

Get robotic joint in arm+ee kinematic chain.

Parameters

strName

Joint name (key).

Returns

If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 601 of file kuonRobot.h.

References m_imapBase, and m_kinBase.

    {
      IMapRobotJoints::iterator  iter;

      if( (iter = m_imapBase.find(strName)) != m_imapBase.end() )
      {
        return &(m_kinBase[iter->second]);
      }
      else
      {
        return NULL;
      }
    }

KuonRobotJoint* kuon::KuonRobot::getBaseJoint ( int  nMotorId )

inline

Get robotic joint in arm+ee kinematic chain.

Parameters

nMotorId

Motor id (primary key).

Returns

If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 622 of file kuonRobot.h.

References m_kinBase.

    {
      MapRobotJoints::iterator  iter;

      if( (iter = m_kinBase.find(nMotorId)) != m_kinBase.end() )
      {
        return &(iter->second);
      }
      else
      {
        return NULL;
      }
    }

KuonRobotJoint* kuon::KuonRobot::getBaseJointAt ( int  index )

inline

Get robotic joint in arm+ee kinematic chain at the given index.

Parameters

index

Joint index.

Returns

If found, returns pointer to joint. Otherwise returns NULL.

Definition at line 589 of file kuonRobot.h.

References m_kinBase.

    {
      return index < m_kinBase.size()? &m_kinBase.at(index): NULL;
    }

float kuon::KuonRobot::getBrake ( )

inline

Get current brake setting.

Returns

Return value.

Definition at line 551 of file kuonRobot.h.

References m_fBrake.

    {
      return m_fBrake;
    }

std::string kuon::KuonRobot::getFullProdBrief ( )

inline

Get the Kuon full brief descirption.

Returns

Returns product brief description.

Definition at line 479 of file kuonRobot.h.

References kuon::KuonDesc::getFullProdBrief(), and m_descKuon.

    {
      return m_descKuon.getFullProdBrief();
    }

float kuon::KuonRobot::getGovernor ( )

inline

Get current speed limit governor setting.

Returns

Return value.

Definition at line 541 of file kuonRobot.h.

References m_fGovernor.

    {
      return m_fGovernor;
    }

int KuonRobot::getJointState

(

KuonJointStatePoint &

jointStatePoint

)

Get the joint states of a kinematic chain.

Parameters

[in,out]

jointStatePoint

Joint state point.

Definition at line 579 of file kuonRobot.cxx.

References kuon::KuonJointStatePoint::append(), kuon::KuonJointStatePoint::clear(), kuon::KUON_OK, KUON_TRY_CONN, kuon::KuonJointState::m_fBrake, kuon::KuonJointState::m_fEffort, kuon::KuonJointState::m_fOdometer, kuon::KuonJointState::m_fPe, kuon::KuonJointState::m_fPm, kuon::KuonJointState::m_fPosition, kuon::KuonJointState::m_fSlew, kuon::KuonJointState::m_fVelocity, kuon::KuonJointState::m_fVelocityMps, kuon::KuonJointState::m_nEncoder, kuon::KuonJointState::m_nMotorId, kuon::KuonRobotJoint::m_nMotorIndex, kuon::KuonJointState::m_nSpeed, kuon::KuonRobotJoint::m_strName, kuon::KuonJointState::m_strName, and kuon::KuonJointStatePoint::setKinematicChainName().

Referenced by incrementGovernor().

{
  MapRobotJoints::iterator  iter;       // kinematic chain iterator
  int                       nMotorId;   // master servo id
  KuonRobotJoint           *pMotor;     // robotic motor joint
  KuonJointState            jointState; // working joint state
  byte_t                    uMask;      // working bit mask
  int                       i;          // working index

  KUON_TRY_CONN();

  jointStatePoint.clear();

  jointStatePoint.setKinematicChainName("base");

  //
  // Build joint state point.
  //
  for(iter = m_kinBase.begin(); iter != m_kinBase.end(); ++iter)
  {
    nMotorId  = iter->first;
    pMotor    = &(iter->second);

    // identifiers
    jointState.m_strName  = pMotor->m_strName;
    jointState.m_nMotorId = nMotorId;

    // positions (unknown without encoders)
    jointState.m_fPosition = 0.0;
    jointState.m_fOdometer = 0.0;
    jointState.m_nEncoder  = 0;

    // velocities
    jointState.m_fVelocity    = 0.0;
    jointState.m_fVelocityMps = 0.0;
    if( pMotor->m_nMotorIndex == RS160D_MOTOR_LEFT_ID )
    {
      jointState.m_nSpeed = m_nSetPtSpeedLeft;
    }
    else
    {
      jointState.m_nSpeed = m_nSetPtSpeedRight;
    }

    // torques and powers
    jointState.m_fEffort  = 0.0;
    jointState.m_fPe      = 0.0;
    jointState.m_fPm      = 0.0;

    // braking and power-up slewing
    jointState.m_fBrake   = m_fBrake;
    jointState.m_fSlew    = m_fSlew;

    // add state
    jointStatePoint.append(jointState);
  }

  return KUON_OK;
}

KuonDesc* kuon::KuonRobot::getKuonDesc ( )

inline

Get the Kuon product description.

Returns

Returns pointer to description.

Definition at line 447 of file kuonRobot.h.

References m_descKuon.

    {
      return &m_descKuon;
    }

int kuon::KuonRobot::getProdId ( )

inline

Convenience function to get this Kuon description's base product id.

Returns

Returns product id. See KuonProdId.

Definition at line 458 of file kuonRobot.h.

References kuon::KuonDesc::getBaseDesc(), kuon::KuonDescBase::getProdId(), and m_descKuon.

    {
      return m_descKuon.getBaseDesc()->getProdId();
    }

std::string kuon::KuonRobot::getProdName ( )

inline

Convenience function to get this Kuon description's base product name.

Returns

Returns product name. See KuonProdName.

Definition at line 469 of file kuonRobot.h.

References kuon::KuonDesc::getBaseDesc(), kuon::KuonDescBase::getProdName(), and m_descKuon.

    {
      return m_descKuon.getBaseDesc()->getProdName();
    }

int KuonRobot::getRobotStatus

(

KuonRobotStatus &

robotStatus

)

Get the robot current status.

Parameters

[in,out]

robotStatus

Robot status.

Definition at line 526 of file kuonRobot.cxx.

References kuon::KuonRobotStatus::clear(), kuon::KUON_ECODE_ALARMED, kuon::KUON_ECODE_ESTOP, kuon::KUON_OK, kuon::KuonTriStateFalse, kuon::KuonTriStateTrue, kuon::KuonRobotStatus::m_eAreDrivesPowered, kuon::KuonRobotStatus::m_eIsEStopped, kuon::KuonRobotStatus::m_eIsInError, kuon::KuonRobotStatus::m_eIsInMotion, kuon::KuonRobotStatus::m_eIsMotionPossible, kuon::KuonRobotStatus::m_eRobotMode, kuon::KuonRobotStatus::m_fBattery, kuon::KuonRobotStatus::m_fGovernor, kuon::KuonMotorHealth::m_fTemperature, kuon::KuonMotorHealth::m_fVoltage, kuon::KuonRobotStatus::m_nErrorCode, kuon::KuonMotorHealth::m_nMotorId, kuon::KuonMotorHealth::m_strName, kuon::KuonMotorHealth::m_uAlarms, and kuon::KuonRobotStatus::m_vecMotorHealth.

Referenced by incrementGovernor().

{
  MapRobotJoints::iterator  iter;       // kinematic chain iterator
  int                       nMotorId;   // motor id
  bool                      bIsMoving;  // robot is [not] moving
  KuonMotorHealth           health;     // motor health

  robotStatus.clear();

  robotStatus.m_eRobotMode        = m_eRobotMode;
  robotStatus.m_eIsEStopped       = m_bIsEStopped?
                                            KuonTriStateTrue: KuonTriStateFalse;
  robotStatus.m_eAreDrivesPowered = m_bAreMotorsPowered? 
                                            KuonTriStateTrue: KuonTriStateFalse;
  robotStatus.m_eIsInMotion = isInMotion()?
                                            KuonTriStateTrue: KuonTriStateFalse;

  // alarmed statue
  if( m_bAlarmState )
  {
    robotStatus.m_eIsMotionPossible = KuonTriStateFalse;
    robotStatus.m_eIsInError = KuonTriStateTrue;
    robotStatus.m_nErrorCode = m_bIsEStopped?
                                      -KUON_ECODE_ESTOP: -KUON_ECODE_ALARMED;
  }

  // unalarmed state
  else
  {
    robotStatus.m_eIsMotionPossible = m_bNoExec? 
                                            KuonTriStateFalse: KuonTriStateTrue;
    robotStatus.m_eIsInError = KuonTriStateFalse;
    robotStatus.m_nErrorCode = KUON_OK;
  }

  robotStatus.m_fGovernor = m_fGovernor;
  robotStatus.m_fBattery  = 0.0;


  for(iter = m_kinBase.begin(); iter != m_kinBase.end(); ++iter)
  {
    health.m_strName      = iter->second.m_strName;
    health.m_nMotorId     = iter->first;
    health.m_fTemperature = 0.0;
    health.m_fVoltage     = 0.0;
    health.m_uAlarms      = 0;

    robotStatus.m_vecMotorHealth.push_back(health);
  }

  return KUON_OK;
}

float kuon::KuonRobot::getSlew ( )

inline

Get current slew setting.

Returns

Return value.

Definition at line 561 of file kuonRobot.h.

References getVelocitySetPoints(), and m_fSlew.

    {
      return m_fSlew;
    }

int KuonRobot::getTrajectoryState

(

KuonWheelTrajectoryFeedback &

trajFeedback

)

Get trajectory feedback.

TODO

Parameters

[in,out]

trajectoryFeedback

Wheel state point.

Definition at line 639 of file kuonRobot.cxx.

References kuon::KuonWheelTrajectoryFeedback::clear(), and kuon::KUON_OK.

Referenced by incrementGovernor().

{
  // TBD
 
  trajFeedback.clear();

  return KUON_OK;
}

void KuonRobot::getVelocitySetPoints	(	double &	fSpeedLeft,
		double &	fSpeedRight,
		units_t	units = units_norm
	)

Get the current left and right side motor velocity set points.

Parameters

[out]	fSpeedLeft	Current left motors set velocity in units.
[out]	fSpeedRight	Current right motors set velocity in units.
	units	Velocity units.

Definition at line 489 of file kuonRobot.cxx.

Referenced by getSlew().

{
  switch(units)
  {
    // [-1.0, 1.0]  of max servo speed
    case units_norm:
      fSpeedLeft  = (double)m_nSetPtSpeedLeft  / (double)RS160D_MOTOR_SPEED_MAX;
      fSpeedRight = (double)m_nSetPtSpeedRight / (double)RS160D_MOTOR_SPEED_MAX;
      break;

    // % of max motor speed
    case units_percent:
      fSpeedLeft  = (double)m_nSetPtSpeedLeft  / (double)RS160D_MOTOR_SPEED_MAX;
      fSpeedRight = (double)m_nSetPtSpeedRight / (double)RS160D_MOTOR_SPEED_MAX;
      fSpeedLeft  *= 100.0;
      fSpeedRight *= 100.0;
      break;

    // %% of max motor speed
    case units_permil:
      fSpeedLeft  = (double)m_nSetPtSpeedLeft  / (double)RS160D_MOTOR_SPEED_MAX;
      fSpeedRight = (double)m_nSetPtSpeedRight / (double)RS160D_MOTOR_SPEED_MAX;
      fSpeedLeft  *= 1000.0;
      fSpeedRight *= 1000.0;
      break;

    // raw
    case units_raw:
    default:
      fSpeedLeft  = (double)m_nSetPtSpeedLeft;
      fSpeedRight = (double)m_nSetPtSpeedRight;
      break;
  }
}

void kuon::KuonRobot::getVersion ( int &  nVerMajor, int &  nVerMinor, int &  nRevision  )

inline

Get the Kuon robotic arm hardware version number.

Parameters

[out]	nVerMajor	Major version number.
[out]	nVerMinor	Minor version number.
[out]	nVerRevision	Revision version number.

Definition at line 420 of file kuonRobot.h.

References kuon::KuonDesc::getBaseDesc(), kuon::KuonDescBase::getProdHwVer(), KUON_VER_MAJOR, KUON_VER_MINOR, KUON_VER_REV, m_descKuon, and kuon::strToVersion().

    {
      uint_t  uHwVer;

      uHwVer    = strToVersion(m_descKuon.getBaseDesc()->getProdHwVer());
      nVerMajor = KUON_VER_MAJOR(uHwVer);
      nVerMinor = KUON_VER_MINOR(uHwVer);
      nRevision = KUON_VER_REV(uHwVer);
    }

std::string kuon::KuonRobot::getVersion ( )

inline

Get the Kuon robotic arm hardware version string.

Version number strings are of the dotted form maj.min.rev.

Returns

Version string.

Definition at line 437 of file kuonRobot.h.

References kuon::KuonDesc::getBaseDesc(), kuon::KuonDescBase::getProdHwVer(), and m_descKuon.

    {
      return m_descKuon.getBaseDesc()->getProdHwVer();
    }

float kuon::KuonRobot::incrementGovernor ( float  fDelta )

inline

Increment/decrement speed limit governor value.

Governor is defined as:
speed = set_speed * governor

Parameters

fDelta

Governor ± delta.

Returns

Returns new governor value.

Definition at line 248 of file kuonRobot.h.

References getJointState(), getRobotStatus(), getTrajectoryState(), m_fGovernor, move(), setBrake(), setGovernor(), setSlew(), setSpeed(), and velocityToRawSpeed().

    {
      return setGovernor(m_fGovernor+fDelta);
    }

int kuon::KuonRobot::isAlarmed ( )

inline

Test if robot is alarmed.

Returns

Returns true or false.

Definition at line 531 of file kuonRobot.h.

References m_bAlarmState.

    {
      return m_bAlarmState;
    }

bool kuon::KuonRobot::isConnected ( )

inline

Test if connected to Kuon hardware.

Returns

Returns true or false.

Definition at line 408 of file kuonRobot.h.

References m_fdMotorCtlr0, and m_fdMotorCtlr1.

    {
      return (m_fdMotorCtlr0 >= 0) && (m_fdMotorCtlr1 >= 0);
    }

int kuon::KuonRobot::isDescribed ( )

inline

Test if robot is fully described via configuration XML.

Returns

Returns true or false.

Definition at line 489 of file kuonRobot.h.

References kuon::KuonDesc::isDescribed(), and m_descKuon.

    {
      return m_descKuon.isDescribed();
    }

int kuon::KuonRobot::isEStopped ( )

inline

Test if robot is current emergency stopped.

Returns

Returns true or false.

Definition at line 499 of file kuonRobot.h.

References m_bIsEStopped.

    {
      return m_bIsEStopped;
    }

bool kuon::KuonRobot::isInMotion ( )

inline

Test if any joint in any of the kinematic chains is moving.

Note

RS160D motor controller provide no feedback.

Returns

Returns true or false.

Definition at line 521 of file kuonRobot.h.

References m_nSetPtSpeedLeft, and m_nSetPtSpeedRight.

    {
      return (m_nSetPtSpeedLeft != 0) || (m_nSetPtSpeedRight != 0);
    }

int KuonRobot::move

(

KuonWheelTrajectoryPoint &

trajectoryPoint

)

Move platform through trajectory point.

TODO

Parameters

trajectoryPoint

Trajectory end point.

Definition at line 446 of file kuonRobot.cxx.

References kuon::KUON_OK.

Referenced by incrementGovernor().

{
  // TBD
  return KUON_OK;
}

int KuonRobot::release

(

)

Release arm and accessories.

Motors will stop driving, so the arm may fall.

Definition at line 344 of file kuonRobot.cxx.

References kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NO_EXEC.

{
  KUON_TRY_NO_EXEC();
  KUON_TRY_CONN();

  m_lastTrajBase.clear();

  // stop 
  setSpeed(0.0, 0.0);

  // put in 'neutral'
  setBrake(0.0);

  LOGDIAG3("Kuon servo drives released.");

  return KUON_OK;
}

void kuon::KuonRobot::resetEStop ( )

inline

Reset (clears) emergency stop condition.

Note

Motors are not re-powered until an move or freeze action is called.

Definition at line 213 of file kuonRobot.h.

References m_bAlarmState, and m_bIsEStopped.

    {
      m_bIsEStopped = false;
      m_bAlarmState = false;
    }

int KuonRobot::setBrake

(

float

fBrake

)

Set robot's auto-brake value.

The brake value is applied to all motors. In the current Kuon, rheostatic motor braking is used. Future versions may use regenerative and/or mechanical braking.

Braking is automatically applied when motor speeds are 0.

Parameters

fBrake

Brake value between [0.0, 1.0] where 0.0 is coasting and 1.0 is full brake.

Definition at line 369 of file kuonRobot.cxx.

References kuon::fcap(), kuon::icap(), kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NOT_ESTOP.

Referenced by incrementGovernor().

{
  int   raw;

  KUON_TRY_CONN();
  KUON_TRY_NOT_ESTOP();

  fBrake = (float)fcap(fBrake, 0.0, 1.0);

  raw = RS160D_MOTOR_BRAKE_MAX * fBrake;
  raw = icap(raw, RS160D_MOTOR_BRAKE_MIN, RS160D_MOTOR_BRAKE_MAX);

  RS160DAlterBraking(raw, m_fdMotorCtlr0, RS160D_MOTOR_LEFT_ID);
  RS160DAlterBraking(raw, m_fdMotorCtlr0, RS160D_MOTOR_RIGHT_ID);
  RS160DAlterBraking(raw, m_fdMotorCtlr1, RS160D_MOTOR_LEFT_ID);
  RS160DAlterBraking(raw, m_fdMotorCtlr1, RS160D_MOTOR_RIGHT_ID);

  m_fBrake = fBrake;

  m_bAreMotorsPowered = m_fBrake > 0.0? true: false;

  LOGDIAG3("Brakes set to %3.1f (raw=%d).", m_fBrake, raw);

  return KUON_OK;
}

float kuon::KuonRobot::setGovernor ( float  fGovernor )

inline

Set speed limit governor value.

Governor is defined as:
speed = cap(set_speed, min_speed * governor, max_speed * governor)

Parameters

fGovernor

Governor value between [0.0, 1.0].

Returns

Returns new governor value.

Definition at line 229 of file kuonRobot.h.

References kuon::fcap(), m_fGovernor, m_nGovernSpeedMax, and m_nGovernSpeedMin.

Referenced by incrementGovernor().

    {
      m_fGovernor       = (float)fcap(fGovernor, 0.0, 1.0);
      m_nGovernSpeedMin = (int)(m_fGovernor * (float)RS160D_MOTOR_SPEED_MIN);
      m_nGovernSpeedMax = (int)(m_fGovernor * (float)RS160D_MOTOR_SPEED_MAX);

      return m_fGovernor;
    }

void kuon::KuonRobot::setRobotMode ( KuonRobotMode  eRobotMode )

inline

Set robot's operational mode.

Returns

eRobotMode Robot operation mode. See KuonRobotMode.

Definition at line 363 of file kuonRobot.h.

References cancelAsyncTask(), and m_eRobotMode.

    {
      m_eRobotMode = eRobotMode;
    }

int KuonRobot::setSlew

(

float

fSlew

)

Set robot's power slew value.

The slew value is applied to all motors. It defines the power ramp up scale when accelerating.

Slewing is automatically applied on acceleration changes.

Parameters

fSlew

Slew value between [0.0, 1.0] where 0.0 is full power applied instantly with quickest response and 1.0 is the slowest ramp up with sloggish response but little back EMF generated.

Definition at line 395 of file kuonRobot.cxx.

References kuon::fcap(), kuon::icap(), kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NOT_ESTOP.

Referenced by incrementGovernor().

{
  int   raw;

  KUON_TRY_CONN();
  KUON_TRY_NOT_ESTOP();

  fSlew = (float)fcap(fSlew, 0.0, 1.0);

  raw = RS160D_MOTOR_SLEW_MAX * fSlew;
  raw = icap(raw, RS160D_MOTOR_SLEW_MIN, RS160D_MOTOR_SLEW_MAX);

  RS160DAlterSlew(raw, m_fdMotorCtlr0, RS160D_MOTOR_LEFT_ID);
  RS160DAlterSlew(raw, m_fdMotorCtlr0, RS160D_MOTOR_RIGHT_ID);
  RS160DAlterSlew(raw, m_fdMotorCtlr1, RS160D_MOTOR_LEFT_ID);
  RS160DAlterSlew(raw, m_fdMotorCtlr1, RS160D_MOTOR_RIGHT_ID);

  m_fSlew = fSlew;

  LOGDIAG3("Power slew set to %3.1f (raw=%d).", m_fSlew, raw);

  return KUON_OK;
}

int KuonRobot::setSpeed	(	double	fSpeedLeft,
		double	fSpeedRight,
		units_t	units = units_norm
	)

Set robot's left and right motor speeds.

Parameters

fSpeedLeft	Left motor speeds in the given units.
fSpeedRight	Right motor speeds in the given units.
units	Speed units. One of: units_norm units_percent units_permil units_raw.

Definition at line 419 of file kuonRobot.cxx.

References kuon::icap(), kuon::KUON_OK, KUON_TRY_CONN, and KUON_TRY_NOT_ESTOP.

Referenced by incrementGovernor().

{
  int   rawLeft;
  int   rawRight;

  KUON_TRY_CONN();
  KUON_TRY_NOT_ESTOP();

  rawLeft  = velocityToRawSpeed(fSpeedLeft,  units);
  rawRight = velocityToRawSpeed(fSpeedRight, units);

  rawLeft  = icap(rawLeft, m_nGovernSpeedMin, m_nGovernSpeedMax);
  rawRight = icap(rawRight, m_nGovernSpeedMin, m_nGovernSpeedMax);

  RS160DUpdateMotorSpeeds(-rawLeft, m_fdMotorCtlr0, RS160D_MOTOR_LEFT_ID);
  RS160DUpdateMotorSpeeds(rawRight, m_fdMotorCtlr0, RS160D_MOTOR_RIGHT_ID);
  RS160DUpdateMotorSpeeds(-rawLeft, m_fdMotorCtlr1, RS160D_MOTOR_LEFT_ID);
  RS160DUpdateMotorSpeeds(rawRight, m_fdMotorCtlr1, RS160D_MOTOR_RIGHT_ID);

  m_nSetPtSpeedLeft   = rawLeft;
  m_nSetPtSpeedRight  = rawRight;

  LOGDIAG3("Speed raw_left=%d, raw_right=%d.", rawLeft, rawRight);

  return KUON_OK;
}

int KuonRobot::velocityToRawSpeed	(	double	fVelocity,
		units_t	units = units_norm
	)

Convert velocity to raw motor speed value.

Parameters

fVelocity	Velocity.
units	Velocity units. One of: units_norm [-1.0, 1.0] units_percent [-100.0, 100.0] units_permil [-1000.0, 1000.0] units_raw [min, max]

Returns

Raw speed value.

Definition at line 452 of file kuonRobot.cxx.

References kuon::fcap().

Referenced by incrementGovernor().

{
  switch(units)
  {
    // [-1.0, 1.0]  of max servo speed
    case units_norm:
      return (int)(RS160D_MOTOR_SPEED_MAX * fcap(fVelocity, -1.0, 1.0));

    // % of max motor speed
    case units_percent:
      return (int)(RS160D_MOTOR_SPEED_MAX * 
                                        fcap(fVelocity, -100.0, 100.0)/100.0);

    // %% of max motor speed
    case units_permil:
      return (int)(RS160D_MOTOR_SPEED_MAX * 
                                      fcap(fVelocity, -1000.0, 1000.0)/1000.0);

    // raw units
    case units_raw:
      return (int)fcap(fVelocity,
                            RS160D_MOTOR_SPEED_MIN, RS160D_MOTOR_SPEED_MAX);

    // future
    case units_rad_per_s:
    case units_m_per_s:
      LOGWARN("%s velocity units not supported until encoders installed.",
          units_shortname(units));
      return 0;

    // bad units
    default:
      LOGWARN("%s velocity units not supported.", units_shortname(units));
      return 0;
  }
}

---

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

• include/Kuon/kuonRobot.h
• sw/libkuon/kuonRobot.cxx