path_8cpp-source.html

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

<a name="l00481"></a>00481    <a class="code" href="classPoint.html">Point&lt;PATH_MAX_DIMENSIONS&gt;</a> pos;
<a name="l00482"></a>00482    <span class="keywordtype">double</span> delayTime;
<a name="l00483"></a>00483 <span class="keyword">public</span>:
<a name="l00484"></a>00484    DelaySeg( <a class="code" href="classPointN.html">PointN</a> &amp;p, <span class="keywordtype">double</span> t )
<a name="l00485"></a>00485    {
<a name="l00486"></a>00486       pos = p;
<a name="l00487"></a>00487       delayTime = t;
<a name="l00488"></a>00488       setLength( 0.0 );
<a name="l00489"></a>00489    }
<a name="l00490"></a>00490    <span class="keyword">virtual</span> <span class="keywordtype">double</span> getMaxVel( <span class="keywordtype">void</span> ){ <span class="keywordflow">return</span> 0.0; }
<a name="l00491"></a>00491    <span class="keyword">virtual</span> <span class="keywordtype">bool</span> Calculate( <span class="keywordtype">void</span> )
<a name="l00492"></a>00492    {
<a name="l00493"></a>00493       <span class="keywordflow">if</span>( calculated ) <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00494"></a>00494       calculated = <span class="keyword">true</span>;
<a name="l00495"></a>00495       tv = delayTime;
<a name="l00496"></a>00496       ta = td = 0.0;
<a name="l00497"></a>00497       <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00498"></a>00498    }
<a name="l00499"></a>00499 
<a name="l00500"></a>00500    <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *getTrjSeg( <span class="keywordtype">double</span> t, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> p[], <a class="code" href="CML__Utils_8h.html#a11">uunit</a> v[] )
<a name="l00501"></a>00501    {
<a name="l00502"></a>00502       <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;pos.getDim(); i++ )
<a name="l00503"></a>00503       {
<a name="l00504"></a>00504     p[i] = pos[i];
<a name="l00505"></a>00505     v[i] = 0;
<a name="l00506"></a>00506       }
<a name="l00507"></a>00507       <span class="keywordflow">return</span> 0;
<a name="l00508"></a>00508    }
<a name="l00509"></a>00509 };
<a name="l00510"></a>00510 
<a name="l00511"></a>00511 <a class="code" href="classPath.html#a0">Path::Path</a>( <a class="code" href="CML__Utils_8h.html#a3">uint</a> d )
<a name="l00512"></a>00512 {
<a name="l00513"></a>00513    <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( d &lt;= PATH_MAX_DIMENSIONS );
<a name="l00514"></a>00514 
<a name="l00515"></a>00515    <span class="keywordflow">if</span>( d &gt; PATH_MAX_DIMENSIONS )
<a name="l00516"></a>00516       d = PATH_MAX_DIMENSIONS;
<a name="l00517"></a>00517    dim = d;
<a name="l00518"></a>00518    <a class="code" href="classPath.html#p0">maxVel</a> = 0.0;
<a name="l00519"></a>00519    <a class="code" href="classPath.html#p1">maxAcc</a> = 0.0;
<a name="l00520"></a>00520    <a class="code" href="classPath.html#p2">maxDec</a> = -1.0;
<a name="l00521"></a>00521    <a class="code" href="classPath.html#p3">first</a> = <a class="code" href="classPath.html#p4">last</a> = 0;
<a name="l00522"></a>00522    dirEnd = 0.0;
<a name="l00523"></a>00523    posEnd.<a class="code" href="classPoint.html#a4">setDim</a>(d);
<a name="l00524"></a>00524    posStart.<a class="code" href="classPoint.html#a4">setDim</a>(d);
<a name="l00525"></a>00525    <a class="code" href="classPath.html#a2">Reset</a>();
<a name="l00526"></a>00526 }
<a name="l00527"></a>00527 
<a name="l00528"></a>00528 <a class="code" href="classPath.html#a1">Path::~Path</a>( <span class="keywordtype">void</span> )
<a name="l00529"></a>00529 {
<a name="l00530"></a>00530    <span class="keywordflow">while</span>( <a class="code" href="classPath.html#p3">first</a> )
<a name="l00531"></a>00531    {
<a name="l00532"></a>00532       PathElement *pe = <a class="code" href="classPath.html#p3">first</a>;
<a name="l00533"></a>00533       <a class="code" href="classPath.html#p3">first</a> = pe-&gt;getNext();
<a name="l00534"></a>00534       <span class="keyword">delete</span> pe;
<a name="l00535"></a>00535    }
<a name="l00536"></a>00536 }
<a name="l00537"></a>00537 
<a name="l00538"></a>00538 <span class="keywordtype">void</span> <a class="code" href="classPath.html#a2">Path::Reset</a>( <span class="keywordtype">void</span> )
<a name="l00539"></a>00539 {
<a name="l00540"></a>00540    <a class="code" href="classPath.html#p5">crntSeg</a> = <a class="code" href="classPath.html#p3">first</a>;
<a name="l00541"></a>00541    <a class="code" href="classPath.html#p6">segTime</a> = 0;
<a name="l00542"></a>00542 }
<a name="l00543"></a>00543 
<a name="l00544"></a>00544 
<a name="l00545"></a>00545 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a3">Path::SetStartPos</a>( <a class="code" href="classPointN.html">PointN</a> &amp;p )
<a name="l00546"></a>00546 {
<a name="l00547"></a>00547    <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( p.<a class="code" href="classPointN.html#a0">getDim</a>() == <a class="code" href="classPath.html#a12">GetDim</a>() );
<a name="l00548"></a>00548    <span class="keywordflow">if</span>( p.<a class="code" href="classPointN.html#a0">getDim</a>() != <a class="code" href="classPath.html#a12">GetDim</a>() )
<a name="l00549"></a>00549       <span class="keywordflow">return</span> &amp;<a class="code" href="classPathError.html#s4">PathError::BadPoint</a>;
<a name="l00550"></a>00550 
<a name="l00551"></a>00551    posStart = p;
<a name="l00552"></a>00552    <span class="keywordflow">return</span> 0;
<a name="l00553"></a>00553 }
<a name="l00554"></a>00554 
<a name="l00555"></a>00555 <span class="comment"></span>
<a name="l00556"></a>00556 <span class="comment">/**</span>
<a name="l00557"></a>00557 <span class="comment">  Offset the passed array of relative positions based</span>
<a name="l00558"></a>00558 <span class="comment">  on the starting position set by the user.</span>
<a name="l00559"></a>00559 <span class="comment">  @p Array of positions to be adjusted</span>
<a name="l00560"></a>00560 <span class="comment"> */</span>
<a name="l00561"></a>00561 <span class="keywordtype">void</span> Path::OffsetPos( <span class="keywordtype">double</span> p[] )
<a name="l00562"></a>00562 {
<a name="l00563"></a>00563    <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;dim; i++ )
<a name="l00564"></a>00564       p[i] += posStart[i];
<a name="l00565"></a>00565 }
<a name="l00566"></a>00566 <span class="comment"></span>
<a name="l00567"></a>00567 <span class="comment">/** </span>
<a name="l00568"></a>00568 <span class="comment"> * Set a new velocity limit which will apply to any new</span>
<a name="l00569"></a>00569 <span class="comment"> * segments added to the path.</span>
<a name="l00570"></a>00570 <span class="comment"> *</span>
<a name="l00571"></a>00571 <span class="comment"> * The passed velocity must be &gt;= zero.</span>
<a name="l00572"></a>00572 <span class="comment"> *</span>
<a name="l00573"></a>00573 <span class="comment"> * @param v The velocity limit to use</span>
<a name="l00574"></a>00574 <span class="comment"> * @return An error object pointer or NULL on success</span>
<a name="l00575"></a>00575 <span class="comment"> */</span>
<a name="l00576"></a>00576 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a4">Path::SetVel</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> v )
<a name="l00577"></a>00577 {
<a name="l00578"></a>00578    <span class="keywordflow">if</span>( v &lt;= 0.0 ) <span class="keywordflow">return</span> &amp;<a class="code" href="classPathError.html#s0">PathError::BadVel</a>;
<a name="l00579"></a>00579    <a class="code" href="classPath.html#p0">maxVel</a> = v;
<a name="l00580"></a>00580    <span class="keywordflow">return</span> 0;
<a name="l00581"></a>00581 }
<a name="l00582"></a>00582 <span class="comment"></span>
<a name="l00583"></a>00583 <span class="comment">/** </span>
<a name="l00584"></a>00584 <span class="comment"> * Set a new acceleration limit which will apply to any new</span>
<a name="l00585"></a>00585 <span class="comment"> * segments added to the path.</span>
<a name="l00586"></a>00586 <span class="comment"> *</span>
<a name="l00587"></a>00587 <span class="comment"> * The passed acceleration value must be &gt;= zero.</span>
<a name="l00588"></a>00588 <span class="comment"> *</span>
<a name="l00589"></a>00589 <span class="comment"> * @param a The acceleration limit to use</span>
<a name="l00590"></a>00590 <span class="comment"> * @return An error object pointer or NULL on success</span>
<a name="l00591"></a>00591 <span class="comment"> */</span>
<a name="l00592"></a>00592 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a5">Path::SetAcc</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> a )
<a name="l00593"></a>00593 {
<a name="l00594"></a>00594    <span class="keywordflow">if</span>( a &lt;= 0.0 ) <span class="keywordflow">return</span> &amp;<a class="code" href="classPathError.html#s1">PathError::BadAcc</a>;
<a name="l00595"></a>00595    <a class="code" href="classPath.html#p1">maxAcc</a> = a;
<a name="l00596"></a>00596    <span class="keywordflow">return</span> 0;
<a name="l00597"></a>00597 }
<a name="l00598"></a>00598 <span class="comment"></span>
<a name="l00599"></a>00599 <span class="comment">/** </span>
<a name="l00600"></a>00600 <span class="comment"> * Set a new deceleration limit which will apply to any new</span>
<a name="l00601"></a>00601 <span class="comment"> * segments added to the path.</span>
<a name="l00602"></a>00602 <span class="comment"> *</span>
<a name="l00603"></a>00603 <span class="comment"> * If a positive deceleration is passed, then this will be used</span>
<a name="l00604"></a>00604 <span class="comment"> * as the deceleration limit.  If a zero or negative value is passed</span>
<a name="l00605"></a>00605 <span class="comment"> * then the acceleration value will be used for deceleration as well</span>
<a name="l00606"></a>00606 <span class="comment"> * as acceleration.</span>
<a name="l00607"></a>00607 <span class="comment"> *</span>
<a name="l00608"></a>00608 <span class="comment"> * @param d The deceleration limit to use</span>
<a name="l00609"></a>00609 <span class="comment"> * @return An error object pointer or NULL on success</span>
<a name="l00610"></a>00610 <span class="comment"> */</span>
<a name="l00611"></a>00611 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a6">Path::SetDec</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> d )
<a name="l00612"></a>00612 {
<a name="l00613"></a>00613    <a class="code" href="classPath.html#p2">maxDec</a> = d;
<a name="l00614"></a>00614    <span class="keywordflow">return</span> 0;
<a name="l00615"></a>00615 }
<a name="l00616"></a>00616 <span class="comment"></span>
<a name="l00617"></a>00617 <span class="comment">/**</span>
<a name="l00618"></a>00618 <span class="comment"> * Add a new segment to the end of this path.</span>
<a name="l00619"></a>00619 <span class="comment"> */</span>
<a name="l00620"></a>00620 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *Path::AddSegment( PathElement *e )
<a name="l00621"></a>00621 {
<a name="l00622"></a>00622    <span class="comment">// Check for uninitialized velocity and accelerations</span>
<a name="l00623"></a>00623    <span class="keywordflow">if</span>( <a class="code" href="classPath.html#p0">maxVel</a> &lt;= 0.0 ) <span class="keywordflow">return</span> &amp;<a class="code" href="classPathError.html#s2">PathError::VelNotInit</a>;
<a name="l00624"></a>00624    <span class="keywordflow">if</span>( <a class="code" href="classPath.html#p1">maxAcc</a> &lt;= 0.0 ) <span class="keywordflow">return</span> &amp;<a class="code" href="classPathError.html#s3">PathError::AccNotInit</a>;
<a name="l00625"></a>00625 
<a name="l00626"></a>00626    <span class="comment">// Initialize the new segment</span>
<a name="l00627"></a>00627    e-&gt;Init( <a class="code" href="classPath.html#p0">maxVel</a>, <a class="code" href="classPath.html#p1">maxAcc</a>, <a class="code" href="classPath.html#p2">maxDec</a> );
<a name="l00628"></a>00628 
<a name="l00629"></a>00629    <span class="comment">// Add this segment to the end of my path</span>
<a name="l00630"></a>00630    e-&gt;Add( <a class="code" href="classPath.html#p4">last</a> );
<a name="l00631"></a>00631    <a class="code" href="classPath.html#p4">last</a> = e;
<a name="l00632"></a>00632    <span class="keywordflow">if</span>( !<a class="code" href="classPath.html#p3">first</a> )
<a name="l00633"></a>00633    {
<a name="l00634"></a>00634       <a class="code" href="classPath.html#p3">first</a> = e;
<a name="l00635"></a>00635       <a class="code" href="classPath.html#a2">Reset</a>();
<a name="l00636"></a>00636    }
<a name="l00637"></a>00637 
<a name="l00638"></a>00638    <span class="comment">// Adjust the ending velocities of preceeding </span>
<a name="l00639"></a>00639    <span class="comment">// segments.</span>
<a name="l00640"></a>00640    <span class="keywordflow">while</span>( 1 )
<a name="l00641"></a>00641    {
<a name="l00642"></a>00642       <span class="keywordtype">double</span> v = e-&gt;getMaxStartVel();
<a name="l00643"></a>00643