path.cpp

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

/**
 * Path segment used to delay for a specified amount of time.
 */
class DelaySeg: public PathElement
{
   Point<PATH_MAX_DIMENSIONS> pos;
   double delayTime;
public:
   DelaySeg( PointN &p, double t )
   {
      pos = p;
      delayTime = t;
      setLength( 0.0 );
   }
   virtual double getMaxVel( void ){ return 0.0; }
   virtual bool Calculate( void )
   {
      if( calculated ) return true;
      calculated = true;
      tv = delayTime;
      ta = td = 0.0;
      return false;
   }

   const Error *getTrjSeg( double t, uunit p[], uunit v[] )
   {
      for( int i=0; i<pos.getDim(); i++ )
      {
	 p[i] = pos[i];
	 v[i] = 0;
      }
      return 0;
   }
};

Path::Path( uint d )
{
   CML_ASSERT( d <= PATH_MAX_DIMENSIONS );

   if( d > PATH_MAX_DIMENSIONS )
      d = PATH_MAX_DIMENSIONS;
   dim = d;
   maxVel = 0.0;
   maxAcc = 0.0;
   maxDec = -1.0;
   first = last = 0;
   dirEnd = 0.0;
   posEnd.setDim(d);
   posStart.setDim(d);
   Reset();
}

Path::~Path( void )
{
   while( first )
   {
      PathElement *pe = first;
      first = pe->getNext();
      delete pe;
   }
}

void Path::Reset( void )
{
   crntSeg = first;
   segTime = 0;
}


const Error *Path::SetStartPos( PointN &p )
{
   CML_ASSERT( p.getDim() == GetDim() );
   if( p.getDim() != GetDim() )
      return &PathError::BadPoint;

   posStart = p;
   return 0;
}


/**
  Offset the passed array of relative positions based
  on the starting position set by the user.
  @p Array of positions to be adjusted
 */
void Path::OffsetPos( double p[] )
{
   for( int i=0; i<dim; i++ )
      p[i] += posStart[i];
}

/** 
 * Set a new velocity limit which will apply to any new
 * segments added to the path.
 *
 * The passed velocity must be >= zero.
 *
 * @param v The velocity limit to use
 * @return An error object pointer or NULL on success
 */
const Error *Path::SetVel( uunit v )
{
   if( v <= 0.0 ) return &PathError::BadVel;
   maxVel = v;
   return 0;
}

/** 
 * Set a new acceleration limit which will apply to any new
 * segments added to the path.
 *
 * The passed acceleration value must be >= zero.
 *
 * @param a The acceleration limit to use
 * @return An error object pointer or NULL on success
 */
const Error *Path::SetAcc( uunit a )
{
   if( a <= 0.0 ) return &PathError::BadAcc;
   maxAcc = a;
   return 0;
}

/** 
 * Set a new deceleration limit which will apply to any new
 * segments added to the path.
 *
 * If a positive deceleration is passed, then this will be used
 * as the deceleration limit.  If a zero or negative value is passed
 * then the acceleration value will be used for deceleration as well
 * as acceleration.
 *
 * @param d The deceleration limit to use
 * @return An error object pointer or NULL on success
 */
const Error *Path::SetDec( uunit d )
{
   maxDec = d;
   return 0;
}

/**
 * Add a new segment to the end of this path.
 */
const Error *Path::AddSegment( PathElement *e )
{
   // Check for uninitialized velocity and accelerations
   if( maxVel <= 0.0 ) return &PathError::VelNotInit;
   if( maxAcc <= 0.0 ) return &PathError::AccNotInit;

   // Initialize the new segment
   e->Init( maxVel, maxAcc, maxDec );

   // Add this segment to the end of my path
   e->Add( last );
   last = e;
   if( !first )
   {
      first = e;
      Reset();
   }

   // Adjust the ending velocities of preceeding 
   // segments.
   while( 1 )
   {
      double v = e->getMaxStartVel();

      e = e->getPrev();
      if( !e ) break;

      if( !e->adjustEndVel(v) )
	 break;
   }

   // Handle internal calculations for all segments
   // that have been fully added.
   while( e )
   {
      if( e->Calculate() )
	 break;
      e = e->getPrev();
   }

   return 0;
}

