amp.cpp

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

   if( stat & ESTAT_TRK_ERR     ) return &AmpError::TrackErr;
   if( stat & ESTAT_OVER_VOLT   ) return &AmpError::OverVolt;
   if( stat & ESTAT_UNDER_VOLT  ) return &AmpError::UnderVolt;
   if( stat & ESTAT_FAULT       ) return &AmpError::Fault;
   if( stat & ESTAT_POSLIM      ) return &AmpError::PosLim;
   if( stat & ESTAT_NEGLIM      ) return &AmpError::NegLim;   
   if( stat & ESTAT_SOFTLIM_POS ) return &AmpError::PosSoftLim;
   if( stat & ESTAT_SOFTLIM_NEG ) return &AmpError::NegSoftLim;
   if( stat & ESTAT_TRK_WARN    ) return &AmpError::TrackWarn;

   return 0;
}

/***************************************************************************/
/**
  Return an appropriate fault object based on the amplifier fault mask.
  @param fault The amplifier fault mask.
  @return A pointer to the fault object, NULL if there is no fault.
  */
/***************************************************************************/
const AmpFault *AmpFault::DecodeFault( AMP_FAULT fault )
{
   if( fault == 0 ) return 0;

   if( fault & FAULT_DATAFLASH   ) return &AmpFault::Memory;
   if( fault & FAULT_ADCOFFSET   ) return &AmpFault::ADC;
   if( fault & FAULT_SHORT_CRCT  ) return &AmpFault::ShortCircuit;
   if( fault & FAULT_AMP_TEMP    ) return &AmpFault::AmpTemp;
   if( fault & FAULT_MTR_TEMP    ) return &AmpFault::MotorTemp;
   if( fault & FAULT_OVER_VOLT   ) return &AmpFault::OverVolt;
   if( fault & FAULT_UNDER_VOLT  ) return &AmpFault::UnderVolt;
   if( fault & FAULT_ENCODER_PWR ) return &AmpFault::EncoderPower;
   if( fault & FAULT_PHASE_ERR   ) return &AmpFault::PhaseErr;
   if( fault & FAULT_TRK_ERR     ) return &AmpFault::TrackErr;
   if( fault & FAULT_I2T_ERR     ) return &AmpFault::I2TLimit;

   return &AmpFault::Unknown;
}

/***************************************************************************/
/**
  This is a simple local function that checks an AMP_MODE to determine if it's
  a true CAN control mode or not.
  */
/***************************************************************************/
static bool isCanMode( AMP_MODE mode )
{
   switch( mode & 0xFF00 )
   {
      case 0:
	 return (mode==0) ? false : true;

      case AMPMODE_CAN_SERVO:
      case AMPMODE_CAN_USTEP:
	 return true;

      default:
	 return false;
   }
}

