laelaps::LaeRobot Class Reference

Laelaps robotic manipulator plus accesories class. More...

#include <laeRobot.h>

Public Member Functions
	LaeRobot (bool bNoExec=false)
	Default initialization constructor. More...
virtual	~LaeRobot ()
	Destructor.
int	connect ()
	Connect to Laelaps. More...
int	disconnect ()
	Disconnect from Laelaps. More...
int	reload ()
	Reload Laelaps's reloadable configuration and reset operational parameters. More...
int	estop ()
	Emergency stop. More...
int	resetEStop ()
	Reset (clears) emergency stop condition. More...
int	freeze ()
	Freeze robot and accessories at current position. More...
int	release ()
	Release robot and accessories. More...
int	stop ()
	Stop robot with full dynamic braking. More...
int	move (const LaeMapVelocity &velocity)
	Move by setting powertrain angular wheel velocities. More...
int	move (double fVelLinear, double fVelAngular)
	Move at the given linear and angular velocities. More...
int	setDutyCycles (const LaeMapDutyCycle &duty)
	Set powertrain motor duty cycles. More...
double	setGovernor (double fGovernor)
	Set speed limit governor value. More...
double	incrementGovernor (double fDelta)
	Increment/decrement speed limit governor value. More...
double	getGovernor ()
	Get current speed limit governor setting. More...
void	setRobotMode (LaeRobotMode eRobotMode)
	Set robot's operational mode. More...
int	setAuxPower (const std::string &strName, LaeTriState eState)
	Set top deck auxilliary power out enable state. More...
int	clearAlarms ()
	Attempt to clear all alarms. More...
virtual int	configDigitalPin (uint_t pin, uint_t dir)
	Configure a digital pin on the the watchdog subprocessor. More...
virtual int	readDigitalPin (uint_t pin, uint_t &val)
	Read the value of a digital pin on the watchdog subprocessor. More...
virtual int	writeDigitalPin (uint_t pin, uint_t val)
	Write a value to a digital pin on the watchdog subprocessor. More...
virtual int	readAnalogPin (uint_t pin, uint_t &val)
	Read the value of an analog pin on the watchdog subprocessor. More...
virtual int	getImu (double accel[], double gyro[], double rpy[], sensor::imu::Quaternion &q)
	Get the last read and converted inertia data. More...
virtual int	getRangeSensorProps (const std::string &strKey, std::string &strRadiationType, double &fFoV, double &fBeamDir, double &fMin, double &fMax)
	Get range sensor properties. More...
virtual int	getRange (const std::string &strKey, double &fRange)
	Get a range measurement. More...
virtual int	getRange (std::vector< std::string > &vecNames, std::vector< double > &vecRanges)
	Get all sensor range measurements. More...
virtual int	getAmbientLight (const std::string &strKey, double &fAmbient)
	Get an ambient light illuminance measurement. More...
virtual int	getAmbientLight (std::vector< std::string > &vecNames, std::vector< double > &vecAmbient)
	Get all sensor ambient light illuminance measurements. More...
int	runAsyncJob ()
	Run asynchronous job. More...
void	cancelAsyncJob ()
	Cancel any asynchronous job. More...
LaeAsyncJob::JobState	getAsyncJobState ()
	Get the current asynchronous job state. More...
int	getAsyncJobRc ()
	Get the last asynchronous job exit return code. More...
int	getRobotStatus (LaeRptRobotStatus &rptBotStatus)
	Get the robot current status. More...
int	getDynamics (LaeRptDynamics &rptDynamics)
	Get the robot full dynamic state. More...
int	getNavigationState (LaeSimplePathFeedback &pathFeedback)
	Get simple navigation feedback. More...
int	getNavigationState (LaePathFeedback &pathFeedback)
	Get navigation feedback. More...
bool	isConnected ()
	Test if connected to Laelaps hardware. More...
void	getVersion (int &nVerMajor, int &nVerMinor, int &nRevision)
	Get the Laelaps hardware version number. More...
std::string	getVersionString ()
	Get the Laelaps robotic arm hardware version string. More...
LaeDesc &	getLaelapsDesc ()
	Get the Laelaps product description. More...
int	getProdId ()
	Convenience function to get this Laelaps description's base product id. More...
std::string	getProdName ()
	Convenience function to get this Laelaps description's base product name. More...
std::string	getFullProdBrief ()
	Get the Laelaps full brief descirption. More...
bool	isDescribed ()
	Test if robot is fully described via configuration XML. More...
LaeRobotMode	getRobotMode ()
	Get robot's operational mode. More...
bool	isEStopped ()
	Test if robot is current emergency stopped. More...
bool	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...
bool	isAlarmed ()
	Test if robot is alarmed. More...
bool	canMove ()
	Test if robot is safe to operate, given the current robot and alarm state. More...
LaePowertrain *	getPowertrain (const std::string &strName)
	Get the robotic joint in kinematic chain. More...
LaePowertrain *	getPowertrain (int nMotorId)
	Get robotic joint in kinematic chain. More...

Static Public Attributes
static const double	GovernorDft = 1.0
	speed limit governor start-up default

Protected Member Functions
void	syncDb ()
	Synchronize real-time database with current robot state.
int	connSensors ()
	Connect to the Laelaps built-in sensors. More...
int	connWatchDog ()
	Connect to the watchdog subprocessor. More...
int	connMotorControllers (const std::string &strDevMotorCtlrs, const int nBaudRate)
	Connect to the Laelaps motor controllers. More...
int	configForOperation ()
	Configure Laelaps for normal operation. More...
int	startCoreThreads ()
	Create and start all real-time persistent core threads. More...
int	startThread (LaeThread *pThread, int nPriority, double fHz)
	Create and start a thread at the given priority and hertz. More...

Protected Attributes
bool	m_bNoExec
	do [not] execute physical movements
LaeDesc	m_descLaelaps
	Laelaps description
bool	m_bIsConnected
	critical hardware [not] connected
LaeRobotMode	m_eRobotMode
	robot operating mode
bool	m_bIsEStopped
	robot is [not] emergency stopped
bool	m_bAlarmState
	robot is [not] alarmed
