laelaps::LaeKinematics Class Reference

Laelaps kinematics class. More...

#include <laeKin.h>

Public Member Functions
	LaeKinematics ()
	Default initializer constructor.
virtual	~LaeKinematics ()
	Desctructor.
int	open (const std::string &strDevMotorCtlrs, const int nBaudRate, int(*fnEnable)(void *, bool)=NULL, void *pEnableArg=NULL)
	Open communication with the Laelaps motor controllers. More...
virtual int	close ()
	Close communication. More...
virtual bool	isOpen ()
	Test if connection is open. More...
virtual bool	isEnabled ()
	Test if motors are enabled. More...
virtual bool	areMotorsPowered ()
	Test if motors are powered. More...
virtual int	configure (const LaeDesc &desc)
	Configure kinematics chains and data from product description. More...
virtual int	configure (const LaeTunes &tunes)
	Configure kinematics for operation. More...
virtual int	reload (const LaeTunes &tunes)
	Reload configuration tuning parameters. More...
LaePlatform &	getPlatform ()
	Get robot platform kinodynamics. More...
motor::roboclaw::RoboClaw *	getMotorCtlr (const int nMotorCtlrId)
	Get pointer to motor controller by name (key). More...
LaeMapPowertrain &	getPowertrainMap ()
	Get map of all powertrain kinodynamics. More...
LaePowertrain *	getPowertrain (const std::string &strName)
	Get pointer to powertrain by name (key). More...
LaePowertrain *	getPowertrain (int nCtlr, int nMotor)
	Get pointer to powertrain by controller id and motor index. More...
LaePowertrain *	getPowertrain (int nMotorId)
	Get pointer to powertrain by motor id. More...
virtual int	getPowertrainState (const std::string &strName, LaePowertrainState &state)
	Get the current powertrain state. More...
virtual bool	isStopped ()
	Test if all motors are stopped. More...
virtual bool	isStopped (const std::string &strName)
	Test if a powertrain motor is stopped. More...
virtual int	resetOdometers ()
	Reset all motors' odometers to the current respective encoder positions. More...
virtual void	estop ()
	Emergency stop the robot. More...
virtual void	resetEStop ()
	Reset emergency stop condition. More...
virtual int	freeze ()
	Freeze (stop) all motors at the current position. More...
virtual int	release ()
	Release motors. More...
virtual int	stop ()
	Stop all motors at the current position. More...
virtual int	stop (const std::vector< std::string > &vecNames)
	Stop the set of powertrain motors at the current position. More...
virtual int	stop (const std::string &strName)
	Stop one powertrain motor at the current position. More...
virtual int	waitForAllStop (double fSeconds)
	Wait for all motors to stop. More...
virtual int	setGoalVelocities (const LaeMapVelocity &velocity)
	Set new or updated motor velocity goals. More...
virtual int	setGoalDutyCycles (const LaeMapDutyCycle &duty)
	Set new or updated motor duty cycle goals. More...
virtual int	setGoalTwist (double fVelLinear, double fVelAngular)
	Set new or updated robot twist velocity goals. More...
virtual void	exec ()
	Execute kinematics task(s). More...
virtual int	monitorHealth ()
	Monitor kinematics health. More...

Protected Member Functions
void	lock ()
	Lock the shared resource. More...
void	unlock ()
	Unlock the shared resource. More...
void	resyncComm ()
	Attempt to resynchronize the serial communication between the host and the motor controllers.
virtual int	configureMotorController (const LaeTunes &tunes, int nCtlr)
	Configure motor controller. More...
virtual int	configMotorCtlrBatteryCutoffs (const LaeTunes &tunes, int nCtlr, bool &bNvmSave)
	Configure motor controller battery cutoff voltages. More...
virtual int	configMotorCtlrLogicCutoffs (const LaeTunes &tunes, int nCtlr, bool &bNvmSave)
	Configure motor controller logic cutoff voltages. More...
virtual int	configMotorCtlrEncoderModes (const LaeTunes &tunes, int nCtlr, bool &bNvmSave)
	Configure motor controller encoder modes. More...
virtual int	resetMotorCtlrEncoders (int nCtlr)
	Reset all motor controller encoders to zero. More...
virtual int	configurePtp (const LaeTunes &tunes, int nCtlr, bool &bNvSave)
	Configure powertrain pair. More...
virtual int	configMotorVelocityPid (const LaeTunes &tunes, LaePowertrain &powertrain, bool &bNvmSave)
	Configure motor velocity PID parameters. More...
virtual int	configMotorMaxAmpLimit (const LaeTunes &tunes, LaePowertrain &powertrain, bool &bNvmSave)
	Configure motor maximum ampere limit. More...
virtual int	saveConfigToNvm (int nCtlr)
	Save configuration to motor controller's non-volatile memory. More...
virtual int	sense ()
	Sense real-time state. More...
virtual int	senseDynamics ()
	Sense real-time kinodynamics. More...
virtual int	react ()
	React to any necessary time-critical events. More...
virtual int	plan ()
	Plan kinematic action. More...
virtual int	act ()
	Act on current action. More...
virtual void	enableMotorCtlrs ()
	Enable power to motor controllers.
virtual void	disableMotorCtlrs ()
	Disable power to motor controllers.

Protected Attributes
bool	m_bIsEnabled
	is [not] enabled
bool	m_bAreMotorsPowered
	motors are [not] powered
bool	m_bIsStopped
	all motors are [not] stopped
int(*	m_fnEnableMotorCtlrs )(void *, bool)
	enable power function
void *	m_pEnableArg
	argument to enable function
motor::roboclaw::RoboClawComm	m_commMotorCtlrs
	serial communication bus
motor::roboclaw::RoboClaw *	m_pMotorCtlr [LaeNumMotorCtlrs]
	RoboClaw motor controllers.
LaePlatform	m_kinPlatform
	robot platform kinematics
LaeMapPowertrain	m_kinPowertrains
	robot powertrain kinematics
LaeKinAction *	m_pAction
	current extended kinematics action
pthread_mutex_t	m_mutex
	mutex

Detailed Description

Laelaps kinematics class.

The LaeKinematics class supports control and monitoring of Laelaps powertrains and base kinodynamics.

Definition at line 119 of file laeKin.h.

Member Function Documentation

int LaeKinematics::act ( )

protectedvirtual

Act on current action.

Context:

Kinematics thread.

Returns

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

Definition at line 1526 of file laeKin.cxx.

Referenced by unlock().

{
  int     rc;

  // motors are not enabled
  if( !m_bIsEnabled )
  {
    rc = LAE_OK;
  }

  // action requried, so execute
  else if( m_pAction->isExecutionRequired() )
  {
    rc = m_pAction->execute();
  }

  // nothing to do
  else
  {
    rc = LAE_OK;
  }

  return rc;
}

virtual bool laelaps::LaeKinematics::areMotorsPowered ( )

inlinevirtual

Test if motors are powered.

Returns

Returns true or false.

Definition at line 183 of file laeKin.h.

References configure(), m_bAreMotorsPowered, and reload().

Referenced by laelaps::LaeRobot::areMotorsPowered().

    {
      return m_bAreMotorsPowered;
    }

int LaeKinematics::close ( )

virtual

Close communication.

Returns

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

Definition at line 221 of file laeKin.cxx.

Referenced by Laelaps.WatchDog.WatchDog::attach(), laelaps::LaeRobot::disconnect(), and Laelaps.WatchDog.WatchDog::open().

