amppvt.cpp

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

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

/** \file
  This file contains the code used by the Amp object
  to stream PVT trajectory profiles over the CANopen 
  network.
  */

#include "CML.h"

CML_NAMESPACE_USE();

/**************************************************
 * PVT buffer errors
 **************************************************/
#define PVTERR_SEQUENCE        0x01
#define PVTERR_OVERFLOW        0x02
#define PVTERR_UNDERFLOW       0x04

/**************************************************
 * PVT buffer status bit mapping
 **************************************************/
#define PVTSTAT_NEXTID         0x0000FFFF
#define PVTSTAT_FREECT         0x00FF0000
#define PVTSTAT_ERROR          0x7F000000
#define PVTSTAT_EMPTY          0x80000000

/***************************************************************************/
/**
  Format a PVT trajectory segment.  The position, velocity and time information
  passed to the function are organized into the proper format and stored in the
  passed buffer.  The buffer is assumed to be at least 8 bytes long.

  @param pos Position at start of segment (encoder counts)
  @param vel Velocity at start of segment (0.1 counts/sec)
  @param time Time till next segment (milliseconds)
  @param buff Points to the buffer where the message will be stored.
  @return An error object
  */
/***************************************************************************/
const Error *Amp::FormatPvtSeg( int32 pos, int32 vel, uint8 time, uint8 *buff )
{
   cml.Debug( "Amp %d PVT Segment: %-10ld %-10ld %3d\n", GetNodeID(), pos, vel, time );

   // Save the lowest 3 bits of the segment ID.  This allows the
   // amplifier to identify missing profile segments.
   buff[0] = 7 & pvtSegID;

   // If position is more then 24 bits then we will have to send a 
   // relative move segment.  I'll just find the difference between 
   // this position and the last one sent.
   if( pos > 0x007FFFFF || -pos > 0x007FFFFF )
   {
      pos -= pvtLastPos;

      // if the result is still more then 24 bits, return an error
      if( pos > 0x007FFFFF || -pos > 0x007FFFFF )
	 return &AmpError::pvtSegPos;

      // mark the segment as relative
      buff[0] |= 0x10;
   }

   // If velocity is more then 24-bits, send it in lower resolution
   if( vel > 0x007FFFFF )
   {
      vel = (vel+50)/100;
      buff[0] |= 0x08;

      if( vel > 0x007FFFFF )
	 return &AmpError::pvtSegVel;
   }
   else if( -vel > 0x007FFFFF )
   {
      vel = (vel-50)/100;
      buff[0] |= 0x08;

      if( -vel > 0x007FFFFF )
	 return &AmpError::pvtSegVel;
   }

   // Format the message and return     
   buff[1] = time;
   buff[2] = pos;
   buff[3] = pos>>8;
   buff[4] = pos>>16;
   buff[5] = vel;
   buff[6] = vel>>8;
   buff[7] = vel>>16;

   return 0;
}

/***************************************************************************/
/**
  Format a PT trajectory segment.  The position and time information
  passed to the function are organized into the proper format and stored in the
  passed buffer.  The buffer is assumed to be at least 8 bytes long.

  @param pos Position at start of segment (encoder counts)
  @param time Time till next segment (milliseconds)
  @param buff Points to the buffer where the message will be stored.
  @return An error object
  */
/***************************************************************************/
const Error *Amp::FormatPtSeg( int32 pos, uint8 time, uint8 *buff )
{
   cml.Debug( "Amp %d PT Segment: %-10ld %3d\n", GetNodeID(), pos, time );

   // Save the lowest 3 bits of the segment ID.  This allows the
   // amplifier to identify missing profile segments.
   buff[0] = 7 & pvtSegID;

   // PT points are always passed using buffer format code 5
   buff[0] |= (5<<3);

   // Format the message and return     
   buff[1] = time;
   buff[2] = ByteCast(pos);
   buff[3] = ByteCast(pos>>8);
   buff[4] = ByteCast(pos>>16);
   buff[5] = ByteCast(pos>>24);

   return 0;
}

