laelaps::LaePowertrain Class Reference

Powertrain data class. More...

#include <laePowertrain.h>

Public Member Functions
	LaePowertrain ()
	Default constructor.
	LaePowertrain (const LaePowertrain &src)
	Copy constructor.
	~LaePowertrain ()
	Destructor.
LaePowertrain	operator= (const LaePowertrain &rhs)
	Assignment operator. More...
std::string	getName () const
	Get this powertrain unique name (key). More...
int	getMotorId () const
	Get motor id. More...
int	getMotorCtlrId () const
	Get motor controller id. More...
int	getMotorIndex () const
	Get motor controller motor index. More...
virtual int	configure (const LaeDescPowertrain &desc)
	Configure powertrain from product description. More...
virtual int	configure (const LaeTunes &tunes)
	Configure powertrain from tune parameters. More...
virtual int	reload (const LaeTunes &tunes)
	Reload tuning parameters and re-configure. More...
virtual int	resetOdometer ()
	Reset odometry to zero. More...
virtual int	updateStateDynamics (s64_t nEncoder, s32_t nSpeed, double fAmps, uint_t uBufLen)
	Update state dynamics. More...
virtual int	updateHealth (double fVolts, double fTemp, uint_t uCtlrStatus)
	Update motor health state. More...

Static Public Member Functions
static int	toMotorId (const int nCtlrId, const int nMotorIndex)
	Map motor controller id and motor index to motor id. More...
static std::string	toKey (const int nCtlrId, const int nMotorIndex)
	Map motor controller id and motor index to powertrain name (key). More...
static std::string	toKey (const int nMotorId)
	Map motor id to powertrain name (key). More...

Public Attributes
std::string	m_strName
	powertrain unique name (key)
LaePowertrainAttr	m_attr
	semi-fixed attribute data
LaePowertrainState	m_state
	dynamic state data

Detailed Description

Powertrain data class.

This class encapsulates the fixed data and current state of one powertrain.

Definition at line 196 of file laePowertrain.h.

Member Function Documentation

int LaePowertrain::configure ( const LaeDescPowertrain &  desc )

virtual

Configure powertrain from product description.

Parameters

desc	Laelaps powertrain product description data.

Returns

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

Definition at line 296 of file laePowertrain.cxx.

References laelaps::LaeDescPowertrain::m_eJointType, laelaps::LaeDescPowertrain::m_fGearRatio, laelaps::LaeDescPowertrain::m_nDir, laelaps::LaeDescPowertrain::m_nMotorCtlrId, laelaps::LaeDescPowertrain::m_nMotorId, laelaps::LaeDescPowertrain::m_nMotorIndex, laelaps::LaeDescPowertrain::m_strKey, and M_TAU.

Referenced by laelaps::LaeKinematics::configure().

{
    // from description
    m_strName               = desc.m_strKey;
    m_attr.m_nMotorId       = desc.m_nMotorId;
    m_attr.m_nMotorCtlrId   = desc.m_nMotorCtlrId;
    m_attr.m_nMotorIndex    = desc.m_nMotorIndex;
    m_attr.m_eJointType     = desc.m_eJointType;
    m_attr.m_nMotorDir      = desc.m_nDir;
    m_attr.m_fGearRatio     = desc.m_fGearRatio;

    // from motor specs and derivations
    m_attr.m_uPulsesPerRev  = LaeQuadPulsesPerRev;
    m_attr.m_fMaxRps        = LaeMotorRatedMaxRpm / 60.0;
    m_attr.m_uMaxQpps       = (uint_t)(
                                      m_attr.m_fMaxRps *
                                      m_attr.m_fGearRatio *
                                      (double)m_attr.m_uPulsesPerRev);

    m_attr.m_fMotorRadsPerPulse = M_TAU / (double)m_attr.m_uPulsesPerRev;
    m_attr.m_fWheelRadsPerPulse = m_attr.m_fMotorRadsPerPulse /
                                          m_attr.m_fGearRatio;

    m_attr.m_fMaxAmps     = LaeMotorRatedAmps;
    m_attr.m_fStallTorque = LaeMotorStallTorque;

    return LAE_OK;
}

