trjscurve.cpp

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

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

#include "CML_Settings.h"
#ifdef CML_ALLOW_FLOATING_POINT

#include <math.h>
#include "CML.h"

CML_NAMESPACE_USE();

CML_NEW_ERROR( ScurveError, BadParam,  "An illegal input parameter was passed" );
CML_NEW_ERROR( ScurveError, NoCalc,    "Trjaectory has not been calculated" );
CML_NEW_ERROR( ScurveError, InUse,     "Trajectory is currently in use" );
CML_NEW_ERROR( ScurveError, NotInUse,  "Trajectory has not been started" );

/***************************************************************************/
/**
S-curve trajectory default constructor.  This simply sets the profile to 
zero length with a starting position of zero.
*/
/***************************************************************************/
TrjScurve::TrjScurve( void )
{
   start = 0;
   inUse = false;
   init = false;
}

/***************************************************************************/
/**
Set the trajectory starting position.  S-curve profiles are internally stored
as absolute moves of some length.  This allows them to be used multiple times 
with different starting positions.  

This function may be used to update the starting position of the trajectory.

@param s The new starting position
*/
/***************************************************************************/
void TrjScurve::SetStartPos( uunit s )
{
   start = s;
   return;
}

/***************************************************************************/
/**
Return the current starting position of the trajectory.  The starting position
will either be the value set using TrjScurve::SetStartPos, or the value set 
using TrjScurve::Calculate.  If neither has been called since construction, 
then the starting position will be zero.

@return The trajectory starting position.
*/
/***************************************************************************/
uunit TrjScurve::GetStartPos( void )
{
   return start;
}

/***************************************************************************/
/**
Calculate a new S-curve profile and also set it's starting position.

@param start The profile starting position.
@param end The profile ending position.
@param vel The maximum allowable velocity for the move.
@param acc The maximum allowable acceleration for the move.
@param dec The maximum allowable deceleration for the move.
@param jrk The maximum jerk (rate of change of acceleration) for the move.

@return A pointer to an error object, or NULL on success.
*/
/***************************************************************************/
const Error *TrjScurve::Calculate( uunit start, uunit end, uunit vel, uunit acc, uunit dec, uunit jrk )
{
   uunit dist = end-start;
   const Error *err = Calculate( dist, vel, acc, dec, jrk );
   if( err ) return err;
   SetStartPos( start );
   return 0;
}