/***************************************************************************/
/**
  Set the amplifier mode of operation.  The mode of operation determines the
  top level control loop that will be controled (position, velocity, or current),
  and the source of that control (CANopen network, digital input pins, etc).

  @param mode The mode of operation to be set
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::SetAmpMode( AMP_MODE mode )
{
   const Error *err = 0;

   // For CAN modes, if the control method (servo or microstep) isn't
   // specified, then fill it in.
   if( isCanMode(mode) && !(mode&0xFF00) )
      mode = (AMP_MODE)(mode | canCtrlMethod);

   // Don't bother updating the mode if it hasn't changed.
   if( lastMode == mode ) return 0;

   cml.Debug( "Amp %d new mode 0x%04x\n", GetNodeID(), mode );

   // If this is a CAN control mode, then I need to set both
   // the amplifier's operating mode & the CAN mode.
   // I don't have to worry about enable/disable issues though.
   if( isCanMode( mode ) )
   {
      // Only set the amplifier desired state if it's changed
      if( 0xFF00 & (lastMode^mode) )
      {
	 uint16 i = ((uint16)mode)>>8;
	 err = sdo.Dnld16( OBJID_AMP_MODE, 0, i );
      }

      if( !err ) err = sdo.Dnld8( OBJID_OP_MODE, 0, ByteCast(mode) );

      // Save the control method for next time
      if( !err ) canCtrlMethod = (AMP_MODE)(mode & 0xFF00);
   }

   // For other modes, if the amp is disabled then I need to set the mode
   // to zero (which does the disable).  The actual mode will be set when
   // the amp is enabled.
   else if( !enabled )
      err = sdo.Dnld16( OBJID_AMP_MODE, 0, (int16)0 );

   // If I'm enabled, then just set the mode and return.
   else
   {
      uint16 i = ((uint16)mode)>>8;
      err = sdo.Dnld16( OBJID_AMP_MODE, 0, i );
   }

   if( !err ) lastMode = mode;

   return err;
}

/***************************************************************************/
/**
  Get the currently active amplifier mode of operation.
  @param mode The active mode of operation is returned here
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::GetAmpMode( AMP_MODE &mode )
{
   mode = lastMode;
   return 0;
}

/***************************************************************************/
/**
  Perform a 'quick stop' on the axis.  The exact meaning of a quick stop 
  can be programmed using the Amp::SetQuickStop function.  
  Regardless of the type of quick stop being performed, the amplifier will
  always end up disabled at the end of the quick stop.  If disabling the 
  amplifier is not desirable, then the Amp::HaltMove function should be 
  used instead.

  Note that the quick stop function is only available when running in one
  of the standard CAN amplifier modes.  If doing low level velocity or current
  control, then moves must be stopped externally.
  */
/***************************************************************************/
const Error *Amp::QuickStop( void )
{
   const Error *err;

   err = SetControlWord( 0x0003 );
   if( err ) return err;

   // Now, wait for the amp to either become disabled, 
   // or indicate that it's doing a quick stop.  One
   // or the other should happen depending on the 
   // quick stop mode that's selected
   EventAny e = AMPEVENT_QUICKSTOP | AMPEVENT_DISABLED;
   return e.Wait( eventMap, sdo.GetTimeout() );
}

/***************************************************************************/
/**
  Halt the current move.  The exact type of halt can be programmed using 
  the Amp::SetHaltMode function.

  Note that the halt function is only available when running in one of the 
  standard CAN amplifier modes.  If doing low level velocity or current
  control, then moves must be stopped externally.
  */
/***************************************************************************/
const Error *Amp::HaltMove( void )
{
   return SetControlWord( lastCtrlWord | 0x0100 );
}