int LaePowertrain::configure ( const LaeTunes &  tunes )

virtual

Configure powertrain from tune parameters.

Parameters

tunes

Laelaps tuning parameters.

Returns

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

Definition at line 325 of file laePowertrain.cxx.

References laelaps::LaeTunes::getTireDimParams().

{
  double  fTuneTireRadius, fTuneTireWidth;      // tire dimensions

  tunes.getTireDimParams(m_strName, fTuneTireRadius, fTuneTireWidth);

  m_attr.m_fTireRadius = fTuneTireRadius;
  m_attr.m_fTireWidth  = fTuneTireWidth;

  m_attr.m_fMetersPerPulse = m_attr.m_fWheelRadsPerPulse * m_attr.m_fTireRadius;
  m_attr.m_fMetersPerRadian = m_attr.m_fTireRadius;

  return LAE_OK;
}

int laelaps::LaePowertrain::getMotorCtlrId ( ) const

inline

Get motor controller id.

Returns

Returns controller id.

Definition at line 283 of file laePowertrain.h.

References laelaps::LaePowertrainAttr::m_nMotorCtlrId.

    {
      return m_attr.m_nMotorCtlrId;
    }

int laelaps::LaePowertrain::getMotorId ( ) const

inline

Get motor id.

Returns

Returns motor id.

Definition at line 273 of file laePowertrain.h.

References laelaps::LaePowertrainAttr::m_nMotorId.

    {
      return m_attr.m_nMotorId;
    }

int laelaps::LaePowertrain::getMotorIndex ( ) const

inline

Get motor controller motor index.

Returns

Returns controller motor index.

Definition at line 293 of file laePowertrain.h.

References laelaps::LaePowertrainAttr::m_nMotorIndex.

    {
      return m_attr.m_nMotorIndex;
    }

std::string laelaps::LaePowertrain::getName ( ) const

inline

Get this powertrain unique name (key).

Returns

String.

Definition at line 263 of file laePowertrain.h.

Referenced by laelaps::LaeKinActionDutyCycle::execute(), and laelaps::LaeKinActionVelocity::plan().

    {
      return m_strName;
    }

LaePowertrain LaePowertrain::operator=

(

const LaePowertrain &

rhs

)

Assignment operator.

Parameters

rhs	Right hand side object.

Returns

Returns copy of this.

Definition at line 231 of file laePowertrain.cxx.

References m_attr, m_state, and m_strName.

{
  m_strName = rhs.m_strName;
  m_attr    = rhs.m_attr;
  m_state   = rhs.m_state;

  return *this;
}

int LaePowertrain::reload ( const LaeTunes &  tunes )

virtual

Reload tuning parameters and re-configure.

Parameters

tunes

Laelaps tuning parameters.

Returns

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

Definition at line 340 of file laePowertrain.cxx.

{
  return configure(tunes);
}

int LaePowertrain::resetOdometer ( )

virtual

Reset odometry to zero.

All related positioning data are reset to 0.

Returns

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

Definition at line 345 of file laePowertrain.cxx.

Referenced by laelaps::LaeKinematics::resetOdometers().

{
  m_state.m_nEncoder  = 0;
  m_state.m_fPosition = 0.0;

  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_fPosition = m_state.m_fPosition;
}

string LaePowertrain::toKey ( const int  nCtlrId, const int  nMotorIndex  )

static

Map motor controller id and motor index to powertrain name (key).

Parameters

nCtrlId	Motor controller id.
nMotorIndex	Motor controller motor index

Returns

Returns key on success, empty string if unknown.

Definition at line 273 of file laePowertrain.cxx.

{
  static const char *unknown = "";

  int nMotorId = LaePowertrain::toMotorId(nCtlrId, nMotorIndex);

  return nMotorId != LaeMotorIdNone? LaeDesc::KeyPowertrain[nMotorId]: unknown;
}

