laelaps::LaeKinActionVelocity Class Reference

Laelaps kinematics velocity action class. More...

#include <laeKin.h>

Inheritance diagram for laelaps::LaeKinActionVelocity:

Public Member Functions
	LaeKinActionVelocity (LaeKinematics &kin)
	Default initialization constructor. More...
virtual	~LaeKinActionVelocity ()
	Destructor.
int	update (const LaeMapVelocity &velocity)
	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
LaeMapVelocity	m_mapGoalVel
	map of goal velocities
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 966 of file laeKin.h.

Constructor & Destructor Documentation

LaeKinActionVelocity::LaeKinActionVelocity

(

LaeKinematics &

kin

)

Default initialization constructor.

Parameters

kin	Bound kinemeatics driver.

Definition at line 1677 of file laeKin.cxx.

References clear().

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

Member Function Documentation

int LaeKinActionVelocity::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 1785 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::setGoalVelocities().

{
  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 LaeKinActionVelocity::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 1727 of file laeKin.cxx.

References laelaps::LaeKinAction::ActionStateExecute, laelaps::LaeKinAction::ActionStateIdle, laelaps::LaePowertrain::getName(), laelaps::LaeKinematics::getPowertrain(), laelaps::LaeKinAction::isPlanningRequired(), laelaps::LAE_OK, laelaps::LaeNumMotorCtlrs, laelaps::LaeNumMotorsPerCtlr, laelaps::LaePowertrain::m_attr, laelaps::LaeKinAction::m_eActionState, laelaps::LaePowertrainAttr::m_fWheelRadsPerPulse, laelaps::LaeKinAction::m_kin, m_mapGoalVel, laelaps::LaePowertrainState::m_nSpeed, m_speed, laelaps::LaePowertrain::m_state, and laelaps::LaeKinAction::plan().

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

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

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

  LaeKinAction::plan();

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

      goal = m_mapGoalVel.find(pTrain->getName());

      // converted goal velocity speed is target speed
      if( goal != m_mapGoalVel.end() )
      {
        nSpeed = goal->second / pTrain->m_attr.m_fWheelRadsPerPulse;
      }
      // current speed is target speed
      else
      {
        nSpeed = pTrain->m_state.m_nSpeed;
      }

      //
      // 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 LaeKinActionVelocity::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 1823 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->cmdQStop();
    pMotorCtlr->cmdStopWithDecel(StopDecel);
  }

  clear();

  LaeKinAction::terminate();

  return LAE_OK;
}

int LaeKinActionVelocity::update

(

const LaeMapVelocity &

velocity

)

Update with new velocities.

Parameters

velocity

Map of powertrain velocities.

Returns

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

Definition at line 1683 of file laeKin.cxx.

References laelaps::LaeKinAction::ActionStateIdle, laelaps::LaeKinAction::ActionStatePlan, laelaps::LaeKinAction::ActionStateUpdate, laelaps::LAE_OK, laelaps::LaeKinAction::m_eActionState, and m_mapGoalVel.

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

{
  LaeMapVelocity::const_iterator iter;
  LaeMapVelocity::iterator       pos;

  // set state to update
  m_eActionState = ActionStateUpdate;

  //
  // Iterate through key,velocity map.
  //
  for(iter = velocity.begin(); iter != velocity.end(); ++iter)
  {
    pos = m_mapGoalVel.find(iter->first);

    // velocity on a new action powertrain
    if( pos == m_mapGoalVel.end() )
    {
      m_mapGoalVel[iter->first] = iter->second;
      m_eActionState = ActionStatePlan;
    }

    // different velocity on an existing action powertrain
    else if( pos->second != iter->second )
    {
      pos->second = iter->second;
      m_eActionState = ActionStatePlan;
    }

    // else same velocity
    
    LOGDIAG3("Wheel %12s velocity = %lf rads/s.",
        iter->first.c_str(), iter->second);
  }

  // updated, but no planning and execution required
  if( m_eActionState == ActionStateUpdate )
  {
    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