path.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"
#include "CML_Path.h"

CML_NAMESPACE_START()

// Error objects specific to path planning
CML_NEW_ERROR( PathError, BadVel,         "Illegal velocity value"         );
CML_NEW_ERROR( PathError, BadAcc,         "Illegal acceleration value"     );
CML_NEW_ERROR( PathError, VelNotInit,     "Velocity limit not yet set"     );
CML_NEW_ERROR( PathError, AccNotInit,     "Acceleration limit not yet set" );
CML_NEW_ERROR( PathError, BadPoint,       "The passed point doesn't match the path" );
CML_NEW_ERROR( PathError, Alloc,          "Unable to allocate memory for path" );
CML_NEW_ERROR( PathError, BadLength,      "An illegal negative length value was passed" );
CML_NEW_ERROR( PathError, Empty,          "Attempt to execute an empty path" );

// This constant defines the maximum angle (radians) between two line
// segments that I will accept without a full stop in between.
//
// FIXME: At the moment this is just set to 0.1 deg without much justification.
//        I should study the issue to come up with a better number
#define PI		3.14159265358979323846	/* pi */
#define PI_by_2		1.57079632679489661923	/* pi/2 */
#define MAX_ANGLE_ERROR      (0.1 * PI/180.0)

#define MIN_PVT_TIME         0.001

/**
 Internal class used by Path planning code.
 */
class PathElement
{
protected:
   PathElement *next;
   PathElement *prev;

   // These values are calculated when a segment is first 
   // initialized and never change during the life of the 
   // segment.
   double velUp;    // Maximum increase in velocity based on accel & distance
   double velDn;    // Maximum decrease in velocity based on decel & distance
   double length;   // Length of current segment

   double velMax;   // Maximum velocity limit during this segment
   double accMax;   // Acceleration limit to us during this segment
   double decMax;   // Deceleration limit to us during this segment

   // These parameters will change as more segments are added
   // to the path.
   double velEnd;   // Maximum velocity at end of segment
   double velPeak;  // Maximum ending velocity possible based on earlier segments

   // Once I've determined the starting and ending velocity for a segment, I'll 
   // calculate the following information which will describe segment
   //   ta - Time to accelerate
   //   pa - Position (distance into path) at the end of ta
   //   va - Velocity (along path) at the end of ta
   //
   //   tv - Time to run at max velocity
   //   pv - Position at the end of (ta+tv)
   //
   //   td - Time to decelerate
   //   pd - Position at the end of (ta+tv+td)
   double ta,tv,td;
   double pa,pv,pd;
   double va;
   bool calculated;

   /**
    * Set the segment length.  This must be called in the 
    * constructor of the segment object before the segment
    * is added to a path.
    */
   void setLength( double L )
   {
      length = L;
   }

public:
   PathElement( void )
   {
      velPeak = velUp = velDn = velEnd = 0.0;
      velMax = accMax = decMax = 0.0;
      length = 0.0;
      next = prev = 0;
      calculated = false;
   }

   virtual ~PathElement()
   {
      Unlink();
   }

   void Unlink( void )
   {
      if( prev ) prev->next = next;
      if( next ) next->prev = prev;
      next = prev = 0;
   }

   /**
    * Add this segment to the end of the 
    * passed path.
    */
   void Add( PathElement *pe )
   {
      // Add this segment after the passed one.
      if( pe ) pe->next = this;
      this->prev = pe;

      // Find the peak velocity that could be 
      // reached at the end of this segment if
      // I didn't have to worry about stopping in 
      // the future.
      //
      // This is the previous segment's peak velocity
      // plus the increase I could provide based on 
      // this segment's acceleration & length.
      if( pe )
	 velPeak = pe->velPeak + velUp;
      else
	 velPeak = velUp;

      if( velPeak > getMaxVel() )
	 velPeak = getMaxVel();
   }

   PathElement *getPrev( void ){ return prev; }
   PathElement *getNext( void ){ return next; }

   double getVelStart( void )
   {
      PathElement *pe = getPrev();
      return (pe) ? pe->getVelEnd() : 0.0;
   }