double	m_fGovernor
	speed limit governor setting
LaeTunes	m_tunes
	tuning parameters
LaeI2C	m_i2cBus
	I2C sensor bus.
sensor::vl6180::LaeRangeSensorGroup	m_range
	range sensor group.
sensor::imu::LaeImuCleanFlight	m_imu
	inertia measurement unit
LaeWd	m_watchdog
	watchdog sub-processor
LaeKinematics	m_kin
	robot base dynamics and kinematics
LaeThreadAsync	m_threadAsync
	asynchronous action thread
LaeThreadImu	m_threadImu
	IMU thread.
LaeThreadKin	m_threadKin
	kinodynamics thread
LaeThreadRange	m_threadRange
	ToF range sensors thread.
LaeThreadWd	m_threadWatchDog
	watchdog thread
LaeAsyncJob *	m_pAsyncJob
	asynchronous job

Friends
class	LaeRptDynamics
class	LaeRptRobotStatus

Detailed Description

Laelaps robotic manipulator plus accesories class.

Definition at line 103 of file laeRobot.h.

Constructor & Destructor Documentation

LaeRobot::LaeRobot

(

bool

bNoExec = false

)

Default initialization constructor.

Parameters

bNoExec

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

Definition at line 164 of file laeRobot.cxx.

References GovernorDft, laelaps::LaeRobotModeUnknown, m_bAlarmState, m_bIsConnected, m_bIsEStopped, m_bNoExec, m_eRobotMode, m_pAsyncJob, setGovernor(), and syncDb().

                               :
    m_i2cBus(), m_watchdog(m_i2cBus), m_range(m_i2cBus),
    m_threadImu(m_imu),
    m_threadKin(m_kin),
    m_threadRange(m_range),
    m_threadWatchDog(m_watchdog)
{
  int   nCtlr;

  // state
  m_bNoExec           = bNoExec;
  m_bIsConnected      = false;
  m_eRobotMode        = LaeRobotModeUnknown;
  m_bIsEStopped       = false;
  m_bAlarmState       = false;

  setGovernor(GovernorDft);

  syncDb();

  // asynchronous job 
  m_pAsyncJob         = NULL;
}

Member Function Documentation

bool LaeRobot::areMotorsPowered

(

)

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

Returns

Returns true or false.

Definition at line 781 of file laeRobot.cxx.

References laelaps::LaeKinematics::areMotorsPowered(), and m_kin.

Referenced by canMove().

{
  return m_kin.areMotorsPowered();
}

void LaeRobot::cancelAsyncJob

(

)

Cancel any asynchronous job.

Note

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

Definition at line 1125 of file laeRobot.cxx.

References m_threadAsync, and laelaps::LaeThreadAsync::terminateThread().

Referenced by disconnect().

{
  m_threadAsync.terminateThread();
}

bool LaeRobot::canMove

(

)

Test if robot is safe to operate, given the current robot and alarm state.

Returns

Returns true or false.

Definition at line 807 of file laeRobot.cxx.

References areMotorsPowered(), laelaps::LaeAlarms::isSafeToOperate(), and m_bIsEStopped.

Referenced by laelaps::LaeRptRobotStatus::generate().

{
  // not safe to operate.
  if( !LaeAlarms::isSafeToOperate() )
  {
    return false;
  }
  // emergency stopped
  else if( m_bIsEStopped )
  {
    return false;
  }
  // motors are unpowered
  else if( !areMotorsPowered() )
  {
    return false;
  }
  // we have a go!
  else
  {
    return true;
  }
}

int LaeRobot::clearAlarms

(

)

Attempt to clear all alarms.

Note

For Laelaps, most alarms are not clearable (e.g. temperature, low battery). Only external intervention is effective.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 598 of file laeRobot.cxx.

References laelaps::LAE_OK, and LAE_TRY_CONN.

{
  LAE_TRY_CONN();

  return LAE_OK;
}

int LaeRobot::configDigitalPin ( uint_t  pin, uint_t  dir  )

virtual

Configure a digital pin on the the watchdog subprocessor.

Parameters

pin	Digital pin number.
dir	Pin direction. 0 == input, 1 == output.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 614 of file laeRobot.cxx.

References laelaps::LaeWd::cmdConfigDPin(), and m_watchdog.

{
  return m_watchdog.cmdConfigDPin(pin, dir);
}

int LaeRobot::configForOperation ( )

protected

Configure Laelaps for normal operation.

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

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 947 of file laeRobot.cxx.

References laelaps::LaeWd::configure(), laelaps::LaeKinematics::configure(), sensor::imu::LaeImu::configure(), sensor::vl6180::LaeRangeSensorGroup::configure(), laelaps::LAE_OK, m_descLaelaps, m_imu, m_kin, m_range, m_tunes, and m_watchdog.

Referenced by connect().

