amp.cpp

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

	 // me up.
	 EVENT_STATUS estat = (EVENT_STATUS)statPdo.estat.Read();

	 // The only good reason for the amplifier to be disabled
	 // at this point is if an input pin is configured to disable
	 // it.  If that's not the case, I'll give the amplifier a
	 // little time to enable on it's own.  This is helpful for the
	 // Xenus amplifier which has a built in delay after closing
	 // it's high voltage relay.
	 if( !(estat & ESTAT_DISABLE_INPUT) )
	 {
	    EventNone e(AMPEVENT_DISABLED);
	    e.Wait( eventMap, 200 );
	 }
      }
   }

retErr:
   if( err )
      cml.Warn( "Amp %d failed init %s\n", nodeID, err->toString() );
   else
      cml.Debug( "Amp %d, init done\n", nodeID );

   return err;
}

/***************************************************************************/
/**
  Re-initialize an amplifier.  This function simply calls Amp::Init using 
  the same parameters that were initially passed.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::ReInit( void )
{
   cml.Debug( "Amp %d ReInit\n", GetNodeID() );
   if( !co ) return &CanOpenError::NotInitialized;
   return Init( *co, GetNodeID(), initialSettings );
}

/***************************************************************************/
/**
  Reset the amplifier object.  This function should be used for Amp objects 
  instead of Node::ResetNode().  It resets the amplifier and re-initializes 
  the amplifier object.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::Reset( void )
{
   cml.Debug( "Amp %d Reset\n", GetNodeID() );
   bool oldResetOnInit = initialSettings.resetOnInit;
   initialSettings.resetOnInit = true;

   const Error *err = ReInit();

   initialSettings.resetOnInit = oldResetOnInit;
   return err;
}

/***************************************************************************/
/**
  Execute a home move.  The various homing parameters are passed in the
  HomeConfig structure.

  This function simply programs all the homing parameters passed in the 
  structure, then calls Amp::GoHome().

  @param cfg The homing configuration parameter structure.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::GoHome( HomeConfig &cfg )
{
   /**************************************************
    * If a move is necessary, make sure that the accel
    * and velocity parameters are non-zero.
    **************************************************/
   const Error *err = 0;

   switch( cfg.method )
   {
      case CHM_HARDSTOP_POS:
      case CHM_HARDSTOP_NEG:
      case CHM_HARDSTOP_ONDX_POS:
      case CHM_HARDSTOP_ONDX_NEG:
	 err = SetHomeCurrent( cfg.current );
	 if( !err ) err = SetHomeDelay( cfg.delay );
	 // fall through

      default:
	 // If no homing move is defined, don't try to set
	 // the velocity & accel parameters.  This avoids
	 // possible errors.
	 if( (cfg.method == CHM_NONE) && (cfg.offset == 0) )
	    break;

	 // Set the home parameters
	 if( !err ) err = SetHomeVelFast( cfg.velFast );
	 if( !err ) err = SetHomeVelSlow( cfg.velSlow );
	 if( !err ) err = SetHomeAccel( cfg.accel );
   }

   if( !err ) err = SetHomeOffset( cfg.offset );
   if( !err ) err = SetHomeMethod( cfg.method, cfg.extended );

   // Start the home.
   if( !err ) err = GoHome();
   return err;
}