   double getVelEnd( void )
   {
      return velEnd;
   }

   /**
    * Initialize some internal parameters based on 
    * segment length and various limits (velocity,
    * accel, decel).
    */
   void Init( double V, double A, double D )
   {
      // Save the constraints
      velMax = V; accMax = A;
      decMax = D;

      // Find the maximum amount that the velocity could increase
      // during this segment based only on the acceleration and
      // length of the segment.
      velUp = sqrt( 2.0 * A * length );

      // Find the maximum amount that the velocity could decrease
      // based only on decel and length.
      if( D <= 0.0 )
	 velDn = velUp;
      else
	 velDn = sqrt( 2.0 * D * length );
   }

   /// Return the length of the path element
   virtual double getLength( void ){ return length; }

   /// Get the maximum velocity limit used during this 
   /// segment.
   virtual double getMaxVel( void ){ return velMax; }

   /// Get the acceleration limit used during this segment.
   virtual double getMaxAcc( void ){ return accMax; }

   /// Get the deceleration limit used during this segment
   virtual double getMaxDec( void )
   {
      return (decMax <= 0.0) ? accMax : decMax;
   }

   /// Return the maximum starting velocity that this segment
   /// could handle based on it's current end velocity requirement,
   /// it's internal velocity limits, and it's acceleration.
   virtual double getMaxStartVel( void )
   {
      double v = velEnd + velDn;
      double max = getMaxVel();

      if( v > max ) v = max;
      return v;
   }

   /// Adjust the ending velocity limit for this segment.
   /// This is called as new segments are added after this one.
   /// @return true if the ending velocity changed, false otherwise.
   bool adjustEndVel( double v )
   {
      // If my running times have already been calculated, then
      // my ending velocity can no longer be adjusted.  This can 
      // happen if segments are being added to a running trajectory.
      if( calculated )
	 return false;

      // Limit the passed ending velocity to my local peak
      if( v > velPeak ) v = velPeak;

      // If the new ending velocity is greater then my current
      // value, then adjust it and return true.
      if( v > velEnd )
      {
	 velEnd = v;
	 return true;
      }

      // If the new ending velocity isn't any greater then my current
      // one, then I now have enough information to calculate my running
      // times.
      Calculate();
      return false;
   }

   // Calculate the running times used by this segment.  The running times
   // define the velocity profile of the segment.  They are calculated when
   // starting and ending velocities of the segment have been found.
   virtual bool Calculate( void )
   {
      if( calculated ) return true;
      calculated = true;

      double ve = velEnd;
      double vs = getVelStart();
      double P = length;
      double A = getMaxAcc();
      double D = getMaxDec();
      double V = getMaxVel();

      // See if we would exceed our velocity limit if we used used
      // an acceleration and deceleration time with no constant velocity.
      if( A*ve*ve + D*vs*vs + 2*A*D*P > V*V*(D+A) )
      {
	 V = getMaxVel();
	 ta = (V-vs) / A;
	 td = (V-ve) / D;

	 double remain = P - (vs*ta + ta*ta*A/2) - (ve*td + td*td*D/2);
	 tv = remain / V;
      }
      else
      {
	 double x = sqrt(D+A) * sqrt( A*ve*ve + D*vs*vs + 2*A*D*P );
	 ta = (x - (D+A)*vs) / (A*D+A*A);
	 td = (x - (D+A)*ve) / (A*D+D*D);
	 tv = 0.0;
      }

      // Calculate positions and velocities along path
      pa = vs * ta + ta*ta*A/2;
      va = vs + ta*A;
      pv = pa + tv*va;
      pd = pv + td*va - td*td*D/2;

      return false;
   }

   /**
     Return the total amount of time it will take me to run through this segment.
     @return time in seconds.
    */
   virtual double getDuration( void )
   {
      Calculate();
      return ta+tv+td;
   }

