amp.cpp

美国COPLEY驱动器,程序开发工具之一.

   if( err ) goto retErr;

   // Clear bit 4 of the control word
   if( lastCtrlWord != 0x000F )
      err = SetControlWord( 0x000F );

   // Wait for the amplifier to respond by clearing
   // the acknowledge bit in it's status register
   if( err ) 
      goto retErr;
   else
   {
      EventNone e = AMPEVENT_SPACK;
      err = e.Wait( eventMap, sdo.GetTimeout() );
      if( err ) goto retErr;
   }

   // Start the move
   if( relative )
      err = SetControlWord( 0x007F );
   else
      err = SetControlWord( 0x003F );

   // Now, wait for the acknowledgement
   if( !err )
   {
      EventAny e = AMPEVENT_SPACK;
      err = e.Wait( eventMap, sdo.GetTimeout() );
   }

retErr:
   if( err )
      cml.Warn( "Amp %d move failed: %s\n", GetNodeID(), err->toString() );

   return err;
}

/***************************************************************************/
/**
  Wait for the currently running move to finish, or for an error to occur.

  @param timeout The maximum time to wait (milliseconds)
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitMoveDone( int32 timeout )
{
   EventAny e = AMPEVENT_MOVEDONE | AMPEVENT_NODEGUARD | AMPEVENT_FAULT | 
      AMPEVENT_ERROR | AMPEVENT_DISABLED | AMPEVENT_QUICKSTOP |
      AMPEVENT_ABORT;

   // Wait for the event
   const Error *err = e.Wait( eventMap, timeout);

   // Return an error code based on the events that occurred.
   if( !err ) err = GetErrorStatus();

   return err;
}

/***************************************************************************/
/**
  Wait for an amplifier event condition. This function can be used to wait
  on any generic event associated with the amplifier.  
  @param e The event to wait on.
  @param timeout The timeout for the wait (milliseconds).  If < 0, then 
  wait forever.
  @param match Returns the matching event condition.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitEvent( Event &e, int32 timeout, AMP_EVENT &match )
{
   const Error *err = e.Wait( eventMap, timeout );
   match = (AMP_EVENT)e.getMask();
   return err;
}

/***************************************************************************/
/**
  Wait for an amplifier event condition. This function can be used to wait
  on any generic event associated with the amplifier.  
  @param e The event to wait on.
  @param timeout The timeout for the wait (milliseconds).  If < 0, then 
  wait forever (default).
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitEvent( Event &e, int32 timeout )
{
   AMP_EVENT match;
   return WaitEvent( e, timeout, match );
}


/***************************************************************************/
/**
  Wait on the amplifier's general purpose input pins.

  The amplifier object maintains an EventMap object which reflects the state
  of the amplifier's general purpose input pins.  Each bit of this EventMap
  corresponds to one input pin; bit 0 for input 0, bit 1 for input 1, etc.
  The bit in the event map is set when the corresponding input pin is high,
  and cleared when the input pin is low.

  This function provides a very flexible method for waiting on a particular
  state on the input pins.  Event objects may be created to define a specific
  state of one or more pins, and these objects may be used in conjunction with
  this function to wait for that state to occur.

  In addition to this function, two simpler functions are also provided.  These
  functions (WaitInputHigh and WaitInputLow) allow the user to wait on one or 
  more input pins to go high or low respectively.  Internally, these function 
  call WaitInputEvent for their implementation.

  @param e       An Event object describing the input pin state to wait on.
  @param timeout The timeout for the wait (milliseconds).  If < 0, then 
  wait forever.
  @param match   On success, the state of the input pins which caused the match 
  to occur will be returned here.

  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitInputEvent( Event &e, int32 timeout, uint32 &match )
{
   const Error *err = e.Wait( inputStateMap, timeout );
   match = e.getMask();
   return err;
}

/***************************************************************************/
/**
  Wait for any of the specified general purpose input pins to be set.  The
  inputs parameter specifies which input(s) to wait on using a bit mask.  Bit
  0 should be set for input 0, bit 1 for input 1, etc.  The function will 
  return when any of the specified input pins goes high.
  @param inputs Specifies which input pin(s) to wait on.
  @param timeout The timeout for the wait (milliseconds).  If < 0, then 
  wait forever.  If not specified, the timeout defaults to -1
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitInputHigh( uint32 inputs, int32 timeout )
{
   EventAny e = inputs;
   uint32 match;

   return WaitInputEvent( e, timeout, match );
}

/***************************************************************************/
/**
  Wait for any of the specified general purpose input pins to be lowered.  The
  inputs parameter specifies which input(s) to wait on using a bit mask.  Bit
  0 should be set for input 0, bit 1 for input 1, etc.  The function will 
  return when any of the specified input pins goes low.
  @param inputs Specifies which input pin(s) to wait on.
  @param timeout The timeout for the wait (milliseconds).  If < 0, then 
  wait forever.  If not specified, the timeout defaults to -1
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::WaitInputLow( uint32 inputs, int32 timeout )
{
   EventAnyClear e = inputs;
   uint32 match;

   return WaitInputEvent( e, timeout, match );
}

/***************************************************************************/
/**
  Get the current state of the amplifier's event mask.  The event mask is a 
  bit-mapped variable identifies many interesting elements of the amplifiers
  state.  The contents of this variable are built up from several different
  amplifier status words which are constantly updated over the CANopen network.

  When the event mask is read using this function, no new messages are passed
  over the network.  The current value of the event mask is simply returned.
  Any time the amplifier's state changes, it sends a message over the CANopen
  network which is used to update this mask.

  It is also possible to wait on a particular value for this mask.  See
  Amp::WaitEvent for details.

  @param e The amplifier's event mask is returned here
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::GetEventMask( AMP_EVENT &e )
{
   e = (AMP_EVENT)eventMap.getMask();
   return 0;
}

/***************************************************************************/
/**
  Return an error object identifying the amplifiers status.  This function 
  causes the amplifier object to examine it's event mask and return an 
  error object corresponding to the most serious error present.
  @param noComm If true, then no CAN message communications will be performed
  by this function.  This is useful if the function is being called from
  a CAN message handler which can't perform SDO communications.
  If false (default), then the amplifier may be queried for more detailed
  error information.
  @return A pointer to an error object, or NULL if no errors are present
  */