string LaePowertrain::toKey ( const int  nMotorId )

static

Map motor id to powertrain name (key).

Parameters

nMotorId

Motor id.

Returns

Returns key on success, empty string if unknown.

Definition at line 282 of file laePowertrain.cxx.

{
  static const char *unknown = "";

  if( (nMotorId >= 0) && (nMotorId < LaeMotorsNumOf) )
  {
    LaeDesc::KeyPowertrain[nMotorId];
  }
  else
  {
    return unknown;
  }
}

int LaePowertrain::toMotorId ( const int  nCtlrId, const int  nMotorIndex  )

static

Map motor controller id and motor index to motor id.

Parameters

nCtrlId	Motor controller id.
nMotorIndex	Motor controller motor index

Returns

Returns unique motor id.

Definition at line 240 of file laePowertrain.cxx.

{
  switch( nCtlrId )
  {
    case LaeMotorCtlrIdFront:
      switch( nMotorIndex )
      {
        case LaeMotorLeft:
          return LaeMotorIdLF;
        case LaeMotorRight:
          return LaeMotorIdRF;
        default:
          break;
      }
      break;
    case LaeMotorCtlrIdRear:
      switch( nMotorIndex )
      {
        case LaeMotorLeft:
          return LaeMotorIdLR;
        case LaeMotorRight:
          return LaeMotorIdRR;
        default:
          break;
      }
      break;
    default:
      break;
  }

  return LaeMotorIdNone;
}

int LaePowertrain::updateHealth ( double  fVolts, double  fTemp, uint_t  uCtlrStatus  )

virtual

Update motor health state.

Parameters

fVolts	Motor input voltage (V).
fTemp	Motor controller board voltage (V).
uCtlrStatus	Motor controller status bits.

Returns

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

Definition at line 407 of file laePowertrain.cxx.

References motor::roboclaw::ParamStatusErrLogicBattHigh, motor::roboclaw::ParamStatusErrLogicBattLow, motor::roboclaw::ParamStatusErrMainBattHigh, motor::roboclaw::ParamStatusErrMot1Fault, motor::roboclaw::ParamStatusErrMot2Fault, motor::roboclaw::ParamStatusErrTemp, motor::roboclaw::ParamStatusEStopped, motor::roboclaw::ParamStatusNormal, motor::roboclaw::ParamStatusWarnMainBattHigh, motor::roboclaw::ParamStatusWarnMainBattLow, motor::roboclaw::ParamStatusWarnMot1OverCur, motor::roboclaw::ParamStatusWarnMot2OverCur, and motor::roboclaw::ParamStatusWarnTemp.

Referenced by laelaps::LaeKinematics::monitorHealth().

