linkage.cpp

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

/************************************************************/
/*                                                          */
/*  Copley Motion Libraries                                 */
/*                                                          */
/*  Author: Stephen Glow                                    */
/*                                                          */
/*  Copyright (c) 2002-2005 Copley Controls Corp.           */
/*                          http://www.copleycontrols.com   */
/*                                                          */
/************************************************************/

/** \file
  Implementation of the Linkage class.
  */

#include "CML.h"

// Note, This disables an annoying VC++ warning
#ifdef _WIN32
#pragma warning( disable: 4355 )
#endif


CML_NAMESPACE_USE();

// Linkage errors
CML_NEW_ERROR( LinkError, BadAmpCount,      "An illegal number of amplifiers was passed to Linkage::Init" );
CML_NEW_ERROR( LinkError, NetworkMismatch,  "The amplifiers passed to Linkage::Init don't share a CanOpen network" );
CML_NEW_ERROR( LinkError, AlreadyInit,      "The linkage is already initialized" );
CML_NEW_ERROR( LinkError, AmpAlreadyLinked, "The passed amplifier object is already assigned to a linkage" );
CML_NEW_ERROR( LinkError, AxisCount,        "The point dimension doesn't match the number of linkage axes" );
CML_NEW_ERROR( LinkError, AmpTrjOverflow,   "Amplifier trajectory structure overflow." );
CML_NEW_ERROR( LinkError, AmpTrjInUse,      "Amplifier trajectory already in use" );
CML_NEW_ERROR( LinkError, AmpTrjNotRunning, "Amplifier trajectory not presently in use" );
CML_NEW_ERROR( LinkError, NoActiveTrj,      "No linkage trajectory is active" );
CML_NEW_ERROR( LinkError, BadMoveLimit,     "A zero or negative move limit was detected" );
CML_NEW_ERROR( LinkError, UnknownAmpErr,    "An amplifier error occurred" );
CML_NEW_ERROR( LinkError, StartMoveTO,      "Timeout waiting on amplifier to respond to start move command" );
CML_NEW_ERROR( LinkError, NotSupported,     "Support for this function was not enabled in the library" );

/***************************************************************************/
/**
  Default constructor.  Linkage::Init must be called before this linkage 
  object may be used.
  */
/***************************************************************************/
Linkage::Linkage( void ): ctrlPDO( *this )
{
   trjUseCount = 0;
   ampct = 0;
   maxVel = maxAcc = maxDec = maxJrk = 0;

   for( int i=0; i<CML_MAX_AMPS_PER_LINK; i++ )
   {
      amp[i] = 0;
#ifdef CML_LINKAGE_TRJ_BUFFER_SIZE
      ampTrj[i].Init( this );
#endif
   }

   ClearLatchedError();
}

/***************************************************************************/
/**
  Linkage object destructor.
  */
/***************************************************************************/
Linkage::~Linkage()
{
   for( int i=0; i<ampct; i++ )
   {
      if( amp[i] ) amp[i]->SetLinkage(0);
   }
}

/***************************************************************************/
/**
  Mark the specified Amp as invalid.  This is called by an Amp object during
  destruction if the amp is still attached to a linkage (which is an error).
  @param a Pointer to the amp
  */
/***************************************************************************/
void Linkage::InvalidateAmp( Amp *a )
{
   for( int i=0; i<ampct; i++ )
   {
      if( amp[i] == a ) amp[i] = 0;
   }
   return;
}

/***************************************************************************/
/**
  Configure a linkage.  The linkage object will be configured to use the 
  various settings passed in the LinkSettings object.

  When a new Linkage object is created, it will be configured using a 
  default set of settings.  These settings can be modified through this
  call.  Set the documentation of the LinkSettings object for details of 
  the available settings and their default values.

  @param settings The new settings to be used.  A local copy of this object
  will be made by the linkage.
  */
/***************************************************************************/
const Error *Linkage::Configure( LinkSettings &settings )
{
   cfg = settings;
   return 0;
}

/***************************************************************************/
/**
  Initialize a new linkage object.  If the object has already been initialized,
  this will fail with an error.

  All amplifiers attached to a linkage must be initialized, and must share the
  same CanOpen network object.  Also, amplifiers may only be attached to one
  linkage at a time, so this function will fail if any of the passed amplifier
  objects is already attached to a Linkage.

  The linkage object will maintain pointers to each of the amplifier objects
  passed to this function.  The amplifiers may not be destroyed until after 
  the linkage object is.  

  @param ct The number of amplifiers to be used with this linkage.
  Note that this must be between 1 and CML_MAX_AMPS_PER_LINK.

  @param a An array of amplifiers to be assigned to this linkage.  There
  must be at least ct amplifiers in this array.
  */
/***************************************************************************/
const Error *Linkage::Init( uint16 ct, Amp a[] )
{
   if( ct < 1 || ct > CML_MAX_AMPS_PER_LINK )
      return &LinkError::BadAmpCount;

   Amp *aptr[ CML_MAX_AMPS_PER_LINK ];

   for( int i=0; i<ct; i++ )
      aptr[i] = &a[i];

   return Init( ct, aptr );
}