{
  if( m_pAction != NULL )
  {
    m_pAction->terminate();
  }

#ifdef LAE_DEPRECATED
  m_csMotorCtlrs.close();     // gpio
#endif // LAE_DEPRECATED

  m_commMotorCtlrs.close();   // on serial device

  disableMotorCtlrs();

  if( m_bIsEnabled )
  {
    LOGWARN("Failed to disable power to motor controllers.");
  }

  LOGDIAG2("Closed motor controllers serial communication.");

  return LAE_OK;
}

int LaeKinematics::configMotorCtlrBatteryCutoffs ( const LaeTunes &  tunes, int  nCtlr, bool &  bNvmSave  )

protectedvirtual

Configure motor controller battery cutoff voltages.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	nCtlr	Motor controller id.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 447 of file laeKin.cxx.

Referenced by unlock().

{
  string strTag = LaeDesc::prettyMotorCtlrName(nCtlr,
                                    m_pMotorCtlr[nCtlr]->getAddress());

  double  fMin, fMax;         // working min,max values
  double  fMinMin, fMaxMax;   // working min min,max max values
  int     rc;                 // return code

  //
  // Read the main battery voltage cutoff settings (min, max) and set if
  // necessary.
  //
 
  LOGDIAG3("%s: Verifying main battery cutoff voltages...", strTag.c_str());

  //
  // Read current settings.
  //
  rc = m_pMotorCtlr[nCtlr]->cmdReadMainBattCutoffs(fMin, fMax);

  if( rc != OK )
  {
    LOGWARN("%s: Failed to read main battery cutoff voltages.", strTag.c_str());
    return LAE_OK;
  }

  LOGDIAG3("   Current settings: [%.1lf, %.1lf]", fMin, fMax);

  // Get supported full main battery range.
  m_pMotorCtlr[nCtlr]->getMainBattCutoffRange(fMinMin, fMaxMax);

  //
  // Change settings. The settings are always reset to defaults on power-up,
  // so cannot save to non-volatile memory.
  //
  if( (fMin != fMinMin) || (fMax != fMaxMax) )
  {
    rc = m_pMotorCtlr[nCtlr]->cmdSetMainBattCutoffs(fMinMin, fMaxMax);

    if( rc != OK )
    {
      LOGWARN("%s: Failed to set main battery cutoff voltages to "
            "[%.1lf, %.1lf].",
          strTag.c_str(), fMinMin, fMaxMax);
    }
    else
    {
      LOGDIAG3("   Changed settings: [%.1lf, %.1lf]", fMinMin, fMaxMax);
    }
  }

  //
  // The settings are good as configured.
  //
  else
  {
    LOGDIAG3("   Good.");
  }

  return LAE_OK;
}

int LaeKinematics::configMotorCtlrEncoderModes ( const LaeTunes &  tunes, int  nCtlr, bool &  bNvmSave  )

protectedvirtual

Configure motor controller encoder modes.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	nCtlr	Motor controller id.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 579 of file laeKin.cxx.

Referenced by unlock().

{
  string strTag = LaeDesc::prettyMotorCtlrName(nCtlr,
                                    m_pMotorCtlr[nCtlr]->getAddress());

  byte_t  byMode1, byMode2;   // motor encoder modes
  int     rc;                 // return code

  //
  // Read both motors' encoder modes and set if necessary. Laelaps uses
  // quadrature encoding. This is a required configuration for operation. 
  //

  LOGDIAG3("%s: Verifying motor quadrature encoder modes...", strTag.c_str());

  //
  // Read current settings.
  //
  rc = m_pMotorCtlr[nCtlr]->cmdReadEncoderMode(byMode1, byMode2);

  if( rc != OK )
  {
    LOGERROR("%s: Failed to read motor encoder modes.", strTag.c_str());
    return -LAE_ECODE_MOT_CTLR;
  }

  LOGDIAG3("   Current settings: [0x%02x, 0x%02x]", byMode1, byMode2);

  //
  // Change settings. The new settings should also be saved to non-volatile
  // memory.
  //
  if( (byMode1 != ParamEncModeQuad) || (byMode2 != ParamEncModeQuad) )
  {
    rc = m_pMotorCtlr[nCtlr]->cmdSetEncoderMode2(ParamEncModeQuad,
                                                   ParamEncModeQuad);

    if( rc != OK )
    {
      LOGERROR("%s: Failed to set motor encoders to quadrature encoding "
            "[0x%02x, 0x%02x].",
            strTag.c_str(), ParamEncModeQuad, ParamEncModeQuad);
      return -LAE_ECODE_MOT_CTLR;
    }

    LOGDIAG3("   Changed settings: [0x%02x, 0x%02x]",
            ParamEncModeQuad, ParamEncModeQuad);

    bNvmSave = true;
  }

  //
  // The settings are good as configured.
  //
  else
  {
      LOGDIAG3("   Good.");
  }

  return LAE_OK;
}

int LaeKinematics::configMotorCtlrLogicCutoffs ( const LaeTunes &  tunes, int  nCtlr, bool &  bNvmSave  )

protectedvirtual

Configure motor controller logic cutoff voltages.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	nCtlr	Motor controller id.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 512 of file laeKin.cxx.

Referenced by unlock().

{
  string strTag = LaeDesc::prettyMotorCtlrName(nCtlr,
                                    m_pMotorCtlr[nCtlr]->getAddress());

  double  fMin, fMax;         // working min,max values
  double  fMinMin, fMaxMax;   // working min min,max max values
  int     rc;                 // return code

  //
  // Read the logic 'battery' voltage cutoff settings (min, max) and set if
  // necessary.
  //
  // Currently, the LB-/LB+ terminals are not connected, and the logic uses
  // the main battery.
  //
 
  LOGDIAG3("%s: Verifying logic cutoff voltages...", strTag.c_str());

  //
  // Read current settings.
  //
  rc = m_pMotorCtlr[nCtlr]->cmdReadLogicCutoffs(fMin, fMax);

  if( rc != OK )
  {
    LOGWARN("%s: Failed to read logic cutoff voltages.", strTag.c_str());
    return LAE_OK;
  }

  LOGDIAG3("   Current settings: [%.1lf, %.1lf]", fMin, fMax);

  // Get supported full logic range.
  m_pMotorCtlr[nCtlr]->getLogicCutoffRange(fMinMin, fMaxMax);

  //
  // Change settings. The settings are always reset to defaults on power-up,
  // so cannot save to non-volatile memory.
  //
  if( (fMin != fMinMin) || (fMax != fMaxMax) )
  {
    rc = m_pMotorCtlr[nCtlr]->cmdSetLogicCutoffs(fMinMin, fMaxMax);

    if( rc != OK )
    {
      LOGWARN("%s: Failed to set logic cutoff voltages to [%.1lf, %.1lf].",
            strTag.c_str(), fMinMin, fMaxMax);
    }
    else
    {
      LOGDIAG3("   Changed settings: [%.1lf, %.1lf]", fMinMin, fMaxMax);
    }
  }

  //
  // The settings are good as configured.
  //
  else
  {
    LOGDIAG3("   Good.");
  }

  return LAE_OK;
}

int LaeKinematics::configMotorMaxAmpLimit ( const LaeTunes &  tunes, LaePowertrain &  powertrain, bool &  bNvmSave  )

protectedvirtual

