amppvt.cpp

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

const Error *Amp::StartPVT( void )
{
   // Start the move
   const Error *err = StartMove();

   if( err && pvtTrj )
   {
      pvtTrj->Finish();
      pvtTrj = 0;
   }

   return err;
}

/***************************************************************************/
/**
  A new abort event was just received.  This function is called from the 
  status PDO when the 'move aborted' status is set.  It's used to clean up
  the PVT move in progress.
  */
/***************************************************************************/
void Amp::MoveAborted( void )
{
   if( pvtTrj )
   {
      pvtTrj->Finish();
      pvtTrj = 0;
   }
}

/***************************************************************************/
/**
  Get the starting position of the PVT segment currently active in the 
  amplifier.  When running in PVT mode, this allows an approximation of the
  amplifier position to be retrieved without adding any additional overhead
  to the CANopen network.

  The position returned by this function is only valid when running in PVT mode.

  @param pos The position is returned here.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/
const Error *Amp::GetPvtSegPos( uunit &pos )
{
   if( pvtCache.GetPosition( &pos, pvtSegActive ) )
      return 0;

   return &AmpError::pvtPosUnavail;
}

/***************************************************************************/
/**
  This function is called by the PVT status PDO receiver function.  It takes
  care of the gory details of streaming out a trajectory that's too big to 
  fit in the amplifier's PVT buffer all at once.

  Note that this function will be called by the receive task and therefore 
  must be as quick as possible.  Also, SDO access is not available from within
  the receive task.

  @param status The amplifier status word passed in the PDO
  */
/***************************************************************************/
void Amp::PvtStatusUpdate( uint32 status )
{
   // Break the status into it's main components.  These are:
   //
   // - The segment ID of the next segment expected by the amp
   // - The number of free positions in the buffer
   // - Any error codes
   uint16 ampNextID = (uint16)(status & PVTSTAT_NEXTID);
   uint8  freeCt    = (uint8)((status & PVTSTAT_FREECT)>>16);
   uint8  errors    = (uint8)((status & PVTSTAT_ERROR )>>24);

   cml.Debug( "Amp %d PVT Stat: %5u %3u 0x%02x\n", GetNodeID(), ampNextID, freeCt, errors );

   // Set an amplifier status bit if the PVT buffer is empty
   if( status & 0x80000000 )
      eventMap.setBits( AMPEVENT_PVT_EMPTY );
   else
      eventMap.clrBits( AMPEVENT_PVT_EMPTY );

   // Keep track of the active segment ID
   pvtSegActive = ampNextID - pvtBuffSize + freeCt - 1;

   // If the amplifier has experienced an underflow error, then
   // it has already aborted the trajectory.  At this point there
   // is no reason to send more segments, so I'll just return.
   if( errors & PVTERR_UNDERFLOW )
   {
      if( pvtTrj )
      {
	 pvtTrj->Finish();
	 pvtTrj = 0;
      }
      return;
   }

   // If a segment sequencing error has occurred, then I'll try
   // to resend data starting at the segment the amp desires.
   // First, I'll clear the error and update my state to
   // indicate that segments need to be resent.
   // That's all I'll do until I get a status indicating that 
   // the error was cleared since there could be other pending
   // status messages coming my way with the old error flagged, and
   // I don't want to get confused about where the error occurred.
   if( errors & PVTERR_SEQUENCE )
   {
      PvtClearErrors( PVTERR_SEQUENCE, false );
      pvtUseCache = true;
      pvtCacheID = ampNextID;
      return;
   }

   // OK, no errors that we care about.  Reduce the free count value
   // passed in the status to take any recently sent segments into
   // account.  The result is the number of new segments we can
   // send safely.
   if( pvtUseCache )
      freeCt -= (pvtCacheID - ampNextID);
   else
      freeCt -= (pvtSegID - ampNextID);

   // Limit the number of new segments I'll send in response
   // to a single status message.  
   if( freeCt > initialSettings.maxPvtSendCt )
      freeCt = initialSettings.maxPvtSendCt;

   // Now, send as many segments as possible.
   for( uint8 i=0; i<freeCt; i++ )
   {
      uint8 buff[8];

      // See if we need to resend an old segment.  
      // If so, I'll pull it out of the cache.
      if( pvtUseCache )
      {
	 // If the necessary segment isn't available, then 
	 // I have no choice but to flush the cache and 
	 // abort the profile.
	 if( !pvtCache.GetSegment( buff, pvtCacheID ) )
	 {
	    PvtBufferFlush( false );
	    if( pvtTrj )
	    {
	       pvtTrj->Finish();
	       pvtTrj = 0;
	    }
	    return;
	 }

	 if( ++pvtCacheID == pvtSegID )
	    pvtUseCache = false;
      }

      // We need to get a new segment from the trajectory generator.
      // If there isn't one then it means that we're done sending
      // segments out, so we can just return.
      else if( !pvtTrj ) 
	 return;

      else
      {
	 // If the trajectory generator returns an error then I'll 
	 // just return and try again next time.
	 uint8 time;
	 uunit p, v;

	 // See if this is a PVT or PT segment
	 bool useVel = pvtTrj->UseVelocityInfo();

	 const Error *err = pvtTrj->NextSegment( p, v, time );
	 if( err )
	 {
	    cml.Warn( "Amp %d PVT NextSegment returned %s\n", err->toString() );
	    return;
	 }

	 int32 pos = PosUser2Load(p);
	 int32 vel = 0;
	 if( useVel ) vel = VelUser2Load(v);

	 // Format the PVT segment
	 if( useVel )
	    err = FormatPvtSeg( pos, vel, time, buff );
	 else
	    err = FormatPtSeg( pos, time, buff );

	 // If the format failed, this is a critical error.
	 // I'll flush the trajectory buffer which causes the
	 // trajectory to be aborted.
	 if( err )
	 {
	    PvtBufferFlush( false );
	    pvtTrj->Finish();
	    pvtTrj = 0;
	    return;
	 }

	 // Add this segment to my cache for later error handling
	 pvtCache.AddSegment( buff, pvtSegID, p );

	 pvtSegID++;
	 pvtLastPos = pos;

	 // If this was the last segment in the trajectory (i.e. the
	 // time was zero), then discard the trajectory object.
	 if( !time )
	 {
	    pvtTrj->Finish();
	    pvtTrj = 0;
	 }
      }

      // Send the segment using a PDO
      pvtPdo.Transmit( buff );
   }

   return;
}

