linkage.cpp

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

  @return A pointer to an error object, or NULL On success
  */
/***************************************************************************/
const Error *Linkage::IncTrjUseCount( void )
{
   if( linkTrjPtr == 0 )
      return &LinkError::NoActiveTrj;

   if( trjUseCount++ == 0 )
      return linkTrjPtr->StartNew();
   else
      return 0;
}

/***************************************************************************/
/**
  Decrement a local counter indicating the number of axes currently using the
  linkage trajectory.
  @return A pointer to an error object, or NULL On success
  */
/***************************************************************************/
const Error *Linkage::DecTrjUseCount( void )
{
   if( linkTrjPtr == 0 )
      return &LinkError::NoActiveTrj;

   if( --trjUseCount == 0 )
      linkTrjPtr->Finish();

   return 0;
}
#endif

/***************************************************************************/
/**
  Update the status event map used by this linkage.  
  */
/***************************************************************************/

#define ERROR_EVENTS       (LINKEVENT_NODEGUARD | LINKEVENT_FAULT | LINKEVENT_ERROR | \
			    LINKEVENT_QUICKSTOP | LINKEVENT_ABORT | LINKEVENT_DISABLED )

void Linkage::UpdateStatus( void )
{
   uint32 orMask = 0;
   uint32 andMask = 0xffffffff;

   uint32 errors = ERROR_EVENTS;

   if( cfg.haltOnPosWarn ) 
      errors |= LINKEVENT_POSWARN;

   if( cfg.haltOnVelWin )
      errors |= LINKEVENT_VELWIN;

   for( int i=0; i<ampct; i++ )
   {
      AMP_EVENT e;

      amp[i]->GetEventMask( e );

      orMask |= (uint32)e;
      andMask &= (uint32)e;

      // If an error condition is being reported, latch it
      if( e & errors )
      {
	 // Check for any real errors
	 const Error *err = amp[i]->GetErrorStatus( true );

	 // If none, it's probably a tracking warning or velocity window
	 if( !err )
	 {
	    if( e & LINKEVENT_POSWARN )
	       err = &AmpError::TrackWarn;
	    else if( e & LINKEVENT_VELWIN )
	       err = &AmpError::VelWin;
	    else
	       err = &AmpError::Unknown;
	 }

	 LatchError( err, i );
	 cml.Warn( "Link %d error latched for amp %d: %s\n", amp[0]->GetNodeID(), i, err->toString() );
      }
   }
   orMask &= ( ERROR_EVENTS | LINKEVENT_POSWARN | 
               LINKEVENT_POSWIN | LINKEVENT_VELWIN | 
               LINKEVENT_POSLIM | LINKEVENT_NEGLIM | 
               LINKEVENT_SOFTLIM_POS | LINKEVENT_SOFTLIM_NEG );

   andMask &= ( LINKEVENT_MOVEDONE | LINKEVENT_TRJDONE );

   orMask |= andMask;

   cml.Debug( "Link %d status: 0x%08x\n", amp[0]->GetNodeID(), orMask );
   eventMap.setMask( orMask );
}

/***************************************************************************/
/**
  Linkage thread.  This thread monitors the linkage during run time.
  */
/***************************************************************************/
void Linkage::run( void )
{
   const Error *err;
   EventAny  allDone = LINKEVENT_MOVEDONE;
   EventAny  doneEvent;
   int errAmp;

   uint32 doneValue = LINKEVENT_MOVEDONE | LINKEVENT_NODEGUARD | LINKEVENT_FAULT |
		      LINKEVENT_ERROR | LINKEVENT_DISABLED | LINKEVENT_QUICKSTOP | 
		      LINKEVENT_ABORT;
   LINK_EVENT match;


   while( 1 )
   {
      // Wait for a move to start on this linkage.
      err = startSema.Get();

      // This should never fail, but if it does just delay and try again.
      if( err )
      {
	 sleep( 100 );
	 continue;
      }

      // Now, wait for the move to finish or an error to occur.
      cml.Debug( "Link thread waiting for move to finish.\n" );

      // Set or clear the position warning bit depending on whether
      // the linkage is configured to watch it.
      if( cfg.haltOnPosWarn ) 
	 doneValue |= LINKEVENT_POSWARN;
      else 
	 doneValue &= ~LINKEVENT_POSWARN;

      // Same thing for velocity window
      if( cfg.haltOnVelWin )
	 doneValue |= LINKEVENT_VELWIN;
      else
	 doneValue &= ~LINKEVENT_VELWIN;

      // Wait for any of my selected events
      doneEvent.setValue( doneValue );

      err = WaitEvent( doneEvent, -1, match );
      match = (LINK_EVENT)(match & doneValue);

      if( err )
	 cml.Debug( "Link %d error waiting on move done: %s\n",  amp[0]->GetNodeID(), err->toString() );

      else if( match == LINKEVENT_MOVEDONE )
      {
	 cml.Debug( "Link thread done OK\n" );
	 continue;
      }

      else if( (err = GetLatchedError( errAmp )) == 0 )
      {
	 cml.Debug( "Link %d stopped move with unexpected event: 0x%08x\n", 
	            amp[0]->GetNodeID(), match );
      }

      else
	 cml.Debug( "Link %d error from amp %d while waiting on move.\n  %s\n",  
	            amp[0]->GetNodeID(), errAmp, err->toString() );

      // On error, halt the linkage
      HaltMove();
      allDone.Wait( eventMap, -1 );
   }
}