Configure motor maximum ampere limit.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	powertrain	Powertrain kinodynmics object.
	strKey	Powertrain key.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 798 of file laeKin.cxx.

References LAE_VERSION, laelaps::LaePowertrain::m_attr, laelaps::LaePowertrainAttr::m_nMotorCtlrId, laelaps::LaePowertrainAttr::m_nMotorId, laelaps::LaePowertrainAttr::m_nMotorIndex, and laelaps::LaePowertrain::m_strName.

Referenced by unlock().

{
  string  strKey    = powertrain.m_strName;
  int     nCtlrId   = powertrain.m_attr.m_nMotorCtlrId;
  int     nMotorId  = powertrain.m_attr.m_nMotorId;
  int     nMotor    = powertrain.m_attr.m_nMotorIndex;
  byte_t  addr      = m_pMotorCtlr[nCtlrId]->getAddress();
  string  strTag    = LaeDesc::prettyMotorName(nCtlrId, addr, nMotorId);

  double  fCurMaxAmps, fMaxAmps;                // max. amp limits
  int     rc;                                   // return code

  //
  // Read motor's maximum current limit, and set if necessary.
  //
  // Note: Not limiting the amps can cause motor burn out.
  //
 
  LOGDIAG3("%s: Verifying maximum amp limit parameter...", strTag.c_str());

  //
  // Read current settings.
  //
  rc = m_pMotorCtlr[nCtlrId]->cmdReadMotorMaxCurrentLimit(nMotor, fCurMaxAmps);

  if( rc != OK )
  {
    LOGERROR("%s: Failed to read maximum current limit.", strTag.c_str());
    return -LAE_ECODE_MOT_CTLR;
  }

  LOGDIAG3("   Current settings: %.2lf amps.", fCurMaxAmps);

  if( RtDb.m_product.m_uProdHwVer == LAE_VERSION(2, 0, 0) )
  {
    fMaxAmps = LaeMotorMaxAmps_2_0 - 0.05;
  }
  else
  {
    fMaxAmps = LaeMotorMaxAmps_2_1 - 0.05;
  }

  //
  // Change settings. The new settings should also be saved to non-volatile
  // memory.
  //
  if( fCurMaxAmps != fMaxAmps )
  {
    rc = m_pMotorCtlr[nCtlrId]->cmdSetMotorMaxCurrentLimit(nMotor, fMaxAmps);

    if( rc != OK)
    {
      LOGERROR("%s: Failed to set maximum current limit to %.2lf.",
          strTag.c_str(), fMaxAmps);
      return -LAE_ECODE_MOT_CTLR;
    }
    else
    {
      LOGDIAG3("   Changed settings: %.2lf amps.", fMaxAmps);
      bNvmSave = true;
    }
  }

  //
  // The settings are good as configured.
  //
  else
  {
    LOGDIAG3("   Good.");
  }

  return LAE_OK;
}

int LaeKinematics::configMotorVelocityPid ( const LaeTunes &  tunes, LaePowertrain &  powertrain, bool &  bNvmSave  )

protectedvirtual

Configure motor velocity PID parameters.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	powertrain	Powertrain kinodynmics object.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 711 of file laeKin.cxx.

References laelaps::LaeTunes::getVelPidKParams(), laelaps::LaePowertrain::m_attr, laelaps::LaePowertrainAttr::m_nMotorCtlrId, laelaps::LaePowertrainAttr::m_nMotorId, laelaps::LaePowertrainAttr::m_nMotorIndex, laelaps::LaePowertrain::m_strName, and laelaps::LaePowertrainAttr::m_uMaxQpps.

Referenced by unlock().

{
  string  strKey    = powertrain.m_strName;
  int     nCtlrId   = powertrain.m_attr.m_nMotorCtlrId;
  int     nMotorId  = powertrain.m_attr.m_nMotorId;
  int     nMotor    = powertrain.m_attr.m_nMotorIndex;
  byte_t  addr      = m_pMotorCtlr[nCtlrId]->getAddress();
  string  strTag    = LaeDesc::prettyMotorName(nCtlrId, addr, nMotorId);

  u32_t   uKp, uKi, uKd, uQpps;                 // existing PID settings.
  double  fTuneKp, fTuneKi, fTuneKd;            // tune PID parameters
  u32_t   uTuneKp, uTuneKi, uTuneKd, uTuneQpps; // converted tune target PID
  int     rc;                                   // return code

  //
  // Read motor's velocity PID parameters, and set if necessary.
  //
   
  LOGDIAG3("%s: Verifying velocity PID parameters...", strTag.c_str());

  //
  // Read current settings.
  //
  rc = m_pMotorCtlr[nCtlrId]->cmdReadVelocityPidConst(nMotor,
                                                      uKp, uKi, uKd, uQpps);

  if( rc != OK )
  {
    LOGWARN("%s: Failed to read velocity PID constants.", strTag.c_str());
    return LAE_OK;
  }

  fTuneKp = (double)uKp / (double)ParamVelPidCvt;
  fTuneKi = (double)uKi / (double)ParamVelPidCvt;
  fTuneKd = (double)uKd / (double)ParamVelPidCvt;

  LOGDIAG3("   Current settings: "
     "Kp=%lf (raw=0x%08x), Ki=%lf (raw=0x%08x), Kd=%lf (raw=0x%08x), Qpps=%u.",
     fTuneKp, uKp, fTuneKi, uKi, fTuneKd, uKd, uQpps);

  // Get the tune parameters in "natural" floating-point representation
  tunes.getVelPidKParams(strKey, fTuneKp, fTuneKi, fTuneKd);

  // Convert to motor controller speak.
  uTuneKp   = (u32_t)(fTuneKp * (double)ParamVelPidCvt);
  uTuneKi   = (u32_t)(fTuneKi * (double)ParamVelPidCvt);
  uTuneKd   = (u32_t)(fTuneKd * (double)ParamVelPidCvt);
  uTuneQpps = (u32_t)(powertrain.m_attr.m_uMaxQpps);

  //
  // Change settings. The new settings should also be saved to non-volatile
  // memory.
  //
  if( (uKp != uTuneKp) || (uKi != uTuneKi) || (uKd != uTuneKd) ||
          (uQpps != uTuneQpps) )
  {
    rc = m_pMotorCtlr[nCtlrId]->cmdSetVelocityPidConst(nMotor,
                                      uTuneKp, uTuneKi, uTuneKd, uTuneQpps);

    if( rc != OK)
    {
      LOGWARN("%s: Failed to set velocity PID constants.", strTag.c_str());
    }
    else
    {
      LOGDIAG3("   Changed settings: "
              "Kp=%lf (raw=0x%08x), Ki=%lf (raw=0x%08x), Kd=%lf (raw=0x%08x), "
              "Qpps=%u.",
            fTuneKp, uTuneKp, fTuneKi, uTuneKi, fTuneKd, uTuneKd, uTuneQpps);

      bNvmSave = true;
    }
  }

  //
  // The settings are good as configured.
  //
  else
  {
    LOGDIAG3("   Good.");
  }

  return LAE_OK;
}

int LaeKinematics::configure ( const LaeDesc &  desc )

virtual

Configure kinematics chains and data from product description.

Parameters

desc	Product description.

Returns

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

Definition at line 331 of file laeKin.cxx.

References laelaps::LaePowertrain::configure(), laelaps::LaeDesc::m_mapDescPowertrain, laelaps::LaeDesc::m_pDescBase, and laelaps::LaePowertrain::m_strName.