/***************************************************************************/
/**
Calculate a new S-curve profile.  The resulting profile may then be sent 
to an Amp object using the Amp::SendTrajectory method.

Note, all profile parameters are passed in 'user units'.  See the 
documentation for Amp::SetCountsPerUnit for details.

Note also that this function calculate the profile as an absolte move from
the starting position that is set using TrjScurve::SetStartPos.  The same
profile may be used multiple times with different starting positions without
calling the TrjScurve::Calculate function again.

@param dist The distance to move.
@param maxVel The maximum allowable velocity for the move.
@param maxAcc The maximum allowable acceleration for the move.
@param maxDec The maximum allowable deceleration for the move.
@param maxJrk The maximum jerk (rate of change of acceleration) for the move.

@return A pointer to an error object, or NULL on success.

The distance will always be met exactly.  This may be either positive or
negative.
  
The velocity, acceleration, deceleration and jerk values are constraints 
and won't be exceeded.  These must all be positive numbers greater then zero.
*/
/***************************************************************************/
const Error *TrjScurve::Calculate( uunit dist, uunit maxVel, uunit maxAcc, uunit maxDec, uunit maxJrk )
{
   /**************************************************
    * Do some sanity checks on the input limits.
    **************************************************/
   if( maxVel<=0 || maxAcc<=0 || maxDec<=0 || maxJrk<=0 )
      return &ScurveError::BadParam;

   if( inUse ) return &ScurveError::InUse;

   /**************************************************
    * Convert to floating point units if necessary
    **************************************************/
#ifdef CML_ENABLE_USER_UNITS
   P = dist;
   V = maxVel;
   A = maxAcc;
   D = maxDec;
   J = maxJrk;
#else
   P = dist;
   V = maxVel * 0.1;
   A = maxAcc * 10.0;
   D = maxDec * 10.0;
   J = maxJrk * 100.0;
#endif

   /**************************************************
    * Assume positive moves for simplicity.  We'll fix
    * this in the end if necessary.
    **************************************************/
   bool negMove = (P<0);
   if( negMove )
      P *= -1.0;

   if( P==0 )
   {
      tj = tk = ta = td = tv = 0;
      init = true;
      return 0;
   }

   /**************************************************
    * Make sure A <= D.  This reduces the number of 
    * tests I need to do later.  I'll fix this at the
    * end of the calculation.
    **************************************************/
   bool swapAD = (A > D);

   if( swapAD )
   {
      double tmp;
      tmp = A; A = D; D = tmp;
   }

   /**************************************************
    * I'll lower jerk to ensure that my jerk segments
    * are at least 1 millisecond long.
    **************************************************/
   if( J > A*1e3 ) J = A * 1e3;
   if( J > V*1e6 ) J = V * 1e6;
   if( J > P*5e8 ) J = P * 5e8;

   /**************************************************
    * These are the key variables I'll need to find.
    *   tj = time to increase/decrease acceleration
    *   ta = time to run at constant accel
    *   tv = time to run at constant velocity
    *   td = time to run at constant decel
    *   tk = time to increase/decrease deceleration
    **************************************************/

   /**************************************************
    * See if a simple jerk limited move will handle 
    * this.  In this case, the move will be 4 segments
    * each of the same time.
    **************************************************/
   tj = pow( P/(2*J), 1.0/3.0 );
   if( (J*tj < A) && (J*tj*tj < V) )
   {
      ta = td = tv = 0;

      // Adjust time & Jerk to the next higher millisecond
      tk = tj = 0.001 * ceil( tj*1000 );

      J  = P / (2 * tj*tj*tj);
   }

   /**************************************************
    * We know we'll hit either the accel or velocity 
    * limit.  See if the accel limit is too high.
    * If so, this must be a Jerk & Velocity move.
    **************************************************/
   else if( J*V < A*A )
   {
      ta = td = 0;
      tj = sqrt( V/J );

      // Adjust the times so they are integer multiples
      // of milliseconds.  I'll adjust J & V to compensate
      tk = tj = 0.001 * ceil( tj*1000 );
      tv = P/V - 2*tj;

      tv = 0.001 * ceil( tv*1000 );

      V = P / (tv + 2*tj);
      J = V / (tj*tj);
   }

   else 
   {
      /**************************************************
       * At this point we know we will hit the accel limit.
       * We may or may not hit the velocity & decel limits.
       * I'll start by assuming that I'll hit the velocity
       * limit.
       **************************************************/
      double vj, vk;

      tj = A/J;
      vj = A*tj / 2.0;

      ta = (V - 2*vj) / A;

      if( J*V < D*D )
      {
	 td = 0.0;
	 tk = sqrt(V/J);
	 vk = V/2;
      }
      else
      {
	 tk = D/J;
	 td = (V-J*tk*tk) / D;
	 vk = D*tk / 2.0;
      }

      // Find the distance moved getting up to 
      // and down from V
      double pa = tj*vj*2 + ta*vj + A*ta*tj + ta*ta*A/2.0;
      double pd = tk*vk*2 + td*vk + D*td*tk + td*td*D/2.0;

      // If this distance is less then the total move,
      // then I've found my solution.  Otherwise, the
      // velocity limit isn't reached.
      if( pa+pd <= P )
	 tv = (P-pa-pd) / V;

      else
      {
	 /**************************************************
	  * At this point, we know we will hit the accel 
	  * limit, but not the velocity limit.  The only 
	  * question now is whether the decel limit will 
	  * be reached.
	  *
	  * I'll try no decel limit first.
	  **************************************************/
	 tv = 0.0;
	 tk = (sqrt( sqrt(2*P*A)*4*J +A*A ) - A) / (2*J);

	 if( J*tk <= D )
	 {
	    ta = (J*tk*tk - J*tj*tj) / A;
	    td = 0.0;
	 }

	 else
	 {