/***************************************************************************/
/**
  When the status of an amplifier connected to this linkage is updated, this
  function is called.  It simply causes the linkage status to be updated also.
  @return false
  */
/***************************************************************************/
bool Linkage::StateEvent::isTrue( uint32 mask )
{
   link->UpdateStatus();
   return false;
}

#ifdef CML_LINKAGE_TRJ_BUFFER_SIZE
/***************************************************************************/
/**
  Initialize the amplifier trajectory structure.  This is called from the 
  Linkage object constructor.
  @param lptr Points to the linkage that owns this object.
  */
/***************************************************************************/
void Linkage::AmpTrj::Init( Linkage *lptr )
{
   linkPtr = lptr;
   head = tail = 0;
   inUse = false;
}

/***************************************************************************/
/**

*/
/***************************************************************************/
const Error *Linkage::AmpTrj::StartNew( void )
{
   if( inUse ) 
      return &LinkError::AmpTrjInUse;

   inUse = true;
   return linkPtr->IncTrjUseCount();
}

/***************************************************************************/
/**

*/
/***************************************************************************/
void Linkage::AmpTrj::Finish( void )
{
   inUse = false;
   head = tail = 0;
   linkPtr->DecTrjUseCount();
}

/***************************************************************************/
/**

*/
/***************************************************************************/
const Error *Linkage::AmpTrj::AddPoint( uunit pos, uunit vel, uint8 time, bool useVel )
{
   int newHead = (head+1) % CML_LINKAGE_TRJ_BUFFER_SIZE;

   if( newHead == tail )
      return &LinkError::AmpTrjOverflow;

   p[head] = pos;
   v[head] = vel;
   t[head] = time;
   u[head] = useVel;

   head = newHead;

   return 0;
}

/***************************************************************************/
/**
  */
/***************************************************************************/
bool Linkage::AmpTrj::UseVelocityInfo( void )
{
   if( head == tail )
      linkPtr->RequestNextTrjPoint();

   return u[tail];
}

/***************************************************************************/
/**
  Get the next segment for the PVT move.
  */
/***************************************************************************/
const Error *Linkage::AmpTrj::NextSegment( uunit &pos, uunit &vel, uint8 &time )
{
   if( !inUse ) return &LinkError::AmpTrjNotRunning;

   const Error *err;

   if( head == tail )
   {
      err = linkPtr->RequestNextTrjPoint();
      if( err ) return err;
   }

   pos  = p[tail];
   vel  = v[tail];
   time = t[tail];

   tail = (tail+1) % CML_LINKAGE_TRJ_BUFFER_SIZE;

   return 0;
}
#endif

/***************************************************************************/
/**
  Latch an error if one isn't already being held.
  @param err Points to the error
  @param ndx Index of the amp that caused it, or -1 if not known.
  @return The new latched error.
  */
/***************************************************************************/
const Error *Linkage::LatchError( const Error *err, int ndx )
{
   if( !latchedErr )
   {
      latchedErr = err;
      latchedErrAmp = ndx;
   }
   return latchedErr;
}

/***************************************************************************/
/**
  Initialize the receive PDO used to control words to each amplifier
  held by a linkage.  The COB ID used for this PDO is the standard
  ID used for RPDO 1 of the first axis.

  @return An error object pointer on failure, NULL on success
  */
/***************************************************************************/
const Error *RPDO_LinkCtrl::Init( void )
{
   int32 cobID = link[0].GetRpdoCobID( (uint16)1 );

   const Error *err = RPDO::Init( cobID );

   if( !err ) err = ctrl.Init( OBJID_CONTROL );
   if( !err ) err = AddVar( ctrl );
   if( !err ) err = SetType( 255 );
   if( err ) return err;

   for( int i=0; i<link.GetAmpCount(); i++ )
   {
      err = link[i].PdoSet( 1, *this );
      if( err ) return err;
   }

   return 0;
}

/***************************************************************************/
/**
  Transmit a control word using this PDO.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *RPDO_LinkCtrl::Transmit( uint16 c )
{
   ctrl.Write( c );
   return RPDO::Transmit( link[0].GetCanOpen() );
}

/***************************************************************************/
/**
  Default constructor.  All settings are set to their default values at
  construction time.
  */
/***************************************************************************/
LinkSettings::LinkSettings()
{
   moveAckTimeout = 200;
   haltOnPosWarn = false;
   haltOnVelWin = false;
}