linkage.cpp

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

  the case, then an error will result.

  Note that this function causes a trajectory to be calculated and passed to the 
  amplifiers as a series of PVT points.  This calculation requires floating point
  math, so this function is not available if floating point support has not been 
  enabled in CML_Settings.h.

  @param p The point in N space to move to.
  @param vel Maximum velocity
  @param acc Maximum acceleration
  @param dec Maximum deceleration
  @param jrk Maximum jerk
  @param start If true (the default), the profile will be started by this call.
               If false, the profile will be uploaded, but not started.  In that case
               the move may be later started by a call to Linkage::StartMove.

  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::MoveTo( PointN &p, uunit vel, uunit acc, uunit dec, uunit jrk, bool start )
{
#ifndef CML_ALLOW_FLOATING_POINT
   return &LinkError::NotSupported;
#else
   ClearLatchedError();

   if( p.getDim() != GetAxesCount() )
      return &LinkError::AxisCount;

   const Error *err;

   Point<CML_MAX_AMPS_PER_LINK> startPos;
   startPos.setDim( GetAxesCount() );

   err = GetPositionCommand( startPos );
   if( err ) return err;

   err = scurve.Calculate( startPos, p, vel, acc, dec, jrk );
   if( err ) return err;

   return SendTrajectory( scurve, start );
#endif
}

#ifdef CML_LINKAGE_TRJ_BUFFER_SIZE
/***************************************************************************/
/**
  Upload a multi-axis PVT move trajectory to the linkage and optionally start the move.

  @param trj Reference to the linkage trajectory to be used.  A local
  pointer to this trajectory will be stored if the entire profile will 
  not fit in the amplifiers on-board buffer.  This pointer will be kept
  until the entire profile has been uploaded to the linkage.  It is therefore
  important to ensure that the trajectory object passed here will remain
  valid (i.e. not be deallocated) until the linkage has called the LinkTrajectory.Finish()
  method.

  @param start If true (the default), the profile will be started by this call.
               If false, the profile will be uploaded, but not started.  In that case
               the move may be later started by a call to Linkage::StartMove.

  @return An error object.
  */
/***************************************************************************/
const Error *Linkage::SendTrajectory( LinkTrajectory &trj, bool start )
{
   ClearLatchedError();

   linkTrjPtr = &trj;
   for( int i=0; i<GetAxesCount(); i++ )
   {
      const Error *err = amp[i]->SendTrajectory( ampTrj[i], false );
      if( err )
	 return LatchError( err, i );
   }

   if( start )
      return StartMove();

   return 0;
}
#endif

/***************************************************************************/
/**
  Start the moves that have already been programmed into all
  axes of this linkage.

  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::StartMove( void )
{
   ClearLatchedError();

   uint32 allAmps = (1<<ampct) - 1;
   const Error *err = 0;
   EventAny events[CML_MAX_AMPS_PER_LINK];
   EventMap map;
   int i;

   // Make sure all amps are in profile mode
   // and are ready to start a new move.
   for( i=0; i<ampct; i++ )
   {
      err = amp[i]->CheckStateForMove();
      if( err )
	 return LatchError( err, i );

      events[i].setChain( map, (1<<i) );
      events[i].setValue( AMPEVENT_SPACK | AMPEVENT_PVT_EMPTY );
      amp[i]->eventMap.Add( &events[i] );
   }

   // Enable the amps, and wait for them to clear
   // the acknowledge bit.
   SetControlWord( 0x000F );
   EventNone none = allAmps;
   err = none.Wait( map, cfg.moveAckTimeout );

   // On timeout, find the offending thread
   if( err == &ThreadError::Timeout )
   {
      for( i=0; i<ampct; i++ )
      {
	 AMP_EVENT e;
	 amp[i]->GetEventMask( e );
	 if( e & AMPEVENT_SPACK )
	    return LatchError( &LinkError::StartMoveTO, i );
      }
   }
   else if( err )
      return LatchError( err, -1 );

   // Now, start a move on all amps, and wait for
   // the acknowledge bit to be set.
   SetControlWord( 0x003F );
   EventAll all = allAmps;
   err = all.Wait( map, cfg.moveAckTimeout );

   // On timeout, find the offending thread
   if( err == &ThreadError::Timeout )
   {
      for( i=0; i<ampct; i++ )
      {
	 AMP_EVENT e;
	 amp[i]->GetEventMask( e );
	 if( !(e & AMPEVENT_SPACK) )
	    return LatchError( &LinkError::StartMoveTO, i );
      }
   }
   else if( err )
      return LatchError( err, -1 );

   // Notify my thread that a move has started
   startSema.Put();

   // Reset the event chaining that I setup.  This ensures that the
   // events won't point to an event map that has been destroyed
   // already.
   for( i=0; i<ampct; i++ )
      events[i].delChain();

   return 0;
}

/***************************************************************************/
/**
  Halt the current move.  The exact type of halt can be programmed individually 
  for each axis using the Amp::SetHaltMode function.
  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::HaltMove( void )
{
   SetControlWord( 0x010F );
   return 0;
}

/***************************************************************************/
/**
  Set the linkage control word.

  @param value The control word value to set
  @return A pointer to an error object, or NULL on success
  */