{
  m_state.m_fVolts  = fVolts;
  m_state.m_fTemp   = fTemp;

  m_state.m_uAlarms   = LaeMotorAlarmNone;
  m_state.m_uWarnings = LaeMotorWarnNone;

  //
  // Errors and/or warnings exist.
  //
  if( uCtlrStatus != ParamStatusNormal )
  {
    // emergency stopped
    if( uCtlrStatus & ParamStatusEStopped )
    {
      m_state.m_uAlarms |= LaeMotorAlarmEStop;
    }

    // temperature alarms
    if( uCtlrStatus & ParamStatusErrTemp )
    {
      m_state.m_uAlarms |= LaeMotorAlarmTemp;
    }

    // temperature warnings
    if( uCtlrStatus & ParamStatusWarnTemp )
    {
      m_state.m_uWarnings |= LaeMotorWarnTemp;
    }

    // voltage alarms
    if( uCtlrStatus & ( ParamStatusErrMainBattHigh |
                        ParamStatusErrLogicBattHigh |
                        ParamStatusErrLogicBattLow ) )
    {
      m_state.m_uAlarms |= LaeMotorAlarmVoltage;
    }

    // voltage warnings
    if( uCtlrStatus & ( ParamStatusWarnMainBattHigh |
                        ParamStatusWarnMainBattLow ) )
    {
      m_state.m_uWarnings |= LaeMotorWarnVoltage;
    }

    if( m_attr.m_nMotorIndex == Motor1 )
    {
      // motor 1 drive fault alarm
      if( uCtlrStatus & ParamStatusErrMot1Fault )
      {
        m_state.m_uAlarms |= LaeMotorAlarmFault;
      }

      // motor 1 over current warning
      if( uCtlrStatus & ParamStatusWarnMot1OverCur )
      {
        m_state.m_uWarnings |= LaeMotorWarnCurrent;
      }
    }
    else if(  m_attr.m_nMotorIndex == Motor2 )
    {
      // motor 2 drive fault alarm
      if( uCtlrStatus & ParamStatusErrMot2Fault )
      {
        m_state.m_uAlarms |= LaeMotorAlarmFault;
      }

      // motor 2 over current warning
      if( uCtlrStatus & ParamStatusWarnMot2OverCur )
      {
        m_state.m_uWarnings |= LaeMotorWarnCurrent;
      }
    }
  }

  // TODO torque calculation
  m_state.m_fTorque = 0.0;

  m_state.m_fPe = m_state.m_fAmps * m_state.m_fVolts;
  m_state.m_fPm = m_state.m_fPe * 1.0;  // TODO need efficiency curve
}

int LaePowertrain::updateStateDynamics ( s64_t  nEncoder, s32_t  nSpeed, double  fAmps, uint_t  uBufLen  )

virtual

Update state dynamics.

Parameters

nEncoder	Current encoder position (quad pulses).
nSpeed	Current motor speed (qpps).
fAmps	Current motor draw (amperes).
uBufLen	Current command queue length.

Returns

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

Definition at line 353 of file laePowertrain.cxx.

References laelaps::fcap(), and motor::roboclaw::ParamAmpMax.

Referenced by laelaps::LaeKinematics::senseDynamics().

{
  m_state.m_nEncoder  = nEncoder;
  m_state.m_nSpeed    = nSpeed;

  if( fAmps <= motor::roboclaw::ParamAmpMax )
  {
    m_state.m_fAmps = fAmps;
  }

  // command buffer length (commands)
  m_state.m_uBufLen   = uBufLen;

  // angular position and velocity
  m_state.m_fPosition = (double)m_state.m_nEncoder *
                                        m_attr.m_fWheelRadsPerPulse;
  m_state.m_fVelocity = (double)m_state.m_nSpeed  *
                                        m_attr.m_fWheelRadsPerPulse;

  // power
  m_state.m_fPe = m_state.m_fAmps * m_state.m_fVolts;
  m_state.m_fPm = m_state.m_fPe * 1.0;  // TODO need efficiency curve

  // torque
  if( fabs(m_state.m_fVelocity) >= 0.0001 )
  {
    m_state.m_fTorque = m_state.m_fPm / m_state.m_fVelocity;
  }
  else if( m_state.m_fAmps >= (m_attr.m_fMaxAmps * 0.9) )
  {
    m_state.m_fTorque = m_attr.m_fStallTorque *
                              m_state.m_fAmps /
                              m_attr.m_fMaxAmps;
  }
  else
  {
    m_state.m_fTorque = 0.0;
  }

  m_state.m_fTorque = fcap(m_state.m_fTorque, -m_attr.m_fStallTorque,
                                              m_attr.m_fStallTorque);

  // update real-time database
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_nEncoder  = m_state.m_nEncoder;
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_nSpeed    = m_state.m_nSpeed;
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_fPosition = m_state.m_fPosition;
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_fVelocity = m_state.m_fVelocity;
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_fPe       = m_state.m_fPe;
  RtDb.m_kin.m_powertrain[m_attr.m_nMotorId].m_fTorque   = m_state.m_fTorque;
}

---

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

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