laelaps::LaeKinActionTwist Class Reference

Laelaps kinematics velocity action class. More...

#include <laeKin.h>

Inheritance diagram for laelaps::LaeKinActionTwist:

Public Member Functions
	LaeKinActionTwist (LaeKinematics &kin)
	Default initialization constructor. More...
virtual	~LaeKinActionTwist ()
	Destructor.
int	update (double fVelLinear, double fVelAngular)
	Update with new velocities. More...
virtual int	plan ()
	Plan next execution. More...
virtual int	execute ()
	Execution new velocities. More...
virtual int	terminate ()
	Terminate action. More...
Public Member Functions inherited from laelaps::LaeKinAction
	LaeKinAction (LaeKinematics &kin, ActionType eActionType=ActionTypeIdle)
	Default intialization constructor. More...
virtual	~LaeKinAction ()
	Destructor.
virtual bool	isIdle ()
	Test if action is idle. More...
virtual bool	isPlanningRequired ()
	Test if action requires (re)planning. More...
virtual bool	isExecutionRequired () const
	Test if more action requires (more) execution. More...
virtual bool	hasTerminated () const
	Test if action has been terminated. More...
virtual ActionType	getActionType () const
	Get the action type for this class. More...
virtual ActionState	getActionState () const
	Get the action state. More...

Protected Member Functions
void	clear ()
	Clear goal.

Protected Attributes
double	m_fGoalVelLinear
	goal platform linear velocity
double	m_fGoalVelAngular
	goal platform angular velocity
s32_t	m_speed [LaeNumMotorCtlrs][LaeNumMotorsPerCtlr]
	target motor speeds (qpps)
Protected Attributes inherited from laelaps::LaeKinAction
LaeKinematics &	m_kin
	bound kinematics driver
ActionType	m_eActionType
	action type enum
ActionState	m_eActionState
	action state.

Additional Inherited Members
Public Types inherited from laelaps::LaeKinAction
enum	ActionType {   ActionTypeIdle,   ActionTypeVelocity,   ActionTypeDutyCycle,   ActionTypeTwist,   ActionTypeNavForDist,   ActionTypeNavToPos }
	Supported kinematic extended actions. More...
enum	ActionState {   ActionStateIdle,   ActionStateUpdate,   ActionStatePlan,   ActionStateExecute,   ActionStateTerminated }
	Action states. More...
Static Public Member Functions inherited from laelaps::LaeKinAction
static LaeKinAction *	newAction (LaeKinematics &kin, const ActionType eActionType)
	Archetype constructor to create action type instance. More...
static LaeKinAction *	replaceAction (LaeKinematics &kin, LaeKinAction *pAction, const ActionType eNewActionType)
	Archetype constructor to replace action type instance. More...

Detailed Description

Laelaps kinematics velocity action class.

A velocity action only sets the velocities of a (sub)set of the powertrain velocities. Powertrains not specified are kept at their current velocities.

Definition at line 1108 of file laeKin.h.

Constructor & Destructor Documentation

LaeKinActionTwist::LaeKinActionTwist

(

LaeKinematics &

kin

)

Default initialization constructor.

Parameters

kin	Bound kinemeatics driver.

Definition at line 2023 of file laeKin.cxx.

References clear().

    : LaeKinAction(kin, ActionTypeTwist)
{
  clear();
}

Member Function Documentation

int LaeKinActionTwist::execute ( )

virtual

Execution new velocities.

All motors with new velocities are written with new speeds.

Returns

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

Reimplemented from laelaps::LaeKinAction.

Definition at line 2159 of file laeKin.cxx.

References laelaps::LaeKinAction::ActionStateIdle, motor::roboclaw::RoboClaw::cmdQDriveWithAccel(), laelaps::LaeKinAction::execute(), laelaps::LaeKinematics::getMotorCtlr(), laelaps::LaeKinAction::isExecutionRequired(), laelaps::LAE_ECODE_MOT_CTLR, laelaps::LAE_OK, laelaps::LaeMotorLeft, laelaps::LaeMotorRight, laelaps::LaeNumMotorCtlrs, laelaps::LaeKinAction::m_eActionState, laelaps::LaeKinAction::m_kin, and m_speed.

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

{
  int       nCtlr;
  RoboClaw *pMotorCtlr;
  int       rc = LAE_OK;

  // no execution required
  if( !isExecutionRequired() )
  {
    return LAE_OK;
  }

  LaeKinAction::execute();

  // set motor speeds
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    pMotorCtlr = m_kin.getMotorCtlr(nCtlr);

    //rc = pMotorCtlr->cmdQDrive2(m_speed[nCtlr][LaeMotorLeft],
    //                            m_speed[nCtlr][LaeMotorRight]);

    rc = pMotorCtlr->cmdQDriveWithAccel(
        m_speed[nCtlr][LaeMotorLeft],  MoveAccel,
        m_speed[nCtlr][LaeMotorRight], MoveAccel);

    if( rc != OK )
    {
      rc = -LAE_ECODE_MOT_CTLR;
      break;
    }
  }

  m_eActionState = ActionStateIdle;

  return rc;
}

int LaeKinActionTwist::plan ( )

virtual

Plan next execution.

If new velocities, the motor quad pulse per second are calculated.

Returns

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

Reimplemented from laelaps::LaeKinAction.

Definition at line 2053 of file laeKin.cxx.