/***************************************************************************/
const Error *Amp::GetErrorStatus( bool noComm )
{
   uint32 events = eventMap.getMask();
   EVENT_STATUS stat = (EVENT_STATUS)statPdo.estat.Read();
   const Error *err = 0;

   if( events & AMPEVENT_NODEGUARD )
      err = &NodeError::GuardTimeout;

   else if( events & AMPEVENT_FAULT )
   {
      // Get detailed fault information if CAN communications are allowed
      if( !noComm )
      {
	 AMP_FAULT faults;
	 GetFaults( faults );
	 err = AmpFault::DecodeFault( faults );
      }

      // If no error was found, look for one in the event status
      if( !err )
	 err = AmpError::DecodeStatus( stat );

      // If all else fails, return a generic error
      if( !err )
	 err = &AmpError::Unknown;
   }

   else if( events & AMPEVENT_ERROR )
   {
      err = AmpError::DecodeStatus( stat );
      if( !err ) err = &AmpError::Unknown;
   }

   else if( events & AMPEVENT_QUICKSTOP )
      err = &AmpError::QuickStopMode;

   else if( events & AMPEVENT_ABORT )
      err = &AmpError::Abort;

   else if( events & AMPEVENT_DISABLED )
      err = &AmpError::Disabled;

   return err;
}

/***************************************************************************/
/**
  Handle an amplifier state change.  This method wakes up any task waiting on
  the move done semaphore in the event of a guard error.  If this feature is
  desired, this method should be called from any class that over rides this
  method.
  */
/***************************************************************************/
void Amp::HandleStateChange( NodeState from, NodeState to )
{
   // On a guard error, wake up any task that's pending
   // on my semaphore (i.e. waiting for move done, etc)
   if( to == NODESTATE_GUARDERR )
      eventMap.setBits( AMPEVENT_NODEGUARD );
}