Referenced by areMotorsPowered(), and laelaps::LaeRobot::configForOperation().

{
  LaeDesc::MapDescPowertrain::const_iterator iter;
  LaePowertrain   train;
  int             rc = LAE_OK;

  //
  // Powertrains
  //
  for(iter = desc.m_mapDescPowertrain.begin();
      iter != desc.m_mapDescPowertrain.end();
      ++iter)
  {
    // configure powertrain kinematics
    train.configure(*iter->second);

    //
    // Sanity checks.
    //

    //
    // Add to kinematic chain.
    //
    m_kinPowertrains[train.m_strName] = train;
  }

  //
  // Robot platform
  //
  m_kinPlatform.configure(*desc.m_pDescBase);

  LOGDIAG2("Configured motor controllers from robot description.");

  return rc;
}

int LaeKinematics::configure ( const LaeTunes &  tunes )

virtual

Configure kinematics for operation.

Context:

Calling thread.

Parameters

tunes

Laelaps tuning parameters.

Returns

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

Definition at line 367 of file laeKin.cxx.

{
  int     nCtlr;              // motor controller id
  int     rc;                 // return code

  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    if( (rc = configureMotorController(tunes, nCtlr)) != LAE_OK )
    {
      break;
    }
  }

  m_kinPlatform.configure(tunes);

  if( rc == LAE_OK )
  {
    LOGDIAG2("Tuned motor controllers.");
  }

  return rc;
}

int LaeKinematics::configureMotorController ( const LaeTunes &  tunes, int  nCtlr  )

protectedvirtual

Configure motor controller.

Context:

Calling thread.

Parameters

tunes	Laelaps tuning parameters.
nCtlr	Motor controller id.

Returns

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

Definition at line 390 of file laeKin.cxx.

Referenced by unlock().

{
  bool    bNvmSave;           // do [not] save changes to ctlr's eeprom
  int     rc;                 // return code

  bNvmSave   = false;

  //
  // Make sure motor controllers are configured correctly.
  //
  // All operations are read, conditional writes.
  // Both controllers have identical config.
  //

  // battery cutoffs
  if( (rc = configMotorCtlrBatteryCutoffs(tunes, nCtlr, bNvmSave)) != LAE_OK )
  {
    return rc;
  }

  // logic cutoffs
  if( (rc = configMotorCtlrLogicCutoffs(tunes, nCtlr, bNvmSave)) != LAE_OK )
  {
    return rc;
  }

  // encoder modes
  if( (rc = configMotorCtlrEncoderModes(tunes, nCtlr, bNvmSave)) != LAE_OK )
  {
    return rc;
  }

  //
  // Configure the motor controller's powertrain pair.
  //
  if( (rc = configurePtp(tunes, nCtlr, bNvmSave)) != LAE_OK )
  {
    return rc;
  }

  //
  // If any settings, that can be saved, have changed, then save to
  // non-volatile EEPROM.
  //
  if( bNvmSave )
  {
    saveConfigToNvm(nCtlr);
  }

  //
  // Set controller state values.
  //
  resetMotorCtlrEncoders(nCtlr);

  return LAE_OK;
}

int LaeKinematics::configurePtp ( const LaeTunes &  tunes, int  nCtlr, bool &  bNvSave  )

protectedvirtual

Configure powertrain pair.

Context:

Calling thread.

Parameters

	tunes	Laelaps tuning parameters.
	nCtlr	Motor controller id.
[out]	bNvmSave	Do [not] save configuration in motor contoller's non-volatile memory.

Returns

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

Definition at line 664 of file laeKin.cxx.

Referenced by unlock().