   /**
    * Give a current time into this segment (less then the 
    * segment duration), return the next largest time into the
    * segment with a fixed acceleration.
    *
    * @param t The starting time into the segment is passed in.
    *          The new time value is passed out.
    * @return true if a new time within the segment could be found
    */
   virtual bool getNextSegTime( double &t )
   {
      Calculate();
      t += MIN_PVT_TIME;

      if( t < ta )
      {
	 t = ta;
	 return false;
      }

      else if( t < (ta+tv) )
      {
	 t = ta+tv;
	 return false;
      }

      t = ta+tv+td;
      return true;
   }

   /**
     Get the position and velocity along this segment's path at 
     the specified time into the segment.
     @param t The time (seconds) into the segment.  Must not be
              greater then the segment's duration
     @param pos The position along the path is returned here.
     @param vel The velocity along the path is returned here.
    */
   virtual void getPathPos( double t, double &pos, double &vel )
   {
      Calculate();
      double vs = getVelStart();

      if( t <= ta )
      {
	 double A = getMaxAcc();
	 vel = vs + t*A;
	 pos = vs*t + t*t*A/2;
      }
      else if( t <= (ta+tv) )
      {
	 t -= ta;
	 vel = va;
	 pos = pa + va*t;
      }
      else
      {
	 double D = getMaxDec();
	 t -= (ta+tv);

	 vel = va - t*D;
	 pos = pv + va*t - t*t*D/2;
      }
   }

   virtual const Error *getTrjSeg( double t, uunit pos[], uunit vel[] ) = 0;
};

/**
 * Line segment path element.
 */
class LineSeg: public PathElement
{
   Point<PATH_MAX_DIMENSIONS> start;
   double direction, sinDir, cosDir;
public:

   LineSeg( PointN &start, double dir, double len )
   {
      this->start = start;
      direction = dir;
      setLength( len );

      sinDir = sin(direction);
      cosDir = cos(direction);
   }

   const Error *getTrjSeg( double t, uunit p[], uunit v[] )
   {
      double pos, vel;

      getPathPos( t, pos, vel );

      p[0] = start[0] + cosDir * pos;
      v[0] = cosDir * vel;

      if( start.getDim() > 1 )
      {
	 p[1] = start[1] + sinDir * pos;
	 v[1] = sinDir * vel;
      }
      return 0;
   }
};

class ArcSeg: public PathElement
{
   Point<PATH_MAX_DIMENSIONS> center;
   double radius, startAng, totAng;
   double velCentrip;
public:
   ArcSeg( PointN &ctr, double r, double start, double tot )
   {
      center = ctr;
      radius = r;
      startAng = start;
      totAng = tot;
      setLength( radius * fabs(tot) );
      velCentrip = -1;
   }

   // Limit max acceleration around a curve based on 
   // centripetal acceleration.
   double getMaxVel( void )
   {
      // Calculate a velocity limit based on centripetal accel
      if( velCentrip <= 0 )
      {
	 double A = getMaxAcc();
	 double D = getMaxDec();
	 if( D < A ) A = D;

	 velCentrip = sqrt( A * radius );
      }

      double max = PathElement::getMaxVel();
      if( velCentrip < max )
	 return velCentrip;
      return max;
   }


   const Error *getTrjSeg( double t, uunit p[], uunit v[] )
   {
      double pos, vel;

      getPathPos( t, pos, vel );

      pos /= radius;

      double ang;
      if( totAng < 0 )
      {
	 vel *= -1.0;
	 ang = startAng + pos;
      }
      else  
	 ang = startAng - pos;

      double sinAng = sin(ang);
      double cosAng = cos(ang);

      p[0] = center[0] + cosAng * radius;
      p[1] = center[1] + sinAng * radius;
      v[0] =  vel * sinAng;
      v[1] = -vel * cosAng;

      return 0;
   }

   bool getNextSegTime( double &t )
   {
      double oldT = t;
      bool ret = PathElement::getNextSegTime(t);

      // Force arc updates to happen at least every 10ms.
      // I should optimize this based on radius and speed.
      if( t-oldT > 0.01 )
      {
	 t = oldT + 0.01;
	 return false;
      }
      return ret;
   }
};