amp.cpp

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

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

/** \file

  This file provides most of the implementation for the Copley Amplifier object.

  Since the Amp object is large and complex, it's member functions have been 
  split into several files:

  - This file: Contains the core code.

  - AmpParam.cpp: Holds functions used to upload and download various
  blocks of amplifier parameters.

  - AmpPDO.cpp: Contains functions used to implement the various PDO objects
  used in conjunction with the Amp object.

*/

#include "CML.h"

CML_NAMESPACE_USE();

// Amplifier specific error objects
CML_NEW_ERROR( AmpError, NodeState,     "Drive state invalid for operation" );
CML_NEW_ERROR( AmpError, pvtSegPos,     "PVT segment position out of range" );
CML_NEW_ERROR( AmpError, pvtSegVel,     "PVT segment velocity out of range" );
CML_NEW_ERROR( AmpError, pvtBufferFull, "PVT trajectory buffer full" );
CML_NEW_ERROR( AmpError, badDeviceID,   "Device does not seem to be a Copley amplifier" );
CML_NEW_ERROR( AmpError, badHomeParam,  "Bad parameter passed to home function" );
CML_NEW_ERROR( AmpError, badMoveParam,  "Bad parameter passed to move function" );
CML_NEW_ERROR( AmpError, InMotion,      "The amplifier is currently executing a move" );
CML_NEW_ERROR( AmpError, GuardError,    "The amplifier did not respond to a node guarding or heartbeat message in time" );
CML_NEW_ERROR( AmpError, Fault,         "The amplifier detected a latching fault" );
CML_NEW_ERROR( AmpError, ShortCircuit,  "The amplifier detected a short circuit condition" );
CML_NEW_ERROR( AmpError, AmpTemp,       "The amplifier detected an over temperature error" );
CML_NEW_ERROR( AmpError, MotorTemp,     "The amplifier detected a motor temperature error" );
CML_NEW_ERROR( AmpError, OverVolt,      "The amplifier detected an over voltage condition" );
CML_NEW_ERROR( AmpError, UnderVolt,     "The amplifier detected an under voltage condition" );
CML_NEW_ERROR( AmpError, EncoderPower,  "The amplifier detected an encoder power error" );
CML_NEW_ERROR( AmpError, PhaseErr,      "The amplifier detected a phasing error" );
CML_NEW_ERROR( AmpError, TrackErr,      "The amplifier detected a tracking error." );
CML_NEW_ERROR( AmpError, PosLim,        "Positive limit switch is active" );
CML_NEW_ERROR( AmpError, NegLim,        "Negative limit switch is active" );
CML_NEW_ERROR( AmpError, PosSoftLim,    "Positive software limit is active" );
CML_NEW_ERROR( AmpError, NegSoftLim,    "Negative software limit is active" );
CML_NEW_ERROR( AmpError, TrackWarn,     "Position tracking warning" );
CML_NEW_ERROR( AmpError, Unknown,       "An unknown amplifier error occurred" );
CML_NEW_ERROR( AmpError, Reset,         "An amplifier reset was detected" );
CML_NEW_ERROR( AmpError, Disabled,      "The amplifier is currently disabled" );
CML_NEW_ERROR( AmpError, QuickStopMode, "The amplifier is currently in quick stop mode" );
CML_NEW_ERROR( AmpError, NoUserUnits,   "User units are not enabled in CML_Settings.h" );
CML_NEW_ERROR( AmpError, Abort,         "Trajectory aborted" );
CML_NEW_ERROR( AmpError, pvtPosUnavail, "The PVT segment position is not available." );
CML_NEW_ERROR( AmpError, VelWin,        "Velocity tracking window exceeded." );

CML_NEW_ERROR( AmpFault, Memory,        "Fatal hardware error: Amplifier flash data is corrupt." );
CML_NEW_ERROR( AmpFault, ADC,           "Fatal hardware error: An A/D offset error has occurred." );
CML_NEW_ERROR( AmpFault, ShortCircuit,  "The amplifier latched a short circuit condition" );
CML_NEW_ERROR( AmpFault, AmpTemp,       "The amplifier latched an over temperature error" );
CML_NEW_ERROR( AmpFault, MotorTemp,     "The amplifier latched a motor temperature error" );
CML_NEW_ERROR( AmpFault, OverVolt,      "The amplifier latched an over voltage condition" );
CML_NEW_ERROR( AmpFault, UnderVolt,     "The amplifier latched an under voltage condition" );
CML_NEW_ERROR( AmpFault, EncoderPower,  "The amplifier latched an encoder power error" );
CML_NEW_ERROR( AmpFault, PhaseErr,      "The amplifier latched a phasing error" );
CML_NEW_ERROR( AmpFault, TrackErr,      "The amplifier latched a tracking error." );
CML_NEW_ERROR( AmpFault, I2TLimit,      "Current limited by i^2t algorithm." );
CML_NEW_ERROR( AmpFault, Unknown, 	    "Some unknown amplifier latched fault has occurred" );