/***************************************************************************/
/**
  Disable the amplifier. 
  Note that if the brake delays are in use, then the amplifier may still be
  enabled when this function returns success.  The outputs will actually be
  disabled after the amplifier finishes the braking procedure.

  @param wait Wait for confirmation from the amplifier if true (default).
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::Disable( bool wait )
{
   const Error *err = 0;

   // If we're not in a CAN controlled mode, then we need
   // to set the mode to 0 to disable
   if( !isCanMode( lastMode ) )
      err = sdo.Dnld16( OBJID_AMP_MODE, 0, (int16)0 );

   // In normal CAN controlled modes, I handle this
   // through the use of the control register.
   // Note that I clear bit 1 to get out of quick stop mode
   else
      err = SetControlWord( 0x0005 );
   if( err ) return err;

   enabled = false;

   if( !wait ) return 0;

   // Now, wait for the amp to actually disable before returning
   // Note that when this returns the amplifier is trying to
   // disable, but if the motor's brake times are set, then this
   // could take a long time.
   EventAny e = AMPEVENT_DISABLED | AMPEVENT_SOFTDISABLE;
   return e.Wait( eventMap, sdo.GetTimeout() );
}

/***************************************************************************/
/**
  Enable the amplifier.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::Enable( bool wait )
{
   // First, check to see if the amplifier is in a fault state.
   // If it is, then it will ignore my enable request causing a
   // timeout error when I wait for the state change below.
   if( eventMap.getMask() & AMPEVENT_FAULT )
   {
      AMP_FAULT faults;
      GetFaults( faults );
      return AmpFault::DecodeFault( faults );
   }

   const Error *err;

   // Check for other errors which would prevent us from enabling
   if( eventMap.getMask() & AMPEVENT_ERROR )
   {
      EVENT_STATUS stat = (EVENT_STATUS)statPdo.estat.Read();
      err = AmpError::DecodeStatus( stat );
      if( err ) return err;
   }

   // If we're not in a CAN controlled mode, then we need
   // update the amplifier mode to enable it.
   enabled = true;

   if( !isCanMode( lastMode ) )
   {
      AMP_MODE newMode = lastMode;
      lastMode = AMPMODE_DISABLED;
      err = SetAmpMode( newMode );
      if( err ) return err;
      lastMode = newMode;
   }

   // In normal CAN controlled modes, I handle this
   // through the use of the control register.
   else
   {
      err = SetControlWord( 0x000F );
      if( err ) return err;
   }

   if( !wait ) return 0;

   // Now, wait for the amp to actually enable before returning
   EventNone e = AMPEVENT_SOFTDISABLE;
   err = e.Wait( eventMap, sdo.GetTimeout() );

   // A timeout here could mean that a fault condition occured
   // between the time I checked above and when I sent the 
   // new control word.  
   if( err == &ThreadError::Timeout )
   {
      if( eventMap.getMask() & AMPEVENT_FAULT )
      {
	 AMP_FAULT faults;
	 GetFaults( faults );
	 return AmpFault::DecodeFault( faults );
      }
   }

   return err;
}

/***************************************************************************/
/**
  Return true if the amplifier's PWM outputs are currently enabled.
  @return true if the amplifier's PWM outputs are currently enabled.
  */
/***************************************************************************/
bool Amp::IsHardwareEnabled( void )
{
   return !(eventMap.getMask() & AMPEVENT_DISABLED);
}

/***************************************************************************/
/**
  Return true if the amplifier is being enabled by software.  The amplifier
  outputs may still be disabled if this is true due to an error condition, etc.
  @return true if the amplifier is enabled by software.
  */
/***************************************************************************/
bool Amp::IsSoftwareEnabled( void )
{
   return enabled;
}

/***************************************************************************/
/**
  Return true if the amplifier has been successfully referenced (homed).

  When an amplifier is first powered up (or after a reset) it does not know
  the absolute position of the motor.  Once the home routine has been 
  successfully executed, the encoder zero location is known and the amplifier
  is considered referenced.

  Once an amplifier has been referenced, it will not loose reference until it
  is reset, or until a new home routine is executed.  During the execution of
  a home routine, the amplifier is considered to be unreferenced.  If the 
  home routine is completed successfully, the amplifier will then be referenced
  again.

  @return true if the amplifier has been referenced.  Return false if the 
  amplifier has not been referenced if an error occurs reading this 
  information from the amplifier.
  */
/***************************************************************************/
bool Amp::IsReferenced( void )
{
   uint16 trjStatus;
   if( sdo.Upld16( OBJID_TRJ_STATUS, 0, trjStatus ) )
      return false;

   return (trjStatus & 0x1000) == 0x1000;
}

/***************************************************************************/
/**
  Default constructor for amplifier settings object.  This constructor sets
  all the settings to the default values.
  */
/***************************************************************************/
AmpSettings::AmpSettings( void )
{
   // Default to 10 ms synch period using the standard ID (0x80)
   synchPeriod      = 10000;
   synchID          = 0x00000080;
   synchProducer    = false;
   synchUseFirstAmp = true;
   timeStampID      = 0x00000180;

   // Default to using node guarding with a 200 ms guard time.
   heartbeatPeriod  = 0;
   heartbeatTimeout = 200;
   guardTime        = 200;
   lifeFactor       = 3;

   // By default, we put the amp into homing mode and
   // enable it at init time.
   enableOnInit = true;
   initialMode = AMPMODE_CAN_HOMING;

   resetOnInit = false;

   maxPvtSendCt = 6;
}