{
  LaeMapPowertrain::iterator pos;

  int     nMotor;                               // motor index
  int     rc;                                   // return code

  string strKey = m_pMotorCtlr[nCtlr]->getNameId();

  for(nMotor = LaeMotorLeft; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
  {
    strKey = LaePowertrain::toKey(nCtlr, nMotor);

    // - - - -
    // Update powertrain attributes with tuning parameters values.
    // - - - -
    if( (pos = m_kinPowertrains.find(strKey)) == m_kinPowertrains.end() )
    {
      LOGWARN("Cannot find powertrain %s - ignoring.", strKey.c_str());
      continue;
    }

    // configure parameters for powertrain kinodynamics
    pos->second.configure(tunes);

    // velocity PID
    if( (rc = configMotorVelocityPid(tunes, pos->second, bNvmSave)) != LAE_OK )
    {
      return rc;
    }

    // maximum ampere limit
    if( (rc = configMotorMaxAmpLimit(tunes, pos->second, bNvmSave)) != LAE_OK )
    {
      return rc;
    }

    //
    // RDK TODO 63,64 Read motor position PID
    // RDK TODO 61,62 Set motor position PID
  }

  return LAE_OK;
}

void LaeKinematics::estop ( )

virtual

Emergency stop the robot.

All motors will stop and power to motor controllers disabled.

Context:

Calling thread.

Definition at line 1064 of file laeKin.cxx.

Referenced by laelaps::LaeRobot::estop(), and getPowertrainMap().

{
  int     nCtlr;    // motor controller id

  lock();

  if( m_bIsEnabled )
  {
    m_pAction->terminate();

    for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
    {
      m_pMotorCtlr[nCtlr]->cmdEStop();
    }

    disableMotorCtlrs();
  }

  unlock();
}

void LaeKinematics::exec ( )

virtual

Execute kinematics task(s).

Context:

Kinematic thread.

Definition at line 1600 of file laeKin.cxx.

Referenced by laelaps::LaeThreadKin::exec(), and freeze().

{
  lock();

  //
  // Auto-Repair
  //
  // The motor controllers should be enabled, but the watchdog thread has
  // detected that they are not. This should not happen, so re-enable.
  //
  if( m_bIsEnabled && !RtDb.m_enable.m_bMotorCtlr )
  {
    LOGERROR("Motor controllers have been unexpectedly disaabled. Re-enable.");
    unlock();
    resyncComm();
    lock();
  }

  // sense environment and dynamic state
  sense();

  // react to immediate stimulii
  react();

  // plan motions
  plan();

  // act on planned motions
  act();

  unlock();
}

virtual int laelaps::LaeKinematics::freeze ( )

inlinevirtual

Freeze (stop) all motors at the current position.

Synomonous with stop().

Context:

Calling thread.

Returns

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

Definition at line 355 of file laeKin.h.

References exec(), monitorHealth(), release(), setGoalDutyCycles(), setGoalTwist(), setGoalVelocities(), stop(), and waitForAllStop().

    {
      return stop();
    }

RoboClaw * LaeKinematics::getMotorCtlr

(

const int

nMotorCtlrId

)

Get pointer to motor controller by name (key).

Parameters

nMotorCtlrId

Motor controller unique identifier.

Returns

On success, the pointer to motor controller object is returned.
On failure, NULL is returned.

Definition at line 918 of file laeKin.cxx.

Referenced by laelaps::LaeKinActionVelocity::execute(), laelaps::LaeKinActionDutyCycle::execute(), laelaps::LaeKinActionTwist::execute(), getPlatform(), laelaps::LaeKinActionVelocity::terminate(), laelaps::LaeKinActionDutyCycle::terminate(), and laelaps::LaeKinActionTwist::terminate().

{
  if( (nMotorCtlrId >= 0) && (nMotorCtlrId < LaeNumMotorCtlrs) )
  {
    return m_pMotorCtlr[nMotorCtlrId];
  }
  else
  {
    return NULL;
  }
}

LaePlatform& laelaps::LaeKinematics::getPlatform ( )

inline

Get robot platform kinodynamics.

Returns

Returns reference to robot platform kinodynamics object.

Definition at line 226 of file laeKin.h.

References getMotorCtlr(), and m_kinPlatform.

Referenced by laelaps::LaeRptRobotStatus::generate(), and laelaps::LaeKinActionTwist::plan().

    {
      return m_kinPlatform;
    }

LaePowertrain* laelaps::LaeKinematics::getPowertrain

(

const std::string &

strName

)

Get pointer to powertrain by name (key).

Parameters

strName

Powertrain name (key).

Returns

On success, the pointer to powertrain kinodynamics object is returned.
On failure, NULL is returned.

Referenced by laelaps::LaeKinActionDutyCycle::execute(), laelaps::LaeRobot::getNavigationState(), laelaps::LaeRobot::getPowertrain(), getPowertrainMap(), laelaps::LaeKinActionVelocity::plan(), and laelaps::LaeKinActionTwist::plan().

LaePowertrain * LaeKinematics::getPowertrain	(	int	nCtlr,
		int	nMotor
	)

Get pointer to powertrain by controller id and motor index.

Parameters

nCtlr	Motor controller id.
nMotor	Motor index within motor controller.

Returns

On success, the pointer to powertrain kinodynamics object is returned.
On failure, NULL is returned.

Definition at line 942 of file laeKin.cxx.

{
  std::string strName = LaePowertrain::toKey(nCtlr, nMotor);

  if( m_kinPowertrains.find(strName) != m_kinPowertrains.end() )
  {
    return &m_kinPowertrains[strName];
  }
  else
  {
    return NULL;
  }
}

LaePowertrain * LaeKinematics::getPowertrain

(

int

nMotorId

)

Get pointer to powertrain by motor id.

Parameters

nMotorId

Motor id.

Returns

On success, the pointer to powertrain kinodynamics object is returned.
On failure, NULL is returned.

Definition at line 956 of file laeKin.cxx.

{
  std::string strName = LaePowertrain::toKey(nMotorId);

  if( m_kinPowertrains.find(strName) != m_kinPowertrains.end() )
  {
    return &m_kinPowertrains[strName];
  }
  else
  {
    return NULL;
  }
}

LaeMapPowertrain& laelaps::LaeKinematics::getPowertrainMap ( )

inline

Get map of all powertrain kinodynamics.

Returns

Returns reference to map of powertrain kinodynamics objects.

Definition at line 247 of file laeKin.h.

References estop(), getPowertrain(), getPowertrainState(), isStopped(), m_kinPowertrains, resetEStop(), and resetOdometers().

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

    {
      return m_kinPowertrains;
    }

int LaeKinematics::getPowertrainState ( const std::string &  strName, LaePowertrainState &  state  )

virtual

Get the current powertrain state.

Context:

Calling thread.

Parameters

[in]	strName	Powertrain unique name.
[out]	state	Current powertrain state.

Returns

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

Definition at line 970 of file laeKin.cxx.

Referenced by getPowertrainMap().

{
  LaeMapPowertrain::iterator pos;

  if( (pos = m_kinPowertrains.find(strName)) != m_kinPowertrains.end() )
  {
    jointState = pos->second.m_state;
    return LAE_OK;
  }
  else
  {
    return -LAE_ECODE_BAD_VAL;
  }
}

virtual bool laelaps::LaeKinematics::isEnabled ( )

inlinevirtual

Test if motors are enabled.

Returns

Returns true or false.

Definition at line 173 of file laeKin.h.

References m_bIsEnabled.

Referenced by laelaps::LaeRobot::estop(), and laelaps::LaeRobot::resetEStop().

    {
      return m_bIsEnabled;
    }

virtual bool laelaps::LaeKinematics::isOpen ( )

inlinevirtual

Test if connection is open.

Returns

Returns true or false.

Definition at line 163 of file laeKin.h.

References motor::roboclaw::RoboClawComm::isOpen(), and m_commMotorCtlrs.

Referenced by Laelaps.WatchDog.WatchDog::cmdConfigDPin(), Laelaps.WatchDog.WatchDog::cmdEnableAuxPort5V(), Laelaps.WatchDog.WatchDog::cmdEnableAuxPortBatt(), Laelaps.WatchDog.WatchDog::cmdEnableMotorCtlrs(), Laelaps.WatchDog.WatchDog::cmdGetFwVersion(), Laelaps.WatchDog.WatchDog::cmdPetTheDog(), Laelaps.WatchDog.WatchDog::cmdReadAPin(), Laelaps.WatchDog.WatchDog::cmdReadDPin(), Laelaps.WatchDog.WatchDog::cmdReadEnables(), Laelaps.WatchDog.WatchDog::cmdReadVoltages(), Laelaps.WatchDog.WatchDog::cmdResetRgbLed(), Laelaps.WatchDog.WatchDog::cmdSetAlarms(), Laelaps.WatchDog.WatchDog::cmdSetBatterySoC(), Laelaps.WatchDog.WatchDog::cmdSetRgbLed(), and Laelaps.WatchDog.WatchDog::cmdWriteDPin().

    {
      return m_commMotorCtlrs.isOpen();
    }

bool LaeKinematics::isStopped ( )

virtual

Test if all motors are stopped.

Returns

Returns true if all are stopped, false otherwise.

Definition at line 986 of file laeKin.cxx.

References iabs().

Referenced by getPowertrainMap(), and laelaps::LaeRobot::isInMotion().

{
  bool  bIsStopped;                 // all motors are [not] stopped
  LaeMapPowertrain::iterator iter;  // kinematics chain iterator

  for(iter = m_kinPowertrains.begin(), bIsStopped = true;
      (iter != m_kinPowertrains.end()) && bIsStopped;
      ++iter)
  {
    if( iabs(iter->second.m_state.m_nSpeed) > SpeedReallyZero )
    {
      bIsStopped = false;
    }
  }

  m_bIsStopped = bIsStopped;
  RtDb.m_robotstatus.m_bInMotion = m_bIsStopped? false: true;

  return m_bIsStopped;
}

bool LaeKinematics::isStopped ( const std::string &  strName )

virtual

Test if a powertrain motor is stopped.

Parameters

strName

Powertrain unique name.

Returns

Returns true if stopped, false otherwise.

Definition at line 1007 of file laeKin.cxx.

References iabs().

{
  LaeMapPowertrain::iterator pos;

  if( (pos = m_kinPowertrains.find(strName)) != m_kinPowertrains.end() )
  {
    return iabs(pos->second.m_state.m_nSpeed) <= SpeedReallyZero;
  }
  else // non-existent joints don't move
  {
    return true;
  }
}

void laelaps::LaeKinematics::lock ( )

inlineprotected

Lock the shared resource.

The lock()/unlock() primitives provide a safe multi-threading.

Context:

Any.

Definition at line 539 of file laeKin.h.

    {
      pthread_mutex_lock(&m_mutex);
    }

int LaeKinematics::monitorHealth ( )

virtual

Monitor kinematics health.

Context:

Kinematics thread.

Returns

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

Definition at line 1551 of file laeKin.cxx.

References laelaps::LaePowertrain::updateHealth().

Referenced by laelaps::LaeThreadKin::exec(), and freeze().

{
  int               nCtlr;          // motor controller id
  int               nMotor;         // motor index
  double            volts;          // main battery voltage
  double            temp;           // board temperature
  uint_t            status;         // board status
  LaePowertrain    *pTrain;         // pointer to joint

  if( !m_bIsEnabled )
  {
    return LAE_OK;
  }

  lock();

  // 
  // Read health data.
  //
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    m_pMotorCtlr[nCtlr]->cmdReadMainBattVoltage(volts);
    m_pMotorCtlr[nCtlr]->cmdReadBoardTemperature(temp);
    m_pMotorCtlr[nCtlr]->cmdReadStatus(status);

    RtDb.m_motorctlr[nCtlr].m_uStatus         = status;
    RtDb.m_motorctlr[nCtlr].m_fTemperature    = temp;
    RtDb.m_motorctlr[nCtlr].m_fBatteryVoltage = volts;

    for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
    {
      if( (pTrain = getPowertrain(nCtlr, nMotor)) != NULL )
      {
        pTrain->updateHealth(volts, temp, status);
      }
    }

    m_kinPlatform.updateCtlrHealth(nCtlr, volts, temp, status);
  }

  m_kinPlatform.updateHealth(m_kinPowertrains);

  LOGDIAG4("Kinematics Thread: Monitored health of motor controllers.");

  unlock();

  return LAE_OK;
}