/***************************************************************************/
/**
  Set the initial position for a PVT trajectory.  This function sends a full
  32-bit position value which can be used to start a PVT move beyond the 24-bit
  limit of normal segments.  It's normally used at the beginning of a PVT 
  trajectory when the starting position is greater then 24-bits and the commanded
  position at the time of the move's start is not obvious.

  @param pos The 32-bit initial position
  @param viaSDO If true, use a SDO to download the message.  If false, use a PDO.
  default is true.
  @return An error object
  */
/***************************************************************************/
const Error *Amp::SetPvtInitialPos( int32 pos, bool viaSDO )
{
   byte data[8];

   data[0] = ByteCast(7 & pvtSegID);
   data[0] |= 0x20;

   data[1] = ByteCast(pos);
   data[2] = ByteCast(pos>>8);
   data[3] = ByteCast(pos>>16);
   data[4] = ByteCast(pos>>24);

   if( viaSDO )
      return sdo.Download( OBJID_PVT_DATA, 0, 8, data );
   else
      return pvtPdo.Transmit( data );
}

/***************************************************************************/
/**
  Flush the amplifier's PVT trajectory buffer.  Flushing the buffer in this 
  way will cause any running profile to be aborted.
  @param viaSDO If true, use a SDO to download the message.  If false, use a PDO.
  default is true.
  @return An error object.
  */
/***************************************************************************/
const Error *Amp::PvtBufferFlush( bool viaSDO )
{
   byte data[8];

   data[0] = 0x80;
   if( viaSDO ) 
      return sdo.Download( OBJID_PVT_DATA, 0, 8, data );
   else
      return pvtPdo.Transmit( data );
}

/***************************************************************************/
/**
  Clear the specified PVT buffer errors.  
  @param mask A bit mask representing which PVT buffer errors to clear.
  @param viaSDO If true, use a SDO to download the message.  If false, use a PDO.
  default is true.
  @return An error object.
  */
/***************************************************************************/
const Error *Amp::PvtClearErrors( uint8 mask, bool viaSDO )
{
   byte data[8];

   data[0] = 0x82;
   data[1] = ByteCast(mask);

   if( viaSDO )
      return sdo.Download( OBJID_PVT_DATA, 0, 8, data );
   else
      return pvtPdo.Transmit( data );
}

/***************************************************************************/
/**
  Pop the N most recently sent segments off the amplifier's PVT trajectory buffer.
  If there are less then N segments on the buffer, then the buffer is cleared.
  Any profile running on the amplifier will continue to run (is not aborted)
  unless a buffer underflow occurs.
  @param n The number of segments to pop off the buffer.  Defaults to 1.
  @param viaSDO If true, use a SDO to download the message.  If false, use a PDO.
  default is true.
  @return An error object.
  */
/***************************************************************************/
const Error *Amp::PvtBufferPop( uint16 n, bool viaSDO )
{
   byte data[8];

   data[0] = ByteCast(0x81);
   data[1] = ByteCast(n);
   data[2] = ByteCast(n>>8);

   if( viaSDO )
      return sdo.Download( OBJID_PVT_DATA, 0, 8, data );
   else
      return pvtPdo.Transmit( data );
}

/***************************************************************************/
/**
  Upload a PVT move trajectory to the amplifier and optionally start the move.

  @param trj Reference to the trajectory that will be feed to the amp.  A local
  pointer to this trajectory will be stored if the entire profile will 
  not fit in the amplifiers on-board buffer.  This pointer will be kept
  until the entire profile has been uploaded to the amp.  It is therefore 
  important to ensure that the trajectory object will remain valid (i.e. not be
  deallocated) until the amplifier object has called the Trajectory.Finish() 
  method on it.

  @param start If true (the default), the profile will be started by this call.
  If false, the profile will be uploaded, but not started.  Use true if
  this is a single axis move, false if this is part of a multi-axis move
  which needs to be synchronized.

  @return An error object.
  */