{
  int     rc;                 // return code

  //
  // Configure watchdog from product description
  //
  if( (rc = m_watchdog.configure(m_descLaelaps)) != LAE_OK )
  {
    LOGERROR("Failed to configure WatchDog.");
  }

  //
  // Configure watchdog from tunable parameters
  //
  else if( (rc = m_watchdog.configure(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to tune WatchDog.");
  }

  //
  // Configure kinodynamics from product description.
  //
  else if( (rc = m_kin.configure(m_descLaelaps)) != LAE_OK )
  {
    LOGERROR("Failed to configure kinodynamics.");
  }

  //
  // Configure kinodynamics from tunable parameters.
  //
  else if( (rc = m_kin.configure(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to tune kinodynamics.");
  }

  //
  // Configure IMU from product description.
  //
  else if( (rc = m_imu.configure(m_descLaelaps)) != LAE_OK )
  {
    LOGERROR("Failed to configure IMU.");
  }

  //
  // Configure IMU from tunable parameters.
  //
  else if( (rc = m_imu.configure(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to tune IMU.");
  }

  //
  // Configure range sensors from product description.
  //
  else if( (rc = m_range.configure(m_descLaelaps)) != LAE_OK )
  {
    LOGERROR("Failed to configure IMU.");
  }

  //
  // Configure range sensors from tunable parameters.
  //
  else if( (rc = m_range.configure(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to configure range sensor group.");
  }

  //
  // Good
  //
  else
  {
    //
    // Enable power to top deck battery and regulated 5V power outputs.
    // Note: Moved to watchdog subprocessor.
    //
    //m_powerBatt.enable();
    //m_power5V.enable();

    rc = LAE_OK;
  }

  LOGDIAG2("Configured for operation.");

  return rc;
}

int LaeRobot::connect

(

)

Connect to Laelaps.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 193 of file laeRobot.cxx.

References configForOperation(), connMotorControllers(), connSensors(), connWatchDog(), disconnect(), freeze(), laelaps::i2cTryOpen(), laelaps::LaeDesc::isDescribed(), laelaps::LAE_ECODE_BAD_OP, laelaps::LAE_OK, laelaps::LaeBaudRateMotorCtlrs, laelaps::LaeDevMotorCtlrs, laelaps::LaeEtcTune, laelaps::LaeI2CAddrWd, laelaps::LaeRobotModeAuto, laelaps::LaeRobotModeUnknown, laelaps::LaeSysCfgPath, laelaps::LaeXmlTune::load(), m_bAlarmState, m_bIsConnected, m_bIsEStopped, m_descLaelaps, m_eRobotMode, m_i2cBus, m_tunes, startCoreThreads(), and syncDb().

Referenced by main().

{
  int     rc;           // return code

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Pre-connect requirements.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  //
  // Been here, did this.
  //
  if( m_bIsConnected )
  {
    LOGWARN("Laelaps already connected to hardware.");
    return LAE_OK;
  }

  //
  // Need a robot description before preceeding. The controlling application, 
  // typically a ROS node, provides the description. The desription is normally
  // the parsed data found in /etc/laelaps/laelaps.conf XML file.
  //
  if( !m_descLaelaps.isDescribed() )
  {
    LOGERROR("Undefined Laelaps description - "
             "don't know how to initialized properly.");
    return -LAE_ECODE_BAD_OP;
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Initialize robot status.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  m_bIsConnected      = false;
  m_eRobotMode        = LaeRobotModeUnknown;
  m_bIsEStopped       = false;
  m_bAlarmState       = false;

  syncDb();

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Parse and set tuning parameters.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  //
  // Override any default tuning parameters from the optional, user-specified
  // tuning XML file.
  //
  LaeXmlTune  xml;

  // parse tune XML file and set tuning parameter overrides
  xml.load(m_tunes, LaeSysCfgPath, LaeEtcTune);

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Establish hardware connections.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

  //
  // Laelaps peripherals I2C bus.
  //
  if( (rc = i2cTryOpen(m_i2cBus, LaeI2CAddrWd)) < 0 )
  {
    LOGSYSERROR("Failed to opend I2C bus.");
  }

  //
  // Connect to all standard built-in sensors.
  //
  if( rc == LAE_OK )
  {
    rc = connSensors();
  }

  //
  // Connect to the external watchdog arduino subprocessor.
  // Note:  The watchdog connection must preceed connection to the motor
  //        controllers.
  //
  if( rc == LAE_OK )
  {
    rc = connWatchDog();
  }

  //
  // Connect to motor controllers.
  //
  if( rc == LAE_OK )
  {
    rc = connMotorControllers(LaeDevMotorCtlrs, LaeBaudRateMotorCtlrs);
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Initialize and Configure
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
  //
  // Configure hardware for operation.
  //
  if( rc == LAE_OK )
  {
    rc = configForOperation();
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Start real-time, persistent threads.
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
  if( rc == LAE_OK )
  {
    rc = startCoreThreads();
  }

  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
  // Finale
  // . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
 
  // failure
  if( rc != LAE_OK )
  {
    LOGERROR("Failed to connect and/or initialize hardware.");
    disconnect();
  }

  // success
  else
  {
    m_bIsConnected      = true;
    m_eRobotMode        = LaeRobotModeAuto;
    m_bIsEStopped       = false;
    m_bAlarmState       = false;

    syncDb();

    freeze(); // place robot in a safe state

    LOGDIAG1("Connected to Laelaps.");
  }

  return rc;
}

int LaeRobot::connMotorControllers ( const std::string &  strDevMotorCtlrs, const int  nBaudRate  )

protected

Connect to the Laelaps motor controllers.

Motors controller serial interface support multi-drop, so one serial device can support up to 8 motor controllers.

Parameters

strDevMotorCtlrs	Motor controllers serial device name.
nBaudRate	Motor controllers serial baudrate.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 928 of file laeRobot.cxx.

References laelaps::LaeWd::enableMotorCtlrs(), m_kin, m_watchdog, and laelaps::LaeKinematics::open().

Referenced by connect().

{
  int     rc;

  rc = m_kin.open(strDevMotorCtlrs, nBaudRate,
                  LaeWd::enableMotorCtlrs, &m_watchdog);

  if( rc < 0 )
  {
    LOGERROR("%s: Failed to open at motor controllers at %d baud.",
        strDevMotorCtlrs.c_str(), nBaudRate);
  }

  LOGDIAG2("Created front and rear motor controller interfaces.");

  return rc;
}

int LaeRobot::connSensors ( )

protected

Connect to the Laelaps built-in sensors.

Sensors are not critical to core operation, so failed sensors are simply blacklisted, rather than failing the robot.

Sensors:

• IMU
• ToF infrared distance sensors

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 849 of file laeRobot.cxx.

References sensor::imu::LaeImu::blacklist(), sensor::vl6180::LaeRangeSensorGroup::blacklist(), sensor::vl6180::LaeRangeSensorGroup::clearSensedData(), sensor::vl6180::LaeRangeSensorGroup::getInterfaceVersion(), laelaps::LaeDesc::getProdHwVer(), laelaps::LAE_OK, laelaps::LaeBaudRateIMU, laelaps::LaeDevIMU, m_descLaelaps, m_imu, m_range, sensor::imu::LaeImu::open(), sensor::imu::LaeImuCleanFlight::readIdentity(), and sensor::vl6180::LaeRangeSensorGroup::setInterface().

Referenced by connect().

{
  string  strIdent;
  uint_t  uVerMajor, uVerMinor, uFwVer;
  int     rc;

  //
  // Connect to the CleanFlight Intertial Measrurement Unit.
  //
  if( (rc = m_imu.open(LaeDevIMU, LaeBaudRateIMU)) < 0 )
  {
    LOGERROR("%s: Failed to open IMU at %d baud.", LaeDevIMU, LaeBaudRateIMU);
  }

  else if( (rc = m_imu.readIdentity(strIdent)) < 0 )
  {
    LOGERROR("%s: Failed to read IMU identity.", LaeDevIMU);
  }

  if( rc == LAE_OK )
  {
    LOGDIAG2("Connected to IMU %s.", strIdent.c_str());
  }
  else
  {
    m_imu.blacklist();
    LOGWARN("IMU is blacklisted from the suite of robot sensors.");
    rc = LAE_OK;
  }

  //
  // Connect to the Range Sensor Group.
  //
  if( (rc = m_range.setInterface(m_descLaelaps.getProdHwVer())) < 0 )
  {
    LOGERROR("Failed to set range sensor group interface.");
  }

  else if((rc = m_range.getInterfaceVersion(uVerMajor, uVerMinor, uFwVer)) < 0)
  {
    LOGERROR("Failed to read range sensor group interface version.");
  }

  if( rc == LAE_OK )
  {
    m_range.clearSensedData();

    LOGDIAG2("Connected to Range Sensor Group v%u.%u, fwver=%u.",
        uVerMajor, uVerMinor, uFwVer);
  }
  else
  {
    m_range.blacklist();
    LOGWARN("Range sensors are blacklisted from the suite of robot sensors.");
    rc = LAE_OK;
  }

  return rc;
}

int LaeRobot::connWatchDog ( )

protected

Connect to the watchdog subprocessor.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 909 of file laeRobot.cxx.

References laelaps::LaeWd::cmdGetFwVersion(), laelaps::LAE_OK, m_watchdog, and laelaps::LaeWd::sync().

Referenced by connect().

{
  uint_t  uFwVer;

  if( m_watchdog.cmdGetFwVersion(uFwVer) < 0 )
  {
    LOGWARN("WatchDog: Failed to get firmware version.");
  }
  else
  {
    LOGDIAG2("Connected to WatchDog sub-processor, fwver=%u.", uFwVer);

    // sync watchdog state with subprocessor
    m_watchdog.sync();
  }

  return LAE_OK;
}

int LaeRobot::disconnect

(

)

Disconnect from Laelaps.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 333 of file laeRobot.cxx.

References cancelAsyncJob(), sensor::imu::LaeImu::clearSensedData(), sensor::vl6180::LaeRangeSensorGroup::clearSensedData(), laelaps::LaeI2C::close(), laelaps::LaeKinematics::close(), sensor::imu::LaeImu::close(), isConnected(), laelaps::LAE_OK, laelaps::LaeRobotModeUnknown, m_bAlarmState, m_bIsConnected, m_bIsEStopped, m_eRobotMode, m_i2cBus, m_imu, m_kin, m_range, m_threadImu, m_threadKin, m_threadRange, m_threadWatchDog, syncDb(), and laelaps::LaeThread::terminateThread().

Referenced by connect(), main(), and ~LaeRobot().

{
  int   nCtlr;

  if( !isConnected() )
  {
    return LAE_OK;
  }

  //
  // Terminate all threads.
  //
  cancelAsyncJob();

  m_threadWatchDog.terminateThread();
  m_threadKin.terminateThread();
  m_threadImu.terminateThread();
  m_threadRange.terminateThread();

  m_kin.close();              // close motors
  m_imu.close();              // on usb device
  m_i2cBus.close();           // sensors/watchdog on i2c device

  m_range.clearSensedData();
  m_imu.clearSensedData();

  // reset robot state
  m_bIsConnected      = false;
  m_eRobotMode        = LaeRobotModeUnknown;
  m_bIsEStopped       = false;
  m_bAlarmState       = false;

  syncDb();

  LOGDIAG1("Disconnected from Laelaps.");

  return LAE_OK;
}

int LaeRobot::estop

(

)

Emergency stop.

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

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 421 of file laeRobot.cxx.

References laelaps::LaeKinematics::estop(), laelaps::LaeKinematics::isEnabled(), laelaps::LAE_ECODE_MOT_CTLR, laelaps::LAE_OK, LAE_TRY_CONN, LAE_TRY_NO_EXEC, m_bAlarmState, m_bIsEStopped, m_kin, and syncDb().

{
  int   rc;   // return code

  LAE_TRY_NO_EXEC();
  LAE_TRY_CONN();

  m_kin.estop();

  if( !m_kin.isEnabled() )
  {
    m_bIsEStopped = true;
    m_bAlarmState = true;

    syncDb();

    rc = LAE_OK;

    LOGDIAG3("Laelaps emergency stopped.");
  }
  else
  {
    rc = -LAE_ECODE_MOT_CTLR;

    LOGERROR("Failed to emergency stop.");
  }

  return rc;
}

int LaeRobot::freeze

(

)

Freeze robot and accessories at current position.

Motors are still being driven. The motors are either dynamically or regeneratively braked.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 486 of file laeRobot.cxx.

References laelaps::LAE_OK, LAE_TRY_CONN, LAE_TRY_NO_EXEC, m_kin, and laelaps::LaeKinematics::stop().

Referenced by connect(), and stop().

{
  int   rc;

  LAE_TRY_NO_EXEC();
  LAE_TRY_CONN();

  if( (rc = m_kin.stop()) == LAE_OK )
  {
    LOGDIAG3("Laelaps frozen at current position.");
  }

  return rc;
}

virtual int laelaps::LaeRobot::getAmbientLight ( const std::string &  strKey, double &  fAmbient  )

virtual

Get an ambient light illuminance measurement.

Parameters

	strKey	Sensor's unique name (key).
[out]	fAmbient	Sensed ambient light (lux).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Referenced by getRangeSensorProps().

virtual int laelaps::LaeRobot::getAmbientLight ( std::vector< std::string > &  vecNames, std::vector< double > &  vecAmbient  )

virtual

Get all sensor ambient light illuminance measurements.

Parameters

[out]	vecNames	Vector of sensor unique names.
[out]	vecAmbient	Vector of associated sensor measured ambients (lux).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

int LaeRobot::getAsyncJobRc

(

)

Get the last asynchronous job exit return code.

Returns

Returns LAE_OK when job terminated successfully. Otherwise, returns < 0 laelaps_ecodes.

Definition at line 1142 of file laeRobot.cxx.

References laelaps::LaeAsyncJob::getRc(), laelaps::LAE_ECODE_NO_RSRC, and m_pAsyncJob.

{
  if( m_pAsyncJob != NULL )
  {
    m_pAsyncJob->getRc();
  }
  else
  {
    return -LAE_ECODE_NO_RSRC;
  }
}

LaeAsyncJob::JobState LaeRobot::getAsyncJobState

(

)

Get the current asynchronous job state.

Returns

Job state enum value.

Definition at line 1130 of file laeRobot.cxx.

References laelaps::LaeAsyncJob::getState(), laelaps::LaeAsyncJob::JobStateNoJob, and m_pAsyncJob.

{
  if( m_pAsyncJob != NULL )
  {
    return m_pAsyncJob->getState();
  }
  else
  {
    return LaeAsyncJob::JobStateNoJob;
  }
}

int LaeRobot::getDynamics

(

LaeRptDynamics &

rptDynamics

)

Get the robot full dynamic state.

Parameters

[in,out]

rptDynamics

Robot dynamics state report.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 682 of file laeRobot.cxx.

References laelaps::LaeRptDynamics::clear(), laelaps::LaeRptDynamics::generate(), laelaps::LAE_OK, and LAE_TRY_CONN.

{
  LAE_TRY_CONN();

  rptDynamics.clear();

  rptDynamics.generate(this);

  return LAE_OK;
}

string LaeRobot::getFullProdBrief

(

)

Get the Laelaps full brief descirption.

Returns

Returns product brief description.

Definition at line 761 of file laeRobot.cxx.

References laelaps::LaeDesc::getProdBrief(), and m_descLaelaps.

{
  return m_descLaelaps.getProdBrief();
}

double LaeRobot::getGovernor

(

)

Get current speed limit governor setting.

Note

Software governor is not implemented as yet.

Returns

Return value.

Definition at line 559 of file laeRobot.cxx.

References m_fGovernor.

{
  return m_fGovernor;
}

int LaeRobot::getImu ( double  accel[], double  gyro[], double  rpy[], sensor::imu::Quaternion &  q  )

virtual

Get the last read and converted inertia data.

Not applicable data are set to zero.

Parameters

[out]	accel	Accelerometer data (m/s^2). The array size must be ≥ NumOfAxes.
[out]	gyro	Gyroscope data (radians/s). The array size must be ≥ NumOfAxes.
[out]	rpy	Roll,pitch,yaw data (radians). The array size must be ≥ NumOfAxes.
[out]	q	Vehicle quaternion.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 634 of file laeRobot.cxx.

References sensor::imu::LaeImu::getImuData(), laelaps::LAE_OK, m_imu, and sensor::imu::NumOfAxes.

{
  double mag[NumOfAxes];    // no magnetometer

  m_imu.getImuData(accel, gyro, mag, rpy, q);

  return LAE_OK;
}

LaeDesc & LaeRobot::getLaelapsDesc

(

)

Get the Laelaps product description.

Returns

Returns reference to description.

Definition at line 746 of file laeRobot.cxx.

References m_descLaelaps.

Referenced by main().

{
  return m_descLaelaps;
}

int LaeRobot::getNavigationState

(

LaeSimplePathFeedback &

pathFeedback

)

Get simple navigation feedback.

Parameters

[in,out]

pathFeedback

Navigation feedback state.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 693 of file laeRobot.cxx.

References laelaps::LaeSimplePathFeedback::clear(), and laelaps::LAE_OK.

{
  // TBD
 
  pathFeedback.clear();

  return LAE_OK;
}

int LaeRobot::getNavigationState

(

LaePathFeedback &

pathFeedback

)

Get navigation feedback.

Parameters

[in,out]

pathFeedback

Navigation feedback state.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 702 of file laeRobot.cxx.

References laelaps::LaePathFeedback::clear(), laelaps::LaeKinematics::getPowertrain(), getPowertrain(), laelaps::LAE_OK, and m_kin.

{
  // TBD
 
  pathFeedback.clear();

  return LAE_OK;
}

LaePowertrain* laelaps::LaeRobot::getPowertrain

(

const std::string &

strName

)

Get the robotic joint in kinematic chain.

Parameters

strName

Powertrain unique name (primary key).

Returns

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

Referenced by getNavigationState().

LaePowertrain * LaeRobot::getPowertrain

(

int

nMotorId

)

Get robotic joint in kinematic chain.

Parameters

nMotorId

Motor id (secondary key).

Returns

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

Definition at line 716 of file laeRobot.cxx.

References laelaps::LaeKinematics::getPowertrain(), and m_kin.

{
  return m_kin.getPowertrain(nMotorId);
}

int LaeRobot::getProdId

(

)

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

Returns

Returns product id. See LaeProdId.

Definition at line 751 of file laeRobot.cxx.

References laelaps::LaeDesc::getProdId(), and m_descLaelaps.

{
  return m_descLaelaps.getProdId();
}

string LaeRobot::getProdName

(

)

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

Returns

Returns product name. See LaeProdName.

Definition at line 756 of file laeRobot.cxx.

References laelaps::LaeDesc::getProdName(), and m_descLaelaps.

{
  return m_descLaelaps.getProdName();
}

virtual int laelaps::LaeRobot::getRange ( const std::string &  strKey, double &  fRange  )

virtual

Get a range measurement.

Parameters

	strKey	Sensor's unique name (key).
[out]	fRange	Sensed object range (meters).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Referenced by getRangeSensorProps().

virtual int laelaps::LaeRobot::getRange ( std::vector< std::string > &  vecNames, std::vector< double > &  vecRanges  )

virtual

Get all sensor range measurements.

Parameters

[out]	vecNames	Vector of sensor unique names.
[out]	vecRanges	Vector of associated sensor measured ranges (meters).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

int LaeRobot::getRangeSensorProps ( const std::string &  strKey, std::string &  strRadiationType, double &  fFoV, double &  fBeamDir, double &  fMin, double &  fMax  )

virtual

Get range sensor properties.

Parameters

	strKey	Sensor's unique name id (key).
[out]	strRadiationType	Radiation type.
[out]	fFoV	Field of View (radians).
[out]	fBeamdir	Center of beam direction (radians).
[out]	fMin	Minimum range (meters).
[out]	fMax	Maximum range (meters).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 643 of file laeRobot.cxx.

References getAmbientLight(), sensor::vl6180::LaeRangeSensorGroup::getAmbientLight(), getRange(), sensor::vl6180::LaeRangeSensorGroup::getRange(), sensor::vl6180::LaeRangeSensorGroup::getSensorProps(), and m_range.

{
  return m_range.getSensorProps(strKey, 
                                strRadiationType,
                                fFoV, fBeamDir,
                                fMin, fMax);
}

LaeRobotMode LaeRobot::getRobotMode

(

)

Get robot's operational mode.

Returns

Robot operation mode. See LaeRobotMode.

Definition at line 771 of file laeRobot.cxx.

References m_eRobotMode.

{
  return m_eRobotMode;
}

int LaeRobot::getRobotStatus

(

LaeRptRobotStatus &

rptBotStatus

)

Get the robot current status.

Parameters

[in,out]

rptBotStatus

Robot status report.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 605 of file laeRobot.cxx.

References laelaps::LaeRptRobotStatus::clear(), laelaps::LaeRptRobotStatus::generate(), and laelaps::LAE_OK.

{
  rptBotStatus.clear();

  rptBotStatus.generate(this);

  return LAE_OK;
}

void LaeRobot::getVersion	(	int &	nVerMajor,
		int &	nVerMinor,
		int &	nRevision
	)

Get the Laelaps hardware version number.

Parameters

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

Definition at line 731 of file laeRobot.cxx.

References laelaps::LaeDesc::getProdHwVer(), LAE_VER_MAJOR, LAE_VER_MINOR, LAE_VER_REV, and m_descLaelaps.

{
  uint_t  uHwVer;

  uHwVer    = m_descLaelaps.getProdHwVer();
  nVerMajor = LAE_VER_MAJOR(uHwVer);
  nVerMinor = LAE_VER_MINOR(uHwVer);
  nRevision = LAE_VER_REV(uHwVer);
}

string LaeRobot::getVersionString

(

)

Get the Laelaps robotic arm hardware version string.

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

Returns

Version string.

Definition at line 741 of file laeRobot.cxx.

References laelaps::LaeDesc::getProdHwVerString(), and m_descLaelaps.

{
  return m_descLaelaps.getProdHwVerString();
}

double LaeRobot::incrementGovernor

(

double

fDelta

)

Increment/decrement speed limit governor value.

Note

Software governor is not implemented as yet.

Governor is defined as:
speed = set_speed * governor

Parameters

fDelta

Governor ± delta.

Returns

Returns new governor value.

Definition at line 554 of file laeRobot.cxx.

References m_fGovernor, and setGovernor().

{
  return setGovernor(m_fGovernor+fDelta);
}

bool LaeRobot::isAlarmed

(

)

Test if robot is alarmed.

Returns

Returns true or false.

Definition at line 791 of file laeRobot.cxx.

References isEStopped(), laelaps::LAE_ALARM_NONE, laelaps::LaeDb::m_alarms, m_bAlarmState, laelaps::LaeDbAlarms::m_system, laelaps::LaeAlarmInfo::m_uAlarms, and laelaps::RtDb.

{
  bool  bAlarmState = false;

  if( RtDb.m_alarms.m_system.m_uAlarms != LAE_ALARM_NONE )
  {
    bAlarmState = true;
  }
  if( isEStopped() )
  {
    bAlarmState = true;
  }

  return m_bAlarmState;
}

bool LaeRobot::isConnected

(

)

Test if connected to Laelaps hardware.

Returns

Returns true or false.

Definition at line 726 of file laeRobot.cxx.

References m_bIsConnected.

Referenced by disconnect(), and main().

{
  return m_bIsConnected;
}

bool LaeRobot::isDescribed

(

)

Test if robot is fully described via configuration XML.

Returns

Returns true or false.

Definition at line 766 of file laeRobot.cxx.

References laelaps::LaeDesc::isDescribed(), and m_descLaelaps.

{
  return m_descLaelaps.isDescribed();
}

bool LaeRobot::isEStopped

(

)

Test if robot is current emergency stopped.

Returns

Returns true or false.

Definition at line 776 of file laeRobot.cxx.

References m_bIsEStopped.

Referenced by isAlarmed().

{
  return m_bIsEStopped;
}

bool LaeRobot::isInMotion

(

)

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 786 of file laeRobot.cxx.

References laelaps::LaeKinematics::isStopped(), and m_kin.

{
  return m_kin.isStopped()? false: true;
}

int LaeRobot::move

(

const LaeMapVelocity &

velocity

)

Move by setting powertrain angular wheel velocities.

The robot is controlled by setting the goal velocities of a [sub]set of powertrain motors.

Parameters

velocity

Map of powertrain velocities. Keys are left_front, right_front, left_rear, and right_rear. Values are drive shaft/wheel angular velocities (radians/second).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 517 of file laeRobot.cxx.

References LAE_TRY_CONN, m_kin, and laelaps::LaeKinematics::setGoalVelocities().

{
  int   rc;

  LAE_TRY_CONN();

  rc = m_kin.setGoalVelocities(velocity);

  return rc;
}

int LaeRobot::move	(	double	fVelLinear,
		double	fVelAngular
	)

Move at the given linear and angular velocities.

This move is typically used with the ROS Twist message where:

• linear velocity is the x component Twist.linear.x
• angular velocity is the yaw component Twist.angular.z

Parameters

fVelLinear	Linear velocity (meters/second).
fVelAngular	Angular velocity (radians/second).

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 528 of file laeRobot.cxx.

References LAE_TRY_CONN, m_kin, and laelaps::LaeKinematics::setGoalTwist().

{
  LAE_TRY_CONN();

  return m_kin.setGoalTwist(fVelLinear, fVelAngular);

} 

int LaeRobot::readAnalogPin ( uint_t  pin, uint_t &  val  )

virtual

Read the value of an analog pin on the watchdog subprocessor.

Parameters

	pin	Analog pin number.
[out]	val	Analog 10-bit value [0-1023].

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 629 of file laeRobot.cxx.

References laelaps::LaeWd::cmdReadAPin(), and m_watchdog.

{
  return m_watchdog.cmdReadAPin(pin, val);
}

int LaeRobot::readDigitalPin ( uint_t  pin, uint_t &  val  )

virtual

Read the value of a digital pin on the watchdog subprocessor.

Parameters

	pin	Digital pin number.
[out]	val	Digital pin low (0) or high (1) value.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 619 of file laeRobot.cxx.

References laelaps::LaeWd::cmdReadDPin(), and m_watchdog.

{
  return m_watchdog.cmdReadDPin(pin, val);
}

int LaeRobot::release

(

)

Release robot and accessories.

Motors will stop being driven. The robot will coast to a stop (on a flat surface) and is capabable of being externally pushed.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 501 of file laeRobot.cxx.

References laelaps::LAE_OK, LAE_TRY_CONN, LAE_TRY_NO_EXEC, m_kin, and laelaps::LaeKinematics::release().

{
  int   rc;

  LAE_TRY_NO_EXEC();
  LAE_TRY_CONN();

  // release motor drives 
  if( (rc = m_kin.release()) == LAE_OK )
  {
    LOGDIAG3("Laelaps motor drives released.");
  }

  return rc;
}

int LaeRobot::reload

(

)

Reload Laelaps's reloadable configuration and reset operational parameters.

The robot connection and calibration states are uneffected.

Definition at line 372 of file laeRobot.cxx.

References laelaps::LAE_OK, laelaps::LaeEtcTune, laelaps::LaeSysCfgPath, laelaps::LaeXmlTune::load(), m_threadImu, m_threadKin, m_threadRange, m_threadWatchDog, m_tunes, laelaps::LaeThreadImu::reload(), laelaps::LaeThreadKin::reload(), laelaps::LaeThreadRange::reload(), and laelaps::LaeThreadWd::reload().

{
  LaeXmlTune  xml;    // xml parser
  int         rc;     // return code

  // reload tune XML file and set tuning parameter overrides
  xml.load(m_tunes, LaeSysCfgPath, LaeEtcTune);

  // re-tune Kinodynamics thread and hardware i/f
  if( (rc = m_threadKin.reload(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to reload kinodynamics tune parameters.");
    return rc;
  }

  // re-tune IMU thread and hardware i/f
  else if( (rc = m_threadImu.reload(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to reload IMU tune parameters.");
    return rc;
  }

  // re-tune Range thread and hardware i/f
  else if( (rc = m_threadRange.reload(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to reload range sensing tune parameters.");
    return rc;
  }

  // re-tune WatchDog thread and hardware i/f
  else if( (rc = m_threadWatchDog.reload(m_tunes)) != LAE_OK )
  {
    LOGERROR("Failed to reload WatchDog tune parameters.");
    return rc;
  }

  if( rc == LAE_OK )
  {
    LOGDIAG2("Reloaded tuning parameters and reconfigured robot.");
  }

  return rc;
}

int LaeRobot::resetEStop

(

)

Reset (clears) emergency stop condition.

Note

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

Definition at line 451 of file laeRobot.cxx.

References laelaps::LaeKinematics::isEnabled(), laelaps::LAE_ECODE_MOT_CTLR, laelaps::LAE_OK, LAE_TRY_CONN, LAE_TRY_NO_EXEC, m_bAlarmState, m_bIsEStopped, m_kin, laelaps::LaeKinematics::resetEStop(), and syncDb().

{
  int   rc;   // return code

  LAE_TRY_NO_EXEC();
  LAE_TRY_CONN();

  m_kin.resetEStop();

  if( m_kin.isEnabled() )
  {
    m_bIsEStopped = false;
    m_bAlarmState = false;

    syncDb();

    rc = LAE_OK;

    LOGDIAG3("Laelaps emergency stopped reset.");
  }
  else
  {
    rc = -LAE_ECODE_MOT_CTLR;

    LOGERROR("Failed to reset emergency stopped condition.");
  }

  return rc;
}

int LaeRobot::runAsyncJob

(

)

Run asynchronous job.

The new job object must be already created.

Context:

Calling thread.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 1113 of file laeRobot.cxx.

References laelaps::LaeThreadAsync::createThread(), laelaps::LAE_OK, m_pAsyncJob, m_threadAsync, and laelaps::LaeThreadAsync::runThread().

{
  int   rc;

  if( (rc = m_threadAsync.createThread(m_pAsyncJob)) == LAE_OK )
  {
    m_threadAsync.runThread();
  }

  return rc;
}

int LaeRobot::setAuxPower	(	const std::string &	strName,
		LaeTriState	eState
	)

Set top deck auxilliary power out enable state.

Parameters

strName	Aux. power port name. One of: aux_batt_en aux_5v_en
eState	Aux. power state. One of: DISABLED(0) ENABLED(1)

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 570 of file laeRobot.cxx.

References laelaps::LaeWd::cmdEnableAuxPort5V(), laelaps::LaeWd::cmdEnableAuxPortBatt(), laelaps::LAE_ECODE_BAD_VAL, laelaps::LAE_OK, LAE_TRY_CONN, laelaps::LaeTriStateEnabled, and m_watchdog.

{
  bool  bGoal, bResult;
  int   rc = LAE_OK;

  LAE_TRY_CONN();

  bGoal = eState == LaeTriStateEnabled? true: false;

  if( strName == "aux_batt_en" )
  {
    rc = m_watchdog.cmdEnableAuxPortBatt(bGoal);
    //bResult = bGoal? m_powerBatt.enable(): m_powerBatt.disable();
  }
  else if( strName == "aux_5v_en" )
  {
    rc = m_watchdog.cmdEnableAuxPort5V(bGoal);
    //bResult = bGoal? m_power5V.enable(): m_power5V.disable();
  }
  else
  {
    LOGERROR("%s: Unknown auxilliary port name.", strName.c_str());
    rc = -LAE_ECODE_BAD_VAL;
  }

  return rc;
}

int LaeRobot::setDutyCycles

(

const LaeMapDutyCycle &

duty

)

Set powertrain motor duty cycles.

The robot is controlled by setting the goal duties of a [sub]set of powertrain motors.

Parameters

duty	Map of powertrain duty cycles. Keys are left_front, right_front, left_rear, and right_rear. Values are normalized duty cycles [-1.0, 1.0].

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 536 of file laeRobot.cxx.

References LAE_TRY_CONN, m_kin, and laelaps::LaeKinematics::setGoalDutyCycles().

{
  int   rc;

  LAE_TRY_CONN();

  rc = m_kin.setGoalDutyCycles(duty);

  return rc;
}

double LaeRobot::setGovernor

(

double

fGovernor

)

Set speed limit governor value.

Note

Software governor is not implemented as yet.

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 547 of file laeRobot.cxx.

References laelaps::fcap(), and m_fGovernor.

Referenced by incrementGovernor(), and LaeRobot().

{
  m_fGovernor = fcap(fGovernor, 0.0, 1.0);

  return m_fGovernor;
}

void LaeRobot::setRobotMode

(

LaeRobotMode

eRobotMode

)

Set robot's operational mode.

Parameters

eRobotMode

Robot operation mode. See LaeRobotMode.

Definition at line 564 of file laeRobot.cxx.

References laelaps::LaeDbRobotStatus::m_eRobotMode, m_eRobotMode, laelaps::LaeDb::m_robotstatus, and laelaps::RtDb.

{
  m_eRobotMode = eRobotMode;
  RtDb.m_robotstatus.m_eRobotMode = m_eRobotMode;
}

int LaeRobot::startCoreThreads ( )

protected

Create and start all real-time persistent core threads.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 1035 of file laeRobot.cxx.

References laelaps::LaeTunes::getImuHz(), laelaps::LaeTunes::getKinematicsHz(), laelaps::LaeTunes::getRangeHz(), laelaps::LaeTunes::getWatchDogTimeout(), laelaps::LAE_OK, m_threadImu, m_threadKin, m_threadRange, m_threadWatchDog, m_tunes, laelaps::LaeThreadWd::optimizeHz(), startThread(), laelaps::LaeThreadImu::ThreadImuPrioDft, laelaps::LaeThreadKin::ThreadKinPrioDft, laelaps::LaeThreadRange::ThreadRangePrioDft, and laelaps::LaeThreadWd::ThreadWdPrioDft.

Referenced by connect().

{
  int     nPriority;
  double  fHz;
  int     rc;

  //
  // WatchDog thread.
  //
  nPriority = LaeThreadWd::ThreadWdPrioDft;
  fHz       = LaeThreadWd::optimizeHz(m_tunes.getWatchDogTimeout());

  if( (rc = startThread(&m_threadWatchDog, nPriority, fHz)) != LAE_OK )
  {
    return rc;
  }

  //
  // Time-of-Flight range sensors thread.
  //
  nPriority = LaeThreadRange::ThreadRangePrioDft;
  fHz       = m_tunes.getRangeHz();

  if( (rc = startThread(&m_threadRange, nPriority, fHz)) != LAE_OK )
  {
    return rc;
  }

  //
  // Inertia Measurement Unit thread.
  //
  nPriority = LaeThreadImu::ThreadImuPrioDft;
  fHz       = m_tunes.getImuHz();

  if( (rc = startThread(&m_threadImu, nPriority, fHz)) != LAE_OK )
  {
    return rc;
  }

  //
  // Kinodynamics thread.
  //
  nPriority = LaeThreadKin::ThreadKinPrioDft;
  fHz       = m_tunes.getKinematicsHz();

  if( (rc = startThread(&m_threadKin, nPriority, fHz)) != LAE_OK )
  {
    return rc;
  }

  return LAE_OK;
}

int LaeRobot::startThread ( LaeThread *  pThread, int  nPriority, double  fHz  )

protected

Create and start a thread at the given priority and hertz.

Parameters

pThread	Pointer the thread object.
nPriority	Thread priority.
fHz	Thread execution hertz.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 1088 of file laeRobot.cxx.

References laelaps::LaeThread::createThread(), laelaps::LaeThread::getThreadName(), laelaps::LAE_OK, and laelaps::LaeThread::runThread().

Referenced by startCoreThreads().

{
  string  strName;
  int     rc;

  strName = pThread->getThreadName();

  if( (rc = pThread->createThread(nPriority)) < 0 )
  {
    LOGERROR("%s thread: Failed to create.", strName.c_str());
  }
  else if( (rc = pThread->runThread(fHz)) < 0 )
  {
    LOGERROR("%s thread: Failed to start.", strName.c_str());
  }
  else
  {
    LOGDIAG2("%s thread started at %.3lfHz with priority %d.",
        strName.c_str(), fHz, nPriority); 
    rc = LAE_OK;
  }

  return rc;
}

int LaeRobot::stop

(

)

Stop robot with full dynamic braking.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 481 of file laeRobot.cxx.

References freeze().

Referenced by laelaps_tune_motors.window::createLeftButtons().

{
  return freeze();
}

int LaeRobot::writeDigitalPin ( uint_t  pin, uint_t  val  )

virtual

Write a value to a digital pin on the watchdog subprocessor.

Parameters

pin	Digital pin number.
val	Digital pin low (0) or high (1) value.

Returns

On success, OK(0) is returned.
On error, RC_ERROR(-1) is returned.

Definition at line 624 of file laeRobot.cxx.

References laelaps::LaeWd::cmdWriteDPin(), and m_watchdog.

{
  return m_watchdog.cmdWriteDPin(pin, val);
}

---

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

• include/Laelaps/laeRobot.h
• sw/liblaelaps/laeRobot.cxx