// local functions
static bool isCanMode( AMP_MODE mode );

/***************************************************************************/
/**
  Construct and initialize an amplifier object.
  @param co Reference to the CANopen network for this amp.
  @param nodeID a valid node ID for the amp
  @param settings Amplifier settings to be used.
  */
/***************************************************************************/
Amp::Amp( CanOpen &co, int16 nodeID, AmpSettings &settings )
{
   myLink = 0;
   Init( co, nodeID, settings );
}

/***************************************************************************/
/**
  Construct and initialize an amplifier object using defaults for 
  all amp settings.
  @param co Reference to the CANopen network for this amp.
  @param nodeID a valid node ID for the amp
  */
/***************************************************************************/
Amp::Amp( CanOpen &co, int16 nodeID )
{
   myLink = 0;
   Init( co, nodeID );
}

/***************************************************************************/
/**
  Amp object destructor.
  */
/***************************************************************************/
Amp::~Amp()
{
   if( myLink )
   {
      cml.Error( "Amp: %d destroyed while owned by a linkage!\n", GetNodeID() );
      myLink->InvalidateAmp( this );
   }
   else
      cml.Debug( "Amp: %d destroyed\n", GetNodeID() );
}

/***************************************************************************/
/**
  Initialize the amplifier object using all default settings.
  @param co Reference to the CANopen network for this amp.
  @param nodeID a valid node ID for the amp
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::Init( CanOpen &co, int16 nodeID )
{
   AmpSettings settings;
   return Init( co, nodeID, settings );
}

/***************************************************************************/
/**
  Initialize the amplifier object with custom amp settings.
  @param co Reference to the CANopen network for this amp.
  @param nodeID a valid node ID for the amp
  @param settings Amplifier settings to be used.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::Init( CanOpen &co, int16 nodeID, AmpSettings &settings )
{
   int i=0;

   cml.Debug( "Amp %d Init\n", nodeID );

   // Initialize the base class
   const Error *err = Node::Init( co, nodeID );
   if( err ) goto retErr;

   if( settings.resetOnInit )
      ResetNode();

   // Make sure this is the right type of node.
   NodeIdentity id;
   cml.Debug( "Amp %d, checking ID\n", nodeID );
   err = GetIdentity( id );
   if( err ) goto retErr;

   if( id.vendorID != 0x000000AB )
   {
      err = &AmpError::badDeviceID;
      goto retErr;
   }

   // Init some local variables
   initialSettings = settings;
   pvtTrj = 0;
   sdo.blkUpldOK = true;

   // If using programmable units, default to units
   // of encoder counts.
#ifdef CML_ENABLE_USER_UNITS
   err = SetCountsPerUnit( 1.0 );
   if( err ) goto retErr;
#endif

   // Read and save the amplifiers software version number 
   err = sdo.Upld16( OBJID_AMP_INFO, 24, SwVersionNum );

   // Disable all the default PDOs 
   cml.Debug( "Amp %d, Initting PDOs\n", nodeID );
   for( i=0; i<8; i++ )
   {
      if( !err ) err = TpdoDisable( i );
      if( !err ) err = RpdoDisable( i );
   }

   // I always start out with the following stopping modes:
   // quick stop - use the quick stop ramp
   // halt - use the profile deceleration
   cml.Debug( "Amp %d, setting default stop modes\n", nodeID );
   if( !err ) err = SetQuickStop( QSTOP_QUICKSTOP );
   if( !err ) err = SetHaltMode( HALT_DECEL );

   // Read the initial mode information from the amplifier
   uint16 desiredState;
   uint8 canOpMode;
   if( !err ) err = sdo.Upld16( OBJID_AMP_MODE, 0, desiredState );
   if( !err ) err = sdo.Upld8( OBJID_OP_MODE, 0, canOpMode );
   if( !err ) err = sdo.Upld16( OBJID_CONTROL, 0, lastCtrlWord );
   if( !err ) err = GetHomeMethod( lastHomeMethod );
   if( err ) goto retErr;

   lastMode = (AMP_MODE)( (desiredState<<8) | canOpMode );
   cml.Debug( "Amp %d, Initial mode is 0x%04x\n", nodeID, lastMode );

   if( !desiredState || !isCanMode( lastMode ) )
      lastMode = (AMP_MODE)( lastMode & 0xFF00 );

   // The default control method is based on the 
   // type of amplifier.
   err = sdo.Upld16( OBJID_AMP_INFO, 13, hwType );
   if( err ) goto retErr;

   if( (hwType & 0xFFC0) == 0x0240 )
      canCtrlMethod = AMPMODE_CAN_USTEP;
   else
      canCtrlMethod = AMPMODE_CAN_SERVO;

   // Initialize my status PDO
   if( !err ) err = statPdo.Init( *this, 0 );

   // Init my 'PVT buffer status' PDO
   if( !err ) err = buffStatPdo.Init( *this, 1 );

   // Initialize my PVT segment PDO
   if( !err ) err = pvtPdo.Init( *this, 0 );
   if( err ) goto retErr;

   // If the amplifier should be disabled on startup, 
   // clear the control word.  This ensures that the
   // amp won't be enabled when I start it.
   enabled = settings.enableOnInit;
   if( !enabled )
      err = Disable( false );

   // Set the initial requested mode
   if( !err ) err = SetAmpMode( settings.initialMode );

   // Amplifiers using the '8367 processor may require a short
   // delay when switching operating modes.
   if( (hwType & 0xFF00) >= 0x0300 )
      stateChangeDelay = 10;

   // Start the node
   cml.Debug( "Amp %d, starting node\n", nodeID );
   if( !err ) err = StartNode();
   if( err ) goto retErr;

   // Request a status PDO update and wait for it to be
   // received before continuing.
   // Note that 8367 based products don't support remote 
   // requests well, so we use an SDO to request the PDO
   cml.Debug( "Amp %d, (type 0x%04x) Getting initial status\n", nodeID, hwType );
   if( (hwType & 0xFF00) < 0x0300 )
      err = statPdo.Request();
   else
      err = sdo.Dnld8( OBJID_PDOREQUEST, 0, (uint8)0 );

   if( !err )
   {
      EventNone e(AMPEVENT_NOT_INIT);
      err = e.Wait( eventMap, sdo.GetTimeout() );
   }

   // Find the value that the amp expects for the next PVT segment
   // ID.  Normally this will be zero (after amp reset).
   if( !err ) err = GetPvtSegID( pvtSegID );

   // Setup the synch message
   cml.Debug( "Amp %d, Setting up synch\n", nodeID );
   if( !err ) err = SetSynchPeriod( settings.synchPeriod );
   if( !err ) err = SetSynchId( settings.synchID );

   // See if we are picking our own synch producer
   // If so, pick the first initialized amplifier.
   if( !err && settings.synchUseFirstAmp )
   {
      settings.synchProducer = (co.GetSynchProducer() == 0);
   }

   if( err ) goto retErr;

   // Now, start the synch if we are the producer.
   if( settings.synchProducer )
   {
      cml.Debug( "Amp %d, Starting synch production\n", nodeID );
      err = SynchStart();
   }
   else
   {
      cml.Debug( "Amp %d, Stopping synch production\n", nodeID );
      err = SynchStop();
   }

   if( err ) goto retErr;

   // Setup heartbeat or node guarding
   cml.Debug( "Amp %d, setting up node guarding\n", nodeID );
   if( settings.heartbeatPeriod )
      err = StartHeartbeat( settings.heartbeatPeriod, settings.heartbeatTimeout );

   else if( settings.guardTime && settings.lifeFactor )
      err = StartNodeGuard( settings.guardTime, settings.lifeFactor );

   else
      err = StopGuarding();

   if( err ) goto retErr;

   // Setup the PDO used to synchronize amplifiers over the network
   cml.Debug( "Amp %d, setting up time synch PDO\n", nodeID );
   err = SetupSynchPDO( settings );
   if( err ) goto retErr;

   // Clear the latched version of the amplifier's 
   // event status register.  We use this register
   // to check for unexpected amplifier resets.
   cml.Debug( "Amp %d, clearing event latch\n", nodeID );
   err = ClearEventLatch( (EVENT_STATUS)0xFFFFFFFF );
   if( err ) goto retErr;

   // Clear any latched fault conditions
   cml.Debug( "Amp %d, clearing faults\n", nodeID );
   err = ClearFaults();
   if( err ) goto retErr;

   // Now, try to enable the amplifier if requested
   if( settings.enableOnInit )
   {
      AMP_EVENT evnt;

      cml.Debug( "Amp %d, Enabling amp\n", nodeID );

      // Wait for any non-latching errors to clear.
      // This can occure on the Xenus as it's high
      // voltage comes up.  I'll wait 400ms which 
      // is about twice what this should take max.
      EventNone e(AMPEVENT_ERROR);
      e.Wait( eventMap, 400 );

      // Now, try to enable the amplifier.
      err = Enable();

      // On failure, try to return a useful error
      // message rather then just a timeout.
      if( err )
      {
	 if( err == &ThreadError::Timeout )
	    err = GetErrorStatus( false );
	 goto retErr;
      }

      // Make sure the amplifier is really enabled.
      // The fact that the Enable() function returned success
      // means that the amplifier is software enabled (i.e. 
      // allowed to enable if possible), but it could still be
      // disabled by other factors.
      err = GetEventMask( evnt );
      if( err ) goto retErr;

      if( evnt & AMPEVENT_DISABLED )
      {
	 // Check the lower level 'event status' register
	 // returned with the most recent status PDO.
	 // This should give me more info on what's holding