/***************************************************************************/
const Error *Amp::SendTrajectory( Trajectory &trj, bool start )
{
   const Error *err;

   // Make sure we are in interpolated position mode
   err = SetAmpMode( AMPMODE_CAN_PVT );
   if( err ) return err;

   // Get the trajectory buffer status value.
   uint32 stat;
   err = GetPvtBuffStat( stat );
   if( err ) return err;

   // Clear any buffer errors that are outstanding.
   if( stat & PVTSTAT_ERROR )
   {
      err = PvtClearErrors( (uint8)( (stat & PVTSTAT_ERROR) >> 24 ) );
      if( !err ) err = GetPvtBuffStat( stat );
      if( err ) return err;
   }

   // We expect the buffer to be empty at the start of the move.
   // If it isn't we'll clear it.
   if( !(stat & PVTSTAT_EMPTY) )
   {
      err = PvtBufferFlush();
      if( !err ) err = GetPvtBuffStat( stat );
      if( err ) return err;
   }

   // See how many segments will fit in the buffer (which should be
   // empty at this point).  I need at least 2 and would normally
   // expect many more.  If for some reason it's less then 2, then
   // I'll just return an error.  This really should never happen.
   uint8 n = (uint8)((stat & PVTSTAT_FREECT) >> 16);
   if( n < 2 )
      return &AmpError::pvtBufferFull;

   pvtBuffSize = n;

   /// Clear the PVT segment cache
   pvtUseCache = false;
   pvtCache.Clear();

   /// Make sure the trajectory object is ready to go
   err = trj.StartNew();
   if( err ) return err;

   // Keep track of the first segment being sent
   pvtSegActive = pvtSegID = (stat & PVTSTAT_NEXTID);

   /**************************************************
    * Upload as many segments as possible.  I use an
    * SDO to upload the segments here, but will use a 
    * PDO to upload the rest of the profile once we
    * start moving.  This simplifies my error handling
    * here when I can afford the overhead of the SDO.
    **************************************************/
   uint8 segBuff[8];
   uint8 time;
   for( uint8 i=0; i<n; i++ )
   {
      uunit p,v;

      // See if this is a PVT or PT segment
      bool useVel = trj.UseVelocityInfo();

      err = trj.NextSegment( p, v, time );
      if( err )
	 break;

      int32 pos = PosUser2Load(p);

      int32 vel = 0;
      if( useVel ) vel = VelUser2Load(v);

      // If this is the first segment of the move, then 
      // make sure my initial position is known.  I really
      // only need to do this if the position value is
      // larger then 24 bits, otherwise I'll be sending it
      // as an absolute value and won't need the previous
      // position anyway.
      if( useVel && !i && (pos > 0x007FFFFF || -pos > 0x007FFFFF) )
      {
	 err = SetPvtInitialPos( pos );
	 if( err ) break;
	 pvtSegID++;
	 n--;
      }

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

      // Send it using an SDO
      //		err = sdo.Download( OBJID_PVT_DATA, 0, 8, segBuff );
      //		if( err ) break;

      // Send it using a PDO
      pvtPdo.Transmit( segBuff );
      pvtCache.AddSegment( segBuff, pvtSegID, p );

      // Update the segment ID counter and the last segment
      // position info.
      pvtSegID++;
      pvtLastPos = pos;

      // If time is zero, this is the last segment in the move.
      if( !time ) break;
   }

   // Get the buffer status.  We'll process this later
   if( !err ) err = GetPvtBuffStat( stat );

   // If an error occurred during the download, flush 
   // the buffer and return the error code.
   if( err )
   {
      trj.Finish();
      PvtBufferFlush();
      return err;
   }

   // If the whole profile hasn't been sent, keep a pointer
   // to the trajectory object so I can spool it up to the 
   // amp as buffer space becomes available.
   if( time )
      pvtTrj = &trj;
   else
   {
      trj.Finish();
      pvtTrj = 0;
   }

   // Process the current buffer status to handle any lost messages.
   PvtStatusUpdate( stat );

   // Start the move if so requested
   if( start )
      err = StartPVT();

   return err;
}

/***************************************************************************/
/**
  Start a PVT move that has already been uploaded.
  @return A pointer to an error object, or NULL on success.
  */
/***************************************************************************/