int LaeKinematics::open	(	const std::string &	strDevMotorCtlrs,
		const int	nBaudRate,
		int(*)(void *, bool)	fnEnable = NULL,
		void *	pEnableArg = NULL
	)

Open communication with the Laelaps motor controllers.

Motors controller serial interface support multi-drop, so one serial device can support up to 8 motor controllers. A GPIO pin selects the motor controller.

Parameters

strDevMotorCtlrs	Motor controllers serial device name.
nBaudRate	Motor controllers serial baudrate.
fnEnable	Motor controllers enable power function.
pEnableArg	Enable power function argument.

Returns

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

Definition at line 141 of file laeKin.cxx.

References laelaps::getRealDeviceName().

Referenced by laelaps::LaeRobot::connMotorControllers().

{
  string  strDevName;   // real device names
  int     nCtlr;        // motor controller id

  m_fnEnableMotorCtlrs  = fnEnable;
  m_pEnableArg          = pEnableArg;
  m_bIsEnabled          = false;

  // Enable power to motor controllers
  enableMotorCtlrs();

  if( !m_bIsEnabled )
  {
    LOGERROR("Failed to enable power to motor controllers.");
    return -LAE_ECODE_IO;
  }

#ifdef LAE_DEPRECATED
  // Open the motor controller chip select via gpio
  if( m_csMotorCtlrs.open(LaeGpioMotorCtlrCs) < 0 )
  {
    LOGSYSERROR("Failed to open motor controller chip select on GPIO pin %d.",
        LaeGpioMotorCtlrCs);
    return -LAE_ECODE_MOT_CTLR;
  }

  LOGDIAG2("Opened motor controllers chip select signal on GPIO %d.",
        LaeGpioMotorCtlrCs);
#endif // LAE_DEPRECATED

  // Get the real device name, not any symbolic links.
  strDevName  = getRealDeviceName(strDevMotorCtlrs);

  // Open the motor controller.
  if( m_commMotorCtlrs.open(strDevName, nBaudRate) < 0 )
  {
    LOGERROR("%s: Failed to open motor controller port at %d baud.",
        strDevName.c_str(), nBaudRate);
    return -LAE_ECODE_MOT_CTLR;
  }

  LOGDIAG3("Open motor controllers serial communication on %s@%d.",
        strDevName.c_str(), nBaudRate);

  // 
  // Create front motor controller interface.
  //
  nCtlr = LaeMotorCtlrIdFront;
  m_pMotorCtlr[nCtlr] = 
            new RoboClaw(m_commMotorCtlrs, LaeMotorCtlrAddrFront, LaeKeyFront);
  m_pMotorCtlr[nCtlr]->setMotorDir(LaeMotorLeft, LaeMotorDirNormal);
  m_pMotorCtlr[nCtlr]->setMotorDir(LaeMotorRight, LaeMotorDirNormal);

  // 
  // Create rear motor controller interface.
  //
  nCtlr = LaeMotorCtlrIdRear;
  m_pMotorCtlr[nCtlr]  =
            new RoboClaw(m_commMotorCtlrs, LaeMotorCtlrAddrRear, LaeKeyRear);
  m_pMotorCtlr[nCtlr]->setMotorDir(LaeMotorLeft, LaeMotorDirNormal);
  m_pMotorCtlr[nCtlr]->setMotorDir(LaeMotorRight, LaeMotorDirNormal);

  m_bAreMotorsPowered = false;
  m_bIsStopped        = true;

  RtDb.m_robotstatus.m_bAreMotorsPowered  = m_bAreMotorsPowered;
  RtDb.m_robotstatus.m_bInMotion          = m_bIsStopped? false: true;

  LOGDIAG2("Created front and rear motor controllers on %s@%d.",
        strDevName.c_str(), nBaudRate);
  //LOGDIAG2("Front and rear motor controllers created on %s@%d, cs=%d.",
  //      strDevName.c_str(), nBaudRate, LaeGpioMotorCtlrCs);

  return LAE_OK;
}

int LaeKinematics::plan ( )

protectedvirtual

Plan kinematic action.

Context:

Kinematics thread.

Returns

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

Definition at line 1501 of file laeKin.cxx.

Referenced by unlock(), laelaps::LaeKinActionTwist::~LaeKinActionTwist(), and laelaps::LaeKinActionVelocity::~LaeKinActionVelocity().

{
  int     rc;

  // motors are not enabled
  if( !m_bIsEnabled )
  {
    return LAE_OK;
  }

  // planning requried, so plan
  else if( m_pAction->isPlanningRequired() )
  {
    rc = m_pAction->plan();
  }

  // nothing to plan
  else
  {
    rc = LAE_OK;
  }

  return rc;
}

int LaeKinematics::react ( )

protectedvirtual

React to any necessary time-critical events.

Context:

Kinematics thread.

Returns

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

Definition at line 1496 of file laeKin.cxx.

Referenced by unlock().

{
  return LAE_OK; // RDK TODO
}

int LaeKinematics::release ( )

virtual

Release motors.

Motors will stop, but are free to roll.

Context:

Calling thread.

Returns

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

Definition at line 1094 of file laeKin.cxx.

Referenced by freeze(), and laelaps::LaeRobot::release().

{
  int   nCtlr;  // motor controller id
  int   rc;     // return code

  lock();

  if( m_bIsEnabled )
  {
    m_pAction->terminate();

    for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
    {
      m_pMotorCtlr[nCtlr]->cmdDutyDrive2(0.0, 0.0);
    }

    m_bAreMotorsPowered = false;
    RtDb.m_robotstatus.m_bAreMotorsPowered = m_bAreMotorsPowered;

    rc = LAE_OK;
  }
  else
  {
    rc = -LAE_ECODE_BAD_OP;
  }

  unlock();

  return rc;
}

int LaeKinematics::reload ( const LaeTunes &  tunes )

virtual

Reload configuration tuning parameters.

Context:

Calling thread.

Parameters

tunes

Laelaps tuning parameters.

Returns

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

Definition at line 897 of file laeKin.cxx.

Referenced by areMotorsPowered(), and laelaps::LaeThreadKin::reload().