/***************************************************************************/
/**
  This function attempts to clear a node guarding event condition.  Node guarding
  events occure when the amplifier fails to respond to it's heartbeat protocol
  for some reason.  This could be caused by a network wiring problem, slow 
  processing on the master controller (causing the amplifier guard message to be
  delayed or lost), or an amplifier error such as a reset or power down.

  In any case, once a node guarding error is identified, the error condition 
  must be cleared before any new moves may be performed.

  This function attempts to clear the node guarding event condition, however if
  it determines that the amplifier has been reset then it fails and returns the
  error object AmpError::Reset.  In this case, the amplifier object must be 
  reinitialized before it can be used.  The amp may be reinitialized by calling
  Amp::Init or Amp::ReInit.

  If node guarding error become a problem, it may mean that the guard time is 
  set too low.  This can be adjusted when the amplifier object is initialized
  by the values in the AmpSettings object.

  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::ClearNodeGuardEvent( void )
{
   // Check the latched event status to determine if the amp was reset
   EVENT_STATUS status;
   const Error *err = GetEventLatch( status );
   if( err ) return err;

   if( status & ESTAT_RESET )
      return &AmpError::Reset;

   eventMap.clrBits( AMPEVENT_NODEGUARD );
   return 0;
}

/***************************************************************************/
/**
  Check the amplifier's state to make sure a move can be started.  This 
  function is used internally by the functions that start moves & homing.
  It looks at the current state of the amplifier and returns an appropriate
  error code if something is wrong that would cause problems during a move/home.
  @return A pointer to an error object, or NULL for no error
  */
/***************************************************************************/
const Error *Amp::CheckStateForMove( void )
{
   // Make sure the node is operational
   if( GetState() != NODESTATE_OPERATIONAL )
      return &AmpError::NodeState;

   if( !enabled )
      return &AmpError::Disabled;

   uint32 mask = eventMap.getMask();

   // First, just look for obvious indications of an error.  If none
   // are found, then I'm good to go.
   if( !(mask & (AMPEVENT_NODEGUARD|AMPEVENT_FAULT|AMPEVENT_ERROR|
	       AMPEVENT_DISABLED|AMPEVENT_QUICKSTOP) ) )
      return 0;

   // Return a node guarding error if that was detected.
   if( mask & AMPEVENT_NODEGUARD ) return &AmpError::GuardError;

   // If the amplifier is in a fault state, return details
   if( mask & AMPEVENT_FAULT )
   {
      AMP_FAULT faults;
      GetFaults( faults );
      return AmpFault::DecodeFault( faults );
   }

   if( mask & AMPEVENT_QUICKSTOP ) return &AmpError::QuickStopMode;

   if( mask & AMPEVENT_DISABLED ) return &AmpError::Disabled;

   // Otherwise, return the error info.  If no errors are detected, 
   // then they may have been cleared recently.
   uint32 event = statPdo.estat.Read();

   event &= (ESTAT_SHORT_CRCT | ESTAT_AMP_TEMP | ESTAT_OVER_VOLT | 
	 ESTAT_UNDER_VOLT | ESTAT_MTR_TEMP | ESTAT_ENCODER_PWR | 
	 ESTAT_PHASE_ERR | ESTAT_TRK_ERR);

   return AmpError::DecodeStatus( (EVENT_STATUS)event );
}

/***************************************************************************/
/**
  Decode the passed event status word and return an appropriate error object.
  @param stat The amplifier event status register
  @return A pointer to an error object, or NULL if there is no error.
  */
/***************************************************************************/
const AmpError *AmpError::DecodeStatus( EVENT_STATUS stat )
{
   if( stat & ESTAT_SHORT_CRCT  ) return &AmpError::ShortCircuit;
   if( stat & ESTAT_AMP_TEMP    ) return &AmpError::AmpTemp;
   if( stat & ESTAT_MTR_TEMP    ) return &AmpError::MotorTemp;
   if( stat & ESTAT_ENCODER_PWR ) return &AmpError::EncoderPower;
   if( stat & ESTAT_PHASE_ERR   ) return &AmpError::PhaseErr;