trjscurve.cpp

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

	    tk = D/J;

	    double a = J*A*(D+A);
	    double b = 3*A*A*D -2*A*D*D +2*A*A*A +3*A*D*D;
	    double c = (A*A + D*D + 2*A*D) * A*A/J - 2*P*J*D;

	    ta = (-b + sqrt(b*b -4*a*c)) / (2*a);
	    td = (J*tj*tj + A*ta - J*tk*tk)/D;
	 }
      }
   }

   /**************************************************
    * If I previously swapped A & D, fix that now.
    **************************************************/
   if( swapAD )
   {
      double tmp;
      tmp = ta; ta = td; td = tmp;
      tmp = tj; tj = tk; tk = tmp;
      tmp =  A;  A =  D;  D = tmp;
   }

   /**************************************************
    * Adjust for negative moves as necessary
    **************************************************/
   if( negMove )
   {
      P  *= -1.0;
      J  *= -1.0;
   }

   init = true;
   return 0;
}

/***************************************************************************/
/**
Reset this object so it may be passed to an amplifier.  This will return an
error if the trajectory has not yet been calculated, or if it is currently 
being sent to another amp.

@return A pointer to an error object, or NULL on success
*/
/***************************************************************************/
const Error *TrjScurve::StartNew( void )
{
   if( !init ) return &ScurveError::NoCalc;
   if( inUse ) return &ScurveError::InUse;

   inUse = true;

   p = start;
   v = a = 0;
   j = J;
   remain = tj;
   seg = 0;

   return 0;
}

/***************************************************************************/
/**
Notify the trajectory object that it is no longer in use.
*/
/***************************************************************************/
void TrjScurve::Finish( void )
{
   inUse = false;
}

/***************************************************************************/
/**
Get the next PVT segment for this s-curve profile.
*/
/***************************************************************************/
const Error *TrjScurve::NextSegment( uunit &pout, uunit &vout, uint8 &tout )
{
   if( !inUse ) return &ScurveError::NotInUse;

   /**************************************************
    * We have previously calculate the position & 
    * velocity for this segment.  I'll set the output
    * values now.
    **************************************************/
#ifdef CML_ENABLE_USER_UNITS
   pout = p;
   vout = v;
#else
   if( p >= 0 ) pout = (uunit)(p + 0.5);
   else         pout = (uunit)(p - 0.5);

   if( v >= 0 ) vout = (uunit)(10.0 * v + 0.5);
   else         vout = (uunit)(10.0 * v - 0.5);
#endif

   if( seg == 7 )
   {
      tout = 0;
      return 0;
   }

   bool eos = false;
   double t;

   if( remain > 0.510 )
   {
      t = 0.255;
      tout = 255;
   }

   else if( remain > 0.254 )
   {
      tout = (uint8)floor(500*remain);
      t = 0.001 * tout;
   }

   else
   {
      eos = true;
      tout = (uint8)floor(remain*1000 + 0.01);
      t = remain;
   }

   p += v*t + a*t*t/2 + j*t*t*t/6;
   v += a*t + j*t*t/2;
   a += j*t;

   remain -= t;
   if( remain < 0 ) remain = 0;

   // We're done if that wasn't the end of a segment.
   if( !eos )
      return 0;

   // There's a good chance that the previous segment time 
   // wasn't an even number of milliseconds.  Find the amount
   // of time I went over the millisecond mark.
   double ms;
   double f = modf( 1000*t, &ms );
   double tf = 0;

   // Increment my output time if I didn't end on an even millisecond.
   if( f > 0.001 )
   {
      tf = 0.001 * (1.0 - f);
      tout++;
   }

   // Now, keep advancing to the next segment until I either come
   // to the end of the move, or find a segment with more then 1ms
   // available.
   while( AdvanceSegment(tf) && (remain < 0.001) );

   return 0;
}

/***************************************************************************/
/**
Move to the next s-curve segment with a non-zero time.  I also adjust my
local position, vel, etc values using the passed time.

@param tf The time value used to advance pos, vel, acc in the new segment.
@return zero if at the end of the move, else non-zero.
*/
/***************************************************************************/
int TrjScurve::AdvanceSegment( double &tf )
{
   // Find the next segment with a non-zero time
   switch( seg++ )
   {
      case 0: remain = ta; j =  0; if( remain > 0.000001 ) break; seg++;
      case 1: remain = tj; j = -J; break;
      case 2: remain = tv; j =  0; if( remain > 0.000001 ) break; seg++;
      case 3: remain = tk; j = -J; break;
      case 4: remain = td; j =  0; if( remain > 0.000001 ) break; seg++;
      case 5: remain = tk; j =  J; break;
      default: return 0;
   }

   // Advance the p,v,a values by the amount passed, or by
   // the total time in this segment if it's less.
   double t = (tf > remain) ? remain : tf;

   p += v*t + a*t*t/2 + j*t*t*t/6;
   v += a*t + j*t*t/2;
   a += j*t;

   // Reduce the total time left in this segment by the value used.
   remain -= t;
   tf -= t;
   return 1;
}

/***************************************************************************/
/**
Default constructor for multi-axis s-curve trajectory.
*/
/***************************************************************************/
LinkTrjScurve::LinkTrjScurve( void ){}

/***************************************************************************/
/**
Calculate a multi-axis s-curve trajectory.  This function calculates the 
straight line move between the two passed positions.
@param s The starting position
@param e The ending position
@param vel The max velocity
@param acc The max acceleration
@param dec The max deceleration
@param jrk The max jerk (rate of change of velocity)
@return A pointer to an error object, or NULL on success.
*/
/***************************************************************************/
const Error *LinkTrjScurve::Calculate( PointN &s, PointN &e, uunit vel, 
                                       uunit acc, uunit dec, uunit jrk )
{
   int d = s.getDim();

   CML_ASSERT( d <= CML_MAX_AMPS_PER_LINK );

   double dist = s.distance( e );

   const Error *err = trj.Calculate( dist, vel, acc, dec, jrk );
   if( err ) return err;

   double invDist = (dist!=0) ? 1.0/dist : 0.0;

   start.setDim( d );
   for( int i=0; i<d; i++ )
   {
      start[i] = s[i];
      scale[i] = (e[i] - s[i]) * invDist;
   }

   return 0;
}

/***************************************************************************/
/**
Start a new move using this trajectory.  The trajectory must have already
been calculated when this function is called.
@return A pointer to an error object, or NULL on success.
*/
/***************************************************************************/
const Error *LinkTrjScurve::StartNew( void )
{
   return trj.StartNew();
}

/***************************************************************************/
/**
Retrieve the next segment of this trajectory.  The positions & velocities for
all axes are returned in the passed arrays.

@param pos An array which will be filled with position information.  
@param vel An array which will be filled with velocity information.
@param time A reference to a variable where the time (milliseconds) will be
       returned.
@return A pointer to an error object, or NULL on success.
*/
/***************************************************************************/
const Error *LinkTrjScurve::NextSegment( uunit pos[], uunit vel[], uint8 &time )
{
   uunit p, v;

   const Error *err = trj.NextSegment( p, v, time );
   if( err ) return err;

   for( int i=0; i<start.getDim(); i++ )
   {
      pos[i] = start[i] + p*scale[i];
      vel[i] = v*scale[i];
   }
   return 0;
}

/***************************************************************************/
/**
Finish this trajectory. 
*/
/***************************************************************************/
void LinkTrjScurve::Finish( void )
{
   trj.Finish();
}

#endif