/***************************************************************************/
/**
  Initialize a new linkage object.  If the object has already been initialized,
  this will fail with an error.

  All amplifiers attached to a linkage must be initialized, and must share the
  same CanOpen network object.  Also, amplifiers may only be attached to one
  linkage at a time, so this function will fail if any of the passed amplifier
  objects is already attached to a Linkage.

  The linkage object will maintain pointers to each of the amplifier objects
  passed to this function.  The amplifiers may not be destroyed until after 
  the linkage object is.  

  @param ct The number of amplifiers to be used with this linkage.
  Note that this must be between 1 and CML_MAX_AMPS_PER_LINK.

  @param a An array of pointer to amplifier objects to be assigned to this 
  linkage.  There must be at least ct pointers in this array.
  */
/***************************************************************************/
const Error *Linkage::Init( uint16 ct, Amp *a[] )
{
   cml.Debug( "Initializing linkage %d\n", a[0]->GetNodeID() );

   CML_ASSERT( ct > 0 );
   CML_ASSERT( ct <= CML_MAX_AMPS_PER_LINK );
   CML_ASSERT( CML_MAX_AMPS_PER_LINK <= 32 );
   CML_ASSERT( amp[0] == 0 );

   if( ct < 1 || ct > CML_MAX_AMPS_PER_LINK )
      return &LinkError::BadAmpCount;

   if( amp[0] )
      return &LinkError::AlreadyInit;

   ClearLatchedError();

   // Make sure all amps are initialized, and share
   // the same network.
   int i;
   CanOpen *co = &a[0]->GetCanOpen();

   for( i=0; i<ct; i++ )
   {
      const Error *err = 0;

      if( !a[i]->IsInitialized() ) 
	 err = &CanOpenError::NotInitialized;

      else if( &(a[i]->GetCanOpen()) != co )
	 err = &LinkError::NetworkMismatch;

      else if( a[i]->GetLinkage() != 0 )
	 err = &LinkError::AmpAlreadyLinked;

      if( err )
	 return LatchError( err, i );
   }

   // Assign all the amplifiers to this linkage
   ampct = ct;
   for( i=0; i<ct; i++ )
   {
      amp[i] = a[i];
      amp[i]->SetLinkage(this);
   }

   // Add my state events to each amplifier
   for( i=0; i<ct; i++ )
   {
      stateEvent[ i ].link = this;
      amp[i]->eventMap.Add( &stateEvent[i] );
   }

   // Start my thread
   start();

   // Initialize a PDO used to send control words to each amplifier
   return ctrlPDO.Init();
}

/***************************************************************************/
/**
  Get a reference to the amplifier object at the specified location in the
  linkage.  Note that if CML_DEBUG_ASSERT is defined, then the standard C 
  assert function will be used to check for an invalid index.

  @param i The index of the amplifier to access.
  @return A reference to the amplifier object.
  */
/***************************************************************************/
Amp &Linkage::GetAmp( uint16 i )
{
   CML_ASSERT( i < ampct );  
   CML_ASSERT( amp[i] != 0 );
   return *amp[i];
}

/***************************************************************************/
/**
  Get the current commanded position of the linkage.  Note that this function
  queries the position of each amplifier sequentially and therefore the returned
  position information will only be accurate if the linkage is at rest when the
  function is called.

  @param p A point that will be filled in with the current Linkage commanded 
  position.
  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::GetPositionCommand( PointN &p )
{
   int axes = GetAxesCount();
   CML_ASSERT( axes <= CML_MAX_AMPS_PER_LINK );

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

   uunit pos[CML_MAX_AMPS_PER_LINK];

   int i;
   const Error *err;

   for( i=0; i<ampct; i++ )
   {
      err = amp[i]->GetPositionCommand( pos[i] );
      if( err )
      {
	 LatchError( err, i );
	 return err;
      }
   }

   err = ConvertAmpToAxisPos( pos );
   if( err )
   {
      LatchError( err, -1 );
      return err;
   }

   for( i=0; i<axes; i++ )
      p[i] = pos[i];

   return 0;
}

/***************************************************************************/
/**
  Set limits used for multi-axis point-to-point moves.

  @param vel Maximum velocity
  @param acc Maximum acceleration
  @param dec Maximum deceleration
  @param jrk Maximum jerk
  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::SetMoveLimits( uunit vel, uunit acc, uunit dec, uunit jrk )
{
   if( vel <= 0 || acc <= 0 || dec <= 0 || jrk <= 0 )
      return 0;

   maxVel = vel;
   maxAcc = acc;
   maxDec = dec;
   maxJrk = jrk;

   return 0;
}

/***************************************************************************/
/**
  Return the move limits currently set for this linkage.
  @param vel Returns maximum velocity
  @param acc Returns maximum acceleration
  @param dec Returns maximum deceleration
  @param jrk Returns maximum jerk
  @return An error object pointer, or NULL on success.
  */
/***************************************************************************/
const Error *Linkage::GetMoveLimits( uunit &vel, uunit &acc, uunit &dec, uunit &jrk )
{
   vel = maxVel;
   acc = maxAcc;
   dec = maxDec;
   jrk = maxJrk;
   return 0;
}

/***************************************************************************/
/**
  Move to a specified position.  This move uses the limits previously set using
  Linkage::SetMoveLimits.

  @param p The point to move to.
  @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, bool start )
{
   return MoveTo( p, maxVel, maxAcc, maxDec, maxJrk, start );
}

/***************************************************************************/
/**
  Move to a point in space.  The number of dimensions of the point must equal
  the number of axes controlled by the Linkage (as returned by Linkage::GetAxesCount).

  This method causes the linkage to perform a straight line move in N space 
  from the present position to the specified point.  The move will be limited
  in velocity, acceleration & jerk to the passed values.

  The linkage is assumed to be at rest when this method is called.  If this isn't