const Error *Path::AddLine( PointN &p )
{
   // Make sure the passed point is of the correct dimension
   CML_ASSERT( p.getDim() == GetDim() );
   if( p.getDim() != GetDim() )
      return &PathError::BadPoint;

   // Adjust the passed point to find a position relative to 
   // the starting position.
   p -= posStart;

   // Find the direction of travel between the current position
   // and the passed point.
   double dx = p[0] - posEnd[0];
   double dy = 0.0;

   if( GetDim() > 1 )
      dy = p[1] - posEnd[1];

   double dirMove = atan2( dy, dx );

   // If the move direction isn't in line with my current direction,
   // then I'll have to come to a halt before adding the line segment.
   // Otherwise, I would have an infinite acceleration during the 
   // direction change.
   if( fabs(dirMove - dirEnd) > MAX_ANGLE_ERROR )
      Pause(0);

   double len = posEnd.distance( p );

   // Now, add the line segment to my path
   LineSeg *seg = new LineSeg( posEnd, dirMove, len );
   if( !seg )
      return &PathError::Alloc;

   const Error *err = AddSegment( seg );
   if( err ) return err;

   // Update my ending position & direction
   posEnd = p;
   dirEnd = dirMove;
   return 0;
}

const Error *Path::AddLine( uunit length )
{
   // Length must be a positive value.
   CML_ASSERT( length >= 0 );

   // Calculate the ending position based on the
   // current position and direction of travel
   Point<PATH_MAX_DIMENSIONS> p;
   p.setDim( GetDim() );

   p = posEnd;
   p[0] += cos(dirEnd) * length;
   if( GetDim() > 1 )
      p[1] += sin(dirEnd) * length;

   // add the line segment to my path
   LineSeg *seg = new LineSeg( posEnd, dirEnd, length );
   if( !seg ) return &PathError::Alloc;

   const Error *err = AddSegment( seg );
   if( err ) return err;

   // Update my ending position & direction
   posEnd = p;
   return 0;
}

const Error *Path::AddArc( PointN &center, double angle )
{
   // Can't add an arc to a one dimensional path
   CML_ASSERT( GetDim() > 1 );

   // Make sure the center position has the right number of dimensions
   CML_ASSERT( GetDim() == center.getDim() );

   // Adjust the center to find a position relative to 
   // the starting position.
   center -= posStart;

   // Find the starting angle on the arc
   double deltaX = center[0] - posEnd[0];
   double deltaY = center[1] - posEnd[1];
   double startAng = PI + atan2( deltaY, deltaX );

   // See if there will be an abrupt change in direction when we start
   // this path.  If so, then I'll need to come to a halt first.
   double deltaDir;
   if( angle < 0 )
      deltaDir = startAng - dirEnd + PI_by_2;
   else
      deltaDir = startAng - dirEnd - PI_by_2;

   if( fabs(deltaDir) > MAX_ANGLE_ERROR )
      Pause(0);

   double radius = center.distance( posEnd );
   ArcSeg *seg = new ArcSeg( center, radius, startAng, angle );
   if( !seg ) return &PathError::Alloc;

   const Error *err = AddSegment( seg );
   if( err ) return err;

   // Update the ending position and direction
   double ang = startAng - angle;
   posEnd[0] = center[0] + cos(ang) * radius;
   posEnd[1] = center[1] + sin(ang) * radius;

   if( angle < 0 ) 
      dirEnd = ang + PI_by_2;
   else 
      dirEnd = ang - PI_by_2;

   return 0;
}