{
  LaeMapPowertrain::iterator iter;    // kinematics chain iterator

  LOGDIAG2("Reloading motor controller tuning parameters...");

  for(iter = m_kinPowertrains.begin(); iter != m_kinPowertrains.end(); ++iter)
  {
    iter->second.reload(tunes);
  }

  m_kinPlatform.reload(tunes);

  return LAE_OK;
}

void LaeKinematics::resetEStop ( )

virtual

Reset emergency stop condition.

Context:

Calling thread.

Definition at line 1085 of file laeKin.cxx.

Referenced by getPowertrainMap(), and laelaps::LaeRobot::resetEStop().

{
  lock();

  enableMotorCtlrs();

  unlock();
}

int LaeKinematics::resetMotorCtlrEncoders ( int  nCtlr )

protectedvirtual

Reset all motor controller encoders to zero.

Context:

Calling thread.

Parameters

nCtlr

Motor controller id.

Returns

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

Definition at line 643 of file laeKin.cxx.

Referenced by unlock().

{
  string strTag = LaeDesc::prettyMotorCtlrName(nCtlr,
                                    m_pMotorCtlr[nCtlr]->getAddress());

  int rc;   // return code

  rc = m_pMotorCtlr[nCtlr]->cmdResetQEncoders();

  if( rc == OK )
  {
    LOGDIAG3("%s: Encoders reset.", strTag.c_str());
    return LAE_OK;
  }
  else
  {
    LOGWARN("%s: Failed to reset encoders.", strTag.c_str());
    return -LAE_ECODE_MOT_CTLR;
  }
}

int LaeKinematics::resetOdometers ( )

virtual

Reset all motors' odometers to the current respective encoder positions.

Context:

Calling thread.

Returns

Number of motor odometers reset.

Definition at line 1026 of file laeKin.cxx.

References laelaps::LaePowertrain::resetOdometer().

Referenced by getPowertrainMap().

{
  int             nCtlr;    // motor controller id
  int             nMotor;   // motor index
  LaePowertrain  *pTrain;   // working powertrain
  int             n;        // number of powertrain odometers actually resetted

  lock();

  n = 0;

  if( m_bIsEnabled )
  {
    m_pAction->terminate();

    for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
    {
      if( m_pMotorCtlr[nCtlr]->cmdResetQEncoders() == OK )
      {
        for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
        {
          if( (pTrain = getPowertrain(nCtlr, nMotor)) != NULL )
          {
            pTrain->resetOdometer();
            ++n;
          }
        }
      }
    }

    m_kinPlatform.resetOdometer();
  }

  unlock();

  return n;
}

int LaeKinematics::saveConfigToNvm ( int  nCtlr )

protectedvirtual

Save configuration to motor controller's non-volatile memory.

Context:

Calling thread.

Parameters

nCtlr

Motor controller id.

Returns

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

Definition at line 874 of file laeKin.cxx.

Referenced by unlock().

{
  string strTag = LaeDesc::prettyMotorCtlrName(nCtlrId,
                                    m_pMotorCtlr[nCtlrId]->getAddress());

  int   rc;   // return code

  LOGDIAG3("%s: Saving new values to EEPROM.", strTag.c_str());

  rc = m_pMotorCtlr[nCtlrId]->cmdWriteSettingsToEEPROM();

  if( rc == OK )
  {
    LOGDIAG3("  Saved.");
  }
  else
  {
    LOGWARN("%s: Failed to write settings to EEPROM.", strTag.c_str());
  }

  return LAE_OK;
}

int LaeKinematics::sense ( )

protectedvirtual

Sense real-time state.

Context:

Kinematics thread.

Returns

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

Definition at line 1423 of file laeKin.cxx.

Referenced by unlock().

{
  int   rc;

  // sense real-time kinodynamics
  rc = senseDynamics();

  // add other sensor reads here (e.g. imu, gps, etc).

  return rc;
}

int LaeKinematics::senseDynamics ( )

protectedvirtual

Sense real-time kinodynamics.

Context:

Kinematics thread.

Returns

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

Definition at line 1435 of file laeKin.cxx.

References iabs(), and laelaps::LaePowertrain::updateStateDynamics().

Referenced by unlock().

{
  int             nCtlr;                        // motor controller id
  int             nMotor;                       // motor index
  double          amp[LaeNumMotorsPerCtlr];     // motor ampere draws
  uint_t          buflen[LaeNumMotorsPerCtlr];  // command buffer lengths
  s64_t           encoder;                      // motor encoder (quad pulses)
  s32_t           speed;                        // motor speed (qpps)
  bool            bIsStopped;                   // all motors are [not] stopped
  LaePowertrain  *pTrain;                       // working powertrain

  if( !m_bIsEnabled )
  {
    return LAE_OK;
  }

  bIsStopped = true;

  //
  // Sense real-time dynamics.
  //
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    //
    // Board level sensing.
    //
    m_pMotorCtlr[nCtlr]->cmdReadMotorCurrentDraw(amp[0], amp[1]);
    m_pMotorCtlr[nCtlr]->cmdReadCmdBufLengths(buflen[0], buflen[1]);

    //
    // Motor level sensing.
    //
    for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
    {
      m_pMotorCtlr[nCtlr]->cmdReadQEncoder(nMotor, encoder);
      m_pMotorCtlr[nCtlr]->cmdReadQSpeed(nMotor, speed);

      if( (pTrain = getPowertrain(nCtlr, nMotor)) == NULL )
      {
        continue;
      }

      pTrain->updateStateDynamics(encoder, speed, amp[nMotor], buflen[nMotor]);

      if( iabs(speed) > SpeedReallyZero )
      {
        bIsStopped = false;
      }

      RtDb.m_motorctlr[nCtlr].m_fMotorCurrent[nMotor] = amp[nMotor];
    }
  }

  m_kinPlatform.updateStateDynamics(m_kinPowertrains);

  m_bIsStopped = bIsStopped;
  RtDb.m_robotstatus.m_bInMotion = m_bIsStopped? false: true;

  return LAE_OK;
}

int LaeKinematics::setGoalDutyCycles ( const LaeMapDutyCycle &  duty )

virtual

Set new or updated motor duty cycle goals.

The duty cycle actions are very simple actions (contrast with a multi-position task). Therefore, to reduce thread context switching and increase responsiveness, after the new/updated duty goals are set, the plan and execute steps are immediately called.

Context:

Calling thread.

Parameters

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

Definition at line 1322 of file laeKin.cxx.

References laelaps::LaeKinActionDutyCycle::execute(), laelaps::LaeKinAction::isExecutionRequired(), laelaps::LaeKinAction::isPlanningRequired(), laelaps::LaeKinAction::plan(), and laelaps::LaeKinActionDutyCycle::update().

Referenced by freeze(), and laelaps::LaeRobot::setDutyCycles().

{
  LaeKinActionDutyCycle  *pAction;  // duty cycle action pointer
  int                     rc;       // return code

  lock();

  if( m_bIsEnabled )
  {
    //
    // Current action is not a velocity action. Terminate the action and
    // create an this new action.
    //
    if( m_pAction->getActionType() != LaeKinAction::ActionTypeDutyCycle )
    {
      m_pAction = LaeKinAction::replaceAction(*this, m_pAction,
                                            LaeKinAction::ActionTypeDutyCycle); 
    }

    pAction = (LaeKinActionDutyCycle *)m_pAction;

    //  update with new goals
    rc = pAction->update(duty);

    if( pAction->isPlanningRequired() )
    {
      rc = pAction->plan();
    }

    if( pAction->isExecutionRequired() )
    {
      if( (rc = pAction->execute()) == LAE_OK )
      {
        m_bAreMotorsPowered = true;
        RtDb.m_robotstatus.m_bAreMotorsPowered = m_bAreMotorsPowered;
      }
    }
  }
  else
  {
    rc = -LAE_ECODE_BAD_OP;
  }

  unlock();

  return rc;
}