/***************************************************************************/
/**
  Execute a home move.  The various homing parameters (method, velocity, etc)
  are assumed to have already be configured.

  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::GoHome( void )
{
   cml.Debug( "Amp %d Home\n", GetNodeID() );

   const Error *err;

   // Check for error conditions
   err = CheckStateForMove();
   if( err ) goto retErr;

   // Make sure we are in homing mode
   err = SetAmpMode( AMPMODE_CAN_HOMING );
   if( err ) goto retErr;

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

   // Start homing
   err = SetControlWord( 0x001F );
   if( err ) goto retErr;

   // Wait for the move to start if the
   // home method isn't NONE.
   if( lastHomeMethod != CHM_NONE )
   {
      EventNone e = AMPEVENT_MOVEDONE;
      err = e.Wait( eventMap, sdo.GetTimeout() );
   }

   // Otherwise, wait for the 'trajectory aborted' bit to be
   // clear if it was set.
   else
   {
      EventNone e = AMPEVENT_ABORT;
      err = e.Wait( eventMap, sdo.GetTimeout() );
   }

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

   return err;
}

/***************************************************************************/
/**
  Setup a point to point move, but do not start it.  The move may be subsequently
  started using Amp::StartMove().

  The move will use the trapezoidal profile mode, and all parameters will be 
  programmed based on the values passed in the cfg structure.

  @param cfg A structure holding all the move configuration parameters.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::SetupMove( ProfileConfigTrap &cfg )
{
   // Setup the move
   const Error *err = SetProfileType( PROFILE_TRAP );

   uunit dec = (cfg.dec <= 0) ? cfg.acc : cfg.dec;

   if( !err ) err = SetProfileVel( cfg.vel ); 
   if( !err ) err = SetProfileAcc( cfg.acc ); 
   if( !err ) err = SetProfileDec( dec );
   if( !err ) err = SetTargetPos( cfg.pos );

   // Make sure we are in point to point move mode
   if( !err ) err = SetAmpMode( AMPMODE_CAN_PROFILE );

   return err;
}

/***************************************************************************/
/**
  Setup a point to point move, but do not start it.  The move may be subsequently
  started using Amp::StartMove().

  The move will use the S-curve profile mode, and all parameters will be 
  programmed based on the values passed in the cfg structure.

  @param cfg A structure holding all the move configuration parameters.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::SetupMove( ProfileConfigScurve &cfg )
{
   const Error *err;

   // Make sure there is no move in progress
   if( !(eventMap.getMask() & AMPEVENT_MOVEDONE) )
      return &AmpError::InMotion;

   // Setup the move
   err = SetProfileType( PROFILE_SCURVE );

   if( !err ) err = SetProfileVel( cfg.vel ); 
   if( !err ) err = SetProfileAcc( cfg.acc ); 
   if( !err ) err = SetProfileJerk( cfg.jrk );
   if( !err ) err = SetTargetPos( cfg.pos );

   // Make sure we are in point to point move mode
   if( !err ) err = SetAmpMode( AMPMODE_CAN_PROFILE );

   return err;
}

/***************************************************************************/
/**
  Setup a point to point move, but do not start it.  The move may be subsequently
  started using Amp::StartMove().

  The move will use the velocity profile mode, and all parameters will be 
  programmed based on the values passed in the cfg structure.

  @param cfg A structure holding all the move configuration parameters.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::SetupMove( ProfileConfigVel &cfg )
{
   // Setup the move
   const Error *err = SetProfileType( PROFILE_VEL );

   if( !err ) err = SetProfileVel( cfg.vel ); 
   if( !err ) err = SetProfileAcc( cfg.acc ); 
   if( !err ) err = SetProfileDec( cfg.dec );
   if( !err ) err = SetTargetPos( cfg.dir );

   // Make sure we are in point to point move mode
   if( !err ) err = SetAmpMode( AMPMODE_CAN_PROFILE );

   return err;
}


/***************************************************************************/
/**
  Perform a point to point move.  The move will use the trapezoidal profile
  mode, and all parameters will be programmed before the move is started.

  This function can also be used to update a move that is alread in progress.

  @param cfg A structure holding all the move configuration parameters.
  @param relative This will be a relative move if true, absolute if false (default)
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::DoMove( ProfileConfigTrap &cfg, bool relative )
{
   // Setup the move
   const Error *err = SetupMove( cfg );

   // Start it
   if( !err ) err = StartMove( relative );

   return err;
}

/***************************************************************************/
/**
  Perform a point to point move.  The move will use the S-curve profile
  mode, and all parameters will be programmed before the move is started.
  @param cfg A structure holding all the move configuration parameters.
  @param relative This will be a relative move if true, absolute if false (default)
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::DoMove( ProfileConfigScurve &cfg, bool relative )
{
   // Setup the move
   const Error *err = SetupMove( cfg );

   // Start it
   if( !err ) err = StartMove( relative );

   return err;
}

/***************************************************************************/
/**
  Perform a velocity profile move.  All parameters will be programmed before 
  the move is started.

  This function can also be used to update a move that is alread in progress.

  @param cfg A structure holding all the move configuration parameters.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::DoMove( ProfileConfigVel &cfg )
{
   // Setup the move
   const Error *err = SetupMove( cfg );

   // Start it
   if( !err ) err = StartMove( false );

   return err;
}

/***************************************************************************/
/**
  Perform a point to point move.  It's assumed that the drive
  is already configured with the properly trajectory parameters (velocity, 
  acceleration, deceleration, profile type, etc).

  This function sets the trajectories target position to the passed value, 
  and manipulates the control word to start the move.  It can be used for 
  either absolute moves or relative moves

  @param pos Position to move to (absolute) or distance to move (relative).
  @param relative True if this is a relative move, false for absolute.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::DoMove( uunit pos, bool relative )
{
   const Error *err;

   // Make sure we are in point to point move mode
   err = SetAmpMode( AMPMODE_CAN_PROFILE );
   if( err ) return err;

   err = SetTargetPos( pos );
   if( err ) return err;

   return StartMove( relative );
}

/***************************************************************************/
/**
  Start the move that's already been programmed.  This function is primarily
  intended for internal use, and is called by DoMove and SendTrajectory.
  Note that the amplifier mode must have already been setup when this function
  is called.  The mode should be either AMPMODE_CAN_PROFILE, or AMPMODE_CAN_PVT.
  This function is not used to start a homing move.

  @param relative If true, start a relative move.  If false, start an absolute
         move.  Note that this is only used with point to point moves, interpolated
         moves should always set relative to false.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::StartMove( bool relative )
{
   cml.Debug( "Amp %d new move\n", GetNodeID() );
   const Error *err;

   // See if there are any errors that would stop us
   err = CheckStateForMove();