const Error *Path::AddArc( double radius, double angle )
{
   // radius must be a positive value.
   CML_ASSERT( radius >= 0 );

   // Can't add an arc to a one dimensional path
   CML_ASSERT( GetDim() > 1 );

   // Find the center.  This will be a point on the line that passes
   // through the current end position and is orthogonal to the current
   // direction of motion.
   Point<PATH_MAX_DIMENSIONS> center;
   center.setDim( GetDim() );
   center = posEnd;

   double startAng;

   // Positive angles are clockwise rotation
   if( angle < 0 )
   {
      center[0] -= radius * sin(dirEnd);
      center[1] += radius * cos(dirEnd);
      startAng = dirEnd - PI_by_2;
   }
   else
   {
      center[0] += radius * sin(dirEnd);
      center[1] -= radius * cos(dirEnd);
      startAng = dirEnd + PI_by_2;
   }

   ArcSeg *seg = new ArcSeg( center, radius, startAng, angle );
   if( !seg ) return &PathError::Alloc;

   const Error *err = AddSegment( seg );
   if( err ) return err;

   // Update the ending position and direction
   double ang = startAng - angle;
   posEnd[0] = center[0] + cos(ang) * radius;
   posEnd[1] = center[1] + sin(ang) * radius;

   dirEnd -= angle;
   return 0;
}

const Error *Path::Pause( double sec )
{
   DelaySeg *seg = new DelaySeg( posEnd, sec );
   if( !seg ) return &PathError::Alloc;
   return AddSegment( seg );
}

int Path::GetDim( void )
{
   return dim;
}

uint8 Path::GetTime( void )
{
   // Ask the current segment for the next largest time
   // increment it can provide.
   double oldT = segTime;
   uint ms;

   while( 1 )
   {
      bool end = crntSeg->getNextSegTime( segTime );
      double diff = segTime - oldT;

      // My maximum PVT segment length is 0.255 seconds.  If the time 
      // difference is more then abount 500ms then I'll use this max
      if( diff >= 0.500 )
      {
	 ms = 255;
	 break;
      }

      // If the time segment is greater then 255ms, I'll use half of it.
      if( diff >= 0.255 )
      {
	 ms = (uint8)(diff * 500);
	 break;
      }

      // If the time is at least my minimum, I'll use it.
      if( diff >= MIN_PVT_TIME )
      {
	 ms = (uint8)(diff * 1000);
	 break;
      }

      // If I'm not at the end of this segment, just ask for 
      // a larger time increment
      if( !end ) continue;

      // If this is not the last segment of the trajectory, then move
      // to the next segment.
      if( crntSeg->getNext() )
      {
	 oldT -= segTime;
	 segTime = 0.0;
	 crntSeg = crntSeg->getNext();
	 continue;
      }

      // I've found the end of the profile.  If there is at least a 
      // millisecond of time available then I'll use that.  Otherwise 
      // I'll return zero.
      if( diff >= 0.001 )
      {
	 ms = (uint8)(1000*diff);

	 break;
      }

      return 0;
   }

   // At this point, ms give the time in milliseconds that I'll use for 
   // this segment.  I need to adjust the segment time to reflect that.
   segTime = oldT + 0.001 * ms;
   return ms;
}

const Error *Path::StartNew( void )
{
   Reset();
   return (crntSeg) ? 0 : &PathError::Empty;
}

const Error *Path::NextSegment( uunit pos[], uunit vel[], uint8 &time )
{
   if( !crntSeg ) return &PathError::Empty;

   const Error *err = crntSeg->getTrjSeg( segTime, pos, vel );
   time = GetTime();

   OffsetPos( pos );

   return err;
}

bool Path::PlayPath( double timeInc, double pos[], double vel[] )
{
   if( !crntSeg ) return true;

   crntSeg->getTrjSeg( segTime, pos, vel );
   segTime += timeInc;

   OffsetPos( pos );

   double t = crntSeg->getDuration();
   if( segTime >= t )
   {
      segTime -= t;
      crntSeg = crntSeg->getNext();
   }

   return (crntSeg) ? false : true;
}

CML_NAMESPACE_END()

#endif