/***************************************************************************/
const Error *Linkage::SetControlWord( uint16 value )
{
   cml.Debug( "Link %d control 0x%04x\n", amp[0]->GetNodeID(), value );

   const Error *err = ctrlPDO.Transmit( value );

   if( err ) return err;

   for( int i=0; i<ampct; i++ )
      amp[i]->lastCtrlWord = value;

   return 0;
}

/***************************************************************************/
/**
  Wait for a linkage event condition. This function can be used to wait
  on any generic event associated with the linkage.
  @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 *Linkage::WaitEvent( Event &e, int32 timeout, LINK_EVENT &match )
{
   const Error *err = e.Wait( eventMap, timeout );
   match = (LINK_EVENT)e.getMask();
   return err;
}

/***************************************************************************/
/**
  Wait for a linkage event condition. This function can be used to wait
  on any generic event associated with the linkage.
  @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 *Linkage::WaitEvent( Event &e, int32 timeout )
{
   LINK_EVENT match;
   return WaitEvent( e, timeout, match );
}

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

  @param timeout The maximum time to wait (milliseconds).  Default is -1 (forever).
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::WaitMoveDone( int32 timeout )
{
   cml.Debug( "Link %d waiting on move\n", amp[0]->GetNodeID() );

   uint32 value = LINKEVENT_MOVEDONE | LINKEVENT_NODEGUARD | LINKEVENT_FAULT |
		  LINKEVENT_ERROR | LINKEVENT_DISABLED | LINKEVENT_QUICKSTOP | 
	          LINKEVENT_ABORT;

   EventAny e( value );

   LINK_EVENT match;
   const Error *err = WaitEvent( e, timeout, match );

   if( !err )
   {
      match = (LINK_EVENT)(match & value);
      if( match == LINKEVENT_MOVEDONE )
	 return 0;

      // There should be a latched error
      int ndx;
      err = GetLatchedError( ndx );

      // If not, take a best guess
      if( !err ) 
	 err = GetError( match );

      cml.Debug( "Linkage::WaitMoveDone returned: %s\n", err->toString() );
   }

   return err;
}

/***************************************************************************/
/**
  Return an error code for a failed move.  The passed mask identifies
  which amplifier in the linkage generated the error.
  @param mask The event mask that caused the error
  @return A pointer to an error object
  */
/***************************************************************************/
const Error *Linkage::GetError( uint32 mask )
{
   const Error *err, *someErr=0;
   int i, someAmp=-1;


   // Try to find an amplifier with an event mask equal 
   // to the passed mask.
   for( i=0; i<ampct; i++ )
   {
      err = amp[i]->GetErrorStatus();
      if( !err ) continue;

      someErr = err;
      someAmp = i;

      AMP_EVENT ae;
      amp[i]->GetEventMask( ae );

      if( mask == (uint32)ae )
      {
	 LatchError( err, i );
	 return err;
      }
   }

   // If no exact match was found, just return an error 
   // reported by one of the amplifiers.
   if( someAmp >= 0 )
   {
      LatchError( someErr, someAmp );
      return someErr;
   }

   // If all else fails, return a generic error 
   LatchError( &LinkError::UnknownAmpErr, -1 );
   return &LinkError::UnknownAmpErr;
}

#ifdef CML_LINKAGE_TRJ_BUFFER_SIZE
/***************************************************************************/
/**
  Get the next PVT segment.  This function is called by an amplifier trajectory
  object when it requires a new trajectory point and doesn't have one cached.
  The linkage trajectory is queried for it's next point, and after this point
  is converted from axis space to amplifier space, the point is distributed to
  all amplifier trajectory objects.

  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::RequestNextTrjPoint( void )
{
   const Error *err;

   if( !linkTrjPtr )
      return &LinkError::NoActiveTrj;

   uunit pos[ CML_MAX_AMPS_PER_LINK ];
   uunit vel[ CML_MAX_AMPS_PER_LINK ];
   uint8 time;

   bool useVel = linkTrjPtr->UseVelocityInfo();
   err = linkTrjPtr->NextSegment( pos, vel, time );
   if( err ) return err;

   err = ConvertAxisToAmp( pos, vel );
   if( err ) return err;

   for( int i=0; i<ampct; i++ )
      ampTrj[i].AddPoint( pos[i], vel[i], time, useVel );

   return 0;
}

/***************************************************************************/
/**
  Increment a local counter indicating that the linkage trajectory is in use
  by one more axis.