References laelaps::LaeKinAction::ActionStateExecute, laelaps::LaeKinAction::ActionStateIdle, laelaps::LaeKinematics::getPlatform(), laelaps::LaeKinematics::getPowertrain(), laelaps::LaeKinAction::isPlanningRequired(), laelaps::LAE_OK, laelaps::LaeMotorLeft, laelaps::LaeNumMotorCtlrs, laelaps::LaeNumMotorsPerCtlr, laelaps::LaePowertrain::m_attr, laelaps::LaeKinAction::m_eActionState, m_fGoalVelAngular, m_fGoalVelLinear, laelaps::LaePowertrainAttr::m_fMetersPerPulse, laelaps::LaePlatform::m_fWheeltrack, laelaps::LaeKinAction::m_kin, m_speed, laelaps::LaePowertrainAttr::m_uMaxQpps, and laelaps::LaeKinAction::plan().

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

{
  int                       nCtlr;      // motor controller id
  int                       nMotor;     // motor index
  LaePowertrain            *pTrain;     // working powertrain
  int                       nSpeed;     // motor speed (qpps)

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

  LaeKinAction::plan();

  double halftrack = m_kin.getPlatform().m_fWheeltrack / 2.0; // radius
  double velAngLin = m_fGoalVelAngular * halftrack;   // anglur to linear
  double ppm;                     // quad pulses/meter
  double ppsLin;                  // quad pulses/second for linear component
  double ppsAng;                  // quad  pulses/second for angular component
  double ppsAbsMax = 0.0;         // absolute max qqps
  double ppsRatedMinMax = 10e10;  // lowest max rated powertrain qpps
  double pps[LaeNumMotorCtlrs][LaeNumMotorsPerCtlr];  // goal qpps

  //
  // Iterate through motors and convert twist to speeds.
  //
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
    {
      if( (pTrain = m_kin.getPowertrain(nCtlr, nMotor)) == NULL )
      {
        continue;
      }

      ppm = 1.0 / pTrain->m_attr.m_fMetersPerPulse;

      ppsLin  = m_fGoalVelLinear * ppm;
      ppsAng  = velAngLin * ppm;

      if( nMotor == LaeMotorLeft )
      {
        pps[nCtlr][nMotor] = ppsLin - ppsAng;
      }
      else
      {
        pps[nCtlr][nMotor] = ppsLin + ppsAng;
      }

      // powetrain with the highest absolute speed
      if( fabs(pps[nCtlr][nMotor]) > ppsAbsMax )
      {
        ppsAbsMax = fabs(pps[nCtlr][nMotor]);
      }

      // powertrain with the lowest maximum speed
      if( (double)pTrain->m_attr.m_uMaxQpps < ppsRatedMinMax )
      {
        ppsRatedMinMax = (double)pTrain->m_attr.m_uMaxQpps;
      }
    }
  }

  //
  // Derate speed to work within the specs of the lowest motor capabilites.
  //
  if( ppsAbsMax > ppsRatedMinMax )
  {
    double derate = ppsRatedMinMax / ppsAbsMax;

    for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
    {
      for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
      {
        pps[nCtlr][nMotor] *= derate;
      }
    }
  }

  //
  // Now set the target motor speeds.
  //
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    for(nMotor = 0; nMotor < LaeNumMotorsPerCtlr; ++nMotor)
    {
      nSpeed = (int)pps[nCtlr][nMotor];

      //
      // New speed
      //
      if( m_speed[nCtlr][nMotor] != nSpeed )
      {
        m_speed[nCtlr][nMotor] = nSpeed;
        m_eActionState = ActionStateExecute;
      }
    }
  }

  // planned, but no execution required
  if( m_eActionState != ActionStateExecute )
  {
    m_eActionState = ActionStateIdle;
  }
}

int LaeKinActionTwist::terminate ( )

virtual

Terminate action.

All motors are stopped.

Returns

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

Reimplemented from laelaps::LaeKinAction.

Definition at line 2197 of file laeKin.cxx.

References clear(), motor::roboclaw::RoboClaw::cmdStopWithDecel(), laelaps::LaeKinematics::getMotorCtlr(), laelaps::LaeKinAction::hasTerminated(), laelaps::LAE_OK, laelaps::LaeNumMotorCtlrs, laelaps::LaeKinAction::m_kin, and laelaps::LaeKinAction::terminate().

{
  LaeMapVelocity::iterator  iter;
  int                       nCtlr;
  RoboClaw                 *pMotorCtlr;

  // already terminated
  if( hasTerminated() )
  {
    return LAE_OK;
  }

  //
  // Stop all motors.
  //
  // Note: Do not call m_kin.stop() here, since it calls this function.
  //
  for(nCtlr = 0; nCtlr < LaeNumMotorCtlrs; ++nCtlr)
  {
    pMotorCtlr = m_kin.getMotorCtlr(nCtlr);
    pMotorCtlr->cmdStopWithDecel(StopDecel);
  }

  clear();

  LaeKinAction::terminate();

  return LAE_OK;
}

int LaeKinActionTwist::update	(	double	fVelLinear,
		double	fVelAngular
	)

Update with new velocities.

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

References laelaps::LaeKinAction::ActionStateIdle, laelaps::LaeKinAction::ActionStatePlan, laelaps::LaeKinAction::ActionStateUpdate, laelaps::LAE_OK, laelaps::LaeKinAction::m_eActionState, m_fGoalVelAngular, m_fGoalVelLinear, and laelaps::radToDeg().

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

{
  // set state to update
  m_eActionState = ActionStateUpdate;

  if( (fVelLinear != m_fGoalVelLinear) || (fVelAngular != m_fGoalVelAngular) )
  {
    m_fGoalVelLinear  = fVelLinear; 
    m_fGoalVelAngular = fVelAngular;
    m_eActionState    = ActionStatePlan;

    LOGDIAG3("Robot linear velocity = %lf m/s, angular velocity = %lf deg/s.",
        m_fGoalVelLinear, radToDeg(m_fGoalVelAngular));
  }

  // updated, but no planning and execution required
  else
  {
    m_eActionState = ActionStateIdle;
  }

  return LAE_OK;
}

---

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

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