int LaeKinematics::setGoalTwist ( double  fVelLinear, double  fVelAngular  )

virtual

Set new or updated robot twist velocity goals.

The velocity actions are very simple actions (contrast with a multi-position task). Therefore, to reduce thread context switching and increase responsiveness, after the new/updated velocity goals are set, the plan and execute steps are immediately called.

Context:

Calling thread.

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 1370 of file laeKin.cxx.

References laelaps::LaeKinActionTwist::execute(), laelaps::LaeKinAction::isExecutionRequired(), laelaps::LaeKinAction::isPlanningRequired(), laelaps::LaeKinActionTwist::plan(), and laelaps::LaeKinActionTwist::update().

Referenced by freeze(), and laelaps::LaeRobot::move().

{
  LaeKinActionTwist  *pAction;  // velocity action pointer
  int                 rc;       // return code

  lock();

  if( m_bIsEnabled )
  {
    //
    // Current action is not a velocity action. Terminate the action and
    // create an this new action.
    //
    if( m_pAction->getActionType() != LaeKinAction::ActionTypeTwist )
    {
      m_pAction = LaeKinAction::replaceAction(*this, m_pAction,
                                            LaeKinAction::ActionTypeTwist); 
    }

    pAction = (LaeKinActionTwist *)m_pAction;

    //  update with new goals
    rc = pAction->update(fVelLinear, fVelAngular);

    if( pAction->isPlanningRequired() )
    {
      rc = pAction->plan();
    }

    if( pAction->isExecutionRequired() )
    {
      if( (rc = pAction->execute()) == LAE_OK )
      {
        m_bAreMotorsPowered = true;
        RtDb.m_robotstatus.m_bAreMotorsPowered = m_bAreMotorsPowered;
      }
    }
  }
  else
  {
    rc = -LAE_ECODE_BAD_OP;
  }

  unlock();

  return rc;
}

int LaeKinematics::setGoalVelocities ( const LaeMapVelocity &  velocity )

virtual

Set new or updated motor velocity goals.

The velocity actions are very simple actions (contrast with a multi-position task). Therefore, to reduce thread context switching and increase responsiveness, after the new/updated velocity goals are set, the plan and execute steps are immediately called.

Context:

Calling thread.

Parameters

velocity

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

Definition at line 1274 of file laeKin.cxx.

References laelaps::LaeKinActionVelocity::execute(), laelaps::LaeKinAction::isExecutionRequired(), laelaps::LaeKinAction::isPlanningRequired(), laelaps::LaeKinActionVelocity::plan(), and laelaps::LaeKinActionVelocity::update().

Referenced by freeze(), and laelaps::LaeRobot::move().

{
  LaeKinActionVelocity *pAction;  // velocity action pointer
  int                   rc;       // return code

  lock();

  if( m_bIsEnabled )
  {
    //
    // Current action is not a velocity action. Terminate the action and
    // create an this new action.
    //
    if( m_pAction->getActionType() != LaeKinAction::ActionTypeVelocity )
    {
      m_pAction = LaeKinAction::replaceAction(*this, m_pAction,
                                            LaeKinAction::ActionTypeVelocity); 
    }

    pAction = (LaeKinActionVelocity *)m_pAction;

    //  update with new goals
    rc = pAction->update(velocity);

    if( pAction->isPlanningRequired() )
    {
      rc = pAction->plan();
    }

    if( pAction->isExecutionRequired() )
    {
      if( (rc = pAction->execute()) == LAE_OK )
      {
        m_bAreMotorsPowered = true;
        RtDb.m_robotstatus.m_bAreMotorsPowered = m_bAreMotorsPowered;
      }
    }
  }
  else
  {
    rc = -LAE_ECODE_BAD_OP;
  }

  unlock();

  return rc;
}

int LaeKinematics::stop ( )

virtual

Stop all motors at the current position.

The motor brakes, if available, are applied.

Note

The current RoboClaw motor controllers do not have electrical braking and the current Laelaps versions do not have mechanical braking.

Context:

Calling thread.

Returns

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

Definition at line 1125 of file laeKin.cxx.

Referenced by laelaps_tune_motors.window::createLeftButtons(), laelaps::LaeRobot::freeze(), and freeze().

{
  int   nCtlr;  // motor controller id
  int   rc;     // return code

  lock();

  if( m_bIsEnabled )
  {
    m_pAction->terminate();

    for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
    {
      //m_pMotorCtlr[nCtlr]->cmdQStop();
      m_pMotorCtlr[nCtlr]->cmdStopWithDecel(StopDecel);
    }

    m_bAreMotorsPowered = true;
    RtDb.m_robotstatus.m_bAreMotorsPowered = m_bAreMotorsPowered;

    rc = LAE_OK;
  }
  else
  {
    rc = -LAE_ECODE_BAD_OP;
  }

  unlock();

  return rc;
}

virtual int laelaps::LaeKinematics::stop ( const std::vector< std::string > &  vecNames )

virtual

Stop the set of powertrain motors at the current position.

Context:

Calling thread.

Parameters

vecName

Vector of unique powertrain names.

Returns

Number of powertrain motors stopped.

Referenced by laelaps_tune_motors.window::createLeftButtons().

virtual int laelaps::LaeKinematics::stop ( const std::string &  strName )

virtual

Stop one powertrain motor at the current position.

Context:

Calling thread.

Parameters

strName

Powertrain unique name.

Returns

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

Referenced by laelaps_tune_motors.window::createLeftButtons().

void laelaps::LaeKinematics::unlock ( )

inlineprotected

Unlock the shared resource.

Context:

Any.

Definition at line 550 of file laeKin.h.

References act(), configMotorCtlrBatteryCutoffs(), configMotorCtlrEncoderModes(), configMotorCtlrLogicCutoffs(), configMotorMaxAmpLimit(), configMotorVelocityPid(), configureMotorController(), configurePtp(), disableMotorCtlrs(), enableMotorCtlrs(), plan(), react(), resetMotorCtlrEncoders(), resyncComm(), saveConfigToNvm(), sense(), and senseDynamics().

    {
      pthread_mutex_unlock(&m_mutex);
    }

int LaeKinematics::waitForAllStop ( double  fSeconds )

virtual

Wait for all motors to stop.

Context:

Calling thread.

Parameters

fSeconds

Maximum number of seconds to wait.

Returns

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

Definition at line 1253 of file laeKin.cxx.

Referenced by freeze().

{
  static const useconds_t WaitDt  = 100;  // usec wait between tests

  int nMaxTries = (int)ceil(fSeconds * MILLION / (double)WaitDt);

  for(int i = 0; i < nMaxTries; ++i)
  {
    if( isStopped() )
    {
      return LAE_OK;
    }
    usleep(WaitDt);
  }

  LOGERROR("Timed out waiting for %.2lf seconds for all joints to stop.",
      fSeconds);

  return -LAE_ECODE_TIMEDOUT;
}

---

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

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