/***************************************************************************/
/**
  Add the passed segment to the cache.  Segments must be passed in order and 
  with no gaps between ID numbers.  If this segment doesn't follow those rules
  then the cache will be cleared before the segment is added.
  @param seg Points to an array of 8 bytes which make up the segment to be added.
  The segment data is copied into the cache.  No copy of the pointer is 
  kept locally.
  @param id The ID number of the passed segment.
  @param p The position corresponding to this segment.
  */
/***************************************************************************/
void PvtSegCache::AddSegment( uint8 *seg, uint16 id, uunit p )
{
   // If the ID is not one greater then the last ID
   // added to the cache, then clear it.  
   // Segments must be added to the cache in order 
   // with no gaps.

   if( id != oldest+ct )
   {
      Clear();
      oldest = id;
   }

   // Figure out where to put this one
   int index;

   if( ct < PVTCACHESIZE )
      index = ct++;
   else
   {
      oldest++;
      index = top++;
      if( top == PVTCACHESIZE )
	 top = 0;
   }

   // Add the new segment
   for( int i=0; i<8; i++ )
      cache[index][i] = seg[i];
   pos[index] = p;
}

/***************************************************************************/
/**
  Get the specified segment from the cache.  If the requested segment is 
  available, it's contents will be copied to the passed pointer.
  @param seg A pointer to an array of 8 bytes where the segment data 
  will be copied on success.
  @param id The ID number of the segment being requested
  @return true on success, false if the requested segment isn't available.
  */
/***************************************************************************/
bool PvtSegCache::GetSegment( uint8 *seg, uint16 id )
{
   int16 index = id - oldest;

   // Fail if the index is outside my range
   if( index < 0 || index >= ct )
      return false;

   index += top;
   if( index >= PVTCACHESIZE )
      index -= PVTCACHESIZE;

   for( int i=0; i<8; i++ )
      seg[i] = cache[index][i];

   return true;
}

/***************************************************************************/
/**
  Get the position corresponding to the specified segment from the cache.
  If the requested position is available, it will be copied to the passed pointer.
  @param p A pointer to where the position information will be copied.
  @param id The ID number of the segment being requested
  @return true on success, false if the requested segment isn't available.
  */
/***************************************************************************/
bool PvtSegCache::GetPosition( uunit *p, uint16 id )
{
   int16 index = id - oldest;

   // Fail if the index is outside my range
   if( index < 0 || index >= ct )
      return false;

   index += top;
   if( index >= PVTCACHESIZE )
      index -= PVTCACHESIZE;

   *p = pos[index];
   return true;
}

/***************************************************************************/
/**
  Initialize a transmit PDO used to send PVT buffer status updates
  The PDO is initialized to transmit a PDO on PVT buffer status events.
  The events occur every time a segment is read out of the buffer
  or an error bit is set.

  @param amp Reference to the amplifier object 
  @param slot TPDO slot number for this PDO
  @return An error object
  */
/***************************************************************************/
const Error *TPDO_PvtBuffStat::Init( Amp &amp, uint16 slot )
{
   ampPtr = &amp;

   // Initialize the transmit PDO
   const Error *err = TPDO::Init( amp.GetCanOpen(), amp.GetTpdoCobID( slot ) );

   // Set transmit type to transmit on events
   if( !err ) err = SetType( 255 );

   // Init the stat mapped object
   if( !err ) err = stat.Init( OBJID_PVT_BUFF_STAT, 0 );

   // Add the mapped variable
   if( !err ) err = AddVar( stat );

   // Enable reception of this PDO's messages
   if( !err ) err = EnableReceiver();

   // Program this PDO in the amp, and enable it
   if( !err ) err = amp.PdoSet( slot, *this );

   return err;
}

/***************************************************************************/
/**
  Handle the reception of status information.  This PDO waits for one of the
  required bits in the status word to be set, and posts the amplifiers 
  semaphore when it is.
  */
/***************************************************************************/
void TPDO_PvtBuffStat::Received()
{
   ampPtr->PvtStatusUpdate( stat.Read() );
}

/***************************************************************************/
/**
  Initialize the receive PDO used to pass PVT segments to an amplifier.
  @param amp The amplifier that the PDO is associated with
  @param slot The RPDO slot used by this PDO
  @return An error object
  */
/***************************************************************************/
const Error *RPDO_Pvt::Init( Amp &amp, uint16 slot )
{
   ampPtr = &amp;

   const Error *err = RPDO::Init( amp.GetRpdoCobID(slot) );

   if( !err ) err = dat.Init( OBJID_PVT_DATA, 0, 64 );
   if( !err ) err = AddVar( dat );
   if( !err ) err = SetType( 255 );

   if( !err ) err = amp.PdoSet( slot, *this );

   return err;
}

/***************************************************************************/
/**
  Transmit the passed data over the PDO
  @param data Pointer to the data to send
  @return An error object
  */
/***************************************************************************/
const Error *RPDO_Pvt::Transmit( byte *data )
{
   dat.Set(data);

   return RPDO::Transmit( ampPtr->GetCanOpen() );
}