trjscurve_8cpp-source.html

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

<a name="l00442"></a>00442    <span class="keywordflow">while</span>( AdvanceSegment(tf) &amp;&amp; (remain &lt; 0.001) );
<a name="l00443"></a>00443 
<a name="l00444"></a>00444    <span class="keywordflow">return</span> 0;
<a name="l00445"></a>00445 }
<a name="l00446"></a>00446 
<a name="l00447"></a>00447 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00448"></a>00448 <span class="comment">/**</span>
<a name="l00449"></a>00449 <span class="comment">Move to the next s-curve segment with a non-zero time.  I also adjust my</span>
<a name="l00450"></a>00450 <span class="comment">local position, vel, etc values using the passed time.</span>
<a name="l00451"></a>00451 <span class="comment"></span>
<a name="l00452"></a>00452 <span class="comment">@param tf The time value used to advance pos, vel, acc in the new segment.</span>
<a name="l00453"></a>00453 <span class="comment">@return zero if at the end of the move, else non-zero.</span>
<a name="l00454"></a>00454 <span class="comment">*/</span>
<a name="l00455"></a>00455 <span class="comment">/***************************************************************************/</span>
<a name="l00456"></a>00456 <span class="keywordtype">int</span> TrjScurve::AdvanceSegment( <span class="keywordtype">double</span> &amp;tf )
<a name="l00457"></a>00457 {
<a name="l00458"></a>00458    <span class="comment">// Find the next segment with a non-zero time</span>
<a name="l00459"></a>00459    <span class="keywordflow">switch</span>( seg++ )
<a name="l00460"></a>00460    {
<a name="l00461"></a>00461       <span class="keywordflow">case</span> 0: remain = ta; j =  0; <span class="keywordflow">if</span>( remain &gt; 0.000001 ) <span class="keywordflow">break</span>; seg++;
<a name="l00462"></a>00462       <span class="keywordflow">case</span> 1: remain = tj; j = -J; <span class="keywordflow">break</span>;
<a name="l00463"></a>00463       <span class="keywordflow">case</span> 2: remain = tv; j =  0; <span class="keywordflow">if</span>( remain &gt; 0.000001 ) <span class="keywordflow">break</span>; seg++;
<a name="l00464"></a>00464       <span class="keywordflow">case</span> 3: remain = tk; j = -J; <span class="keywordflow">break</span>;
<a name="l00465"></a>00465       <span class="keywordflow">case</span> 4: remain = td; j =  0; <span class="keywordflow">if</span>( remain &gt; 0.000001 ) <span class="keywordflow">break</span>; seg++;
<a name="l00466"></a>00466       <span class="keywordflow">case</span> 5: remain = tk; j =  J; <span class="keywordflow">break</span>;
<a name="l00467"></a>00467       <span class="keywordflow">default</span>: <span class="keywordflow">return</span> 0;
<a name="l00468"></a>00468    }
<a name="l00469"></a>00469 
<a name="l00470"></a>00470    <span class="comment">// Advance the p,v,a values by the amount passed, or by</span>
<a name="l00471"></a>00471    <span class="comment">// the total time in this segment if it's less.</span>
<a name="l00472"></a>00472    <span class="keywordtype">double</span> t = (tf &gt; remain) ? remain : tf;
<a name="l00473"></a>00473 
<a name="l00474"></a>00474    p += v*t + a*t*t/2 + j*t*t*t/6;
<a name="l00475"></a>00475    v += a*t + j*t*t/2;
<a name="l00476"></a>00476    a += j*t;
<a name="l00477"></a>00477 
<a name="l00478"></a>00478    <span class="comment">// Reduce the total time left in this segment by the value used.</span>
<a name="l00479"></a>00479    remain -= t;
<a name="l00480"></a>00480    tf -= t;
<a name="l00481"></a>00481    <span class="keywordflow">return</span> 1;
<a name="l00482"></a>00482 }
<a name="l00483"></a>00483 
<a name="l00484"></a>00484 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00485"></a>00485 <span class="comment">/**</span>
<a name="l00486"></a>00486 <span class="comment">Default constructor for multi-axis s-curve trajectory.</span>
<a name="l00487"></a>00487 <span class="comment">*/</span>
<a name="l00488"></a>00488 <span class="comment">/***************************************************************************/</span>
<a name="l00489"></a>00489 LinkTrjScurve::LinkTrjScurve( <span class="keywordtype">void</span> ){}
<a name="l00490"></a>00490 
<a name="l00491"></a>00491 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00492"></a>00492 <span class="comment">/**</span>
<a name="l00493"></a>00493 <span class="comment">Calculate a multi-axis s-curve trajectory.  This function calculates the </span>
<a name="l00494"></a>00494 <span class="comment">straight line move between the two passed positions.</span>
<a name="l00495"></a>00495 <span class="comment">@param s The starting position</span>
<a name="l00496"></a>00496 <span class="comment">@param e The ending position</span>
<a name="l00497"></a>00497 <span class="comment">@param vel The max velocity</span>
<a name="l00498"></a>00498 <span class="comment">@param acc The max acceleration</span>
<a name="l00499"></a>00499 <span class="comment">@param dec The max deceleration</span>
<a name="l00500"></a>00500 <span class="comment">@param jrk The max jerk (rate of change of velocity)</span>
<a name="l00501"></a>00501 <span class="comment">@return A pointer to an error object, or NULL on success.</span>
<a name="l00502"></a>00502 <span class="comment">*/</span>
<a name="l00503"></a>00503 <span class="comment">/***************************************************************************/</span>
<a name="l00504"></a>00504 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *LinkTrjScurve::Calculate( <a class="code" href="classPointN.html">PointN</a> &amp;s, <a class="code" href="classPointN.html">PointN</a> &amp;e, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> vel, 
<a name="l00505"></a>00505                                        <a class="code" href="CML__Utils_8h.html#a11">uunit</a> acc, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> dec, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> jrk )
<a name="l00506"></a>00506 {
<a name="l00507"></a>00507    <span class="keywordtype">int</span> d = s.<a class="code" href="classPointN.html#a0">getDim</a>();
<a name="l00508"></a>00508 
<a name="l00509"></a>00509    <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( d &lt;= <a class="code" href="CML__Settings_8h.html#a6">CML_MAX_AMPS_PER_LINK</a> );
<a name="l00510"></a>00510 
<a name="l00511"></a>00511    <span class="keywordtype">double</span> dist = s.<a class="code" href="classPointN.html#a5">distance</a>( e );
<a name="l00512"></a>00512 
<a name="l00513"></a>00513    <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = trj.<a class="code" href="classTrjScurve.html#a3">Calculate</a>( dist, vel, acc, dec, jrk );
<a name="l00514"></a>00514    <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;
<a name="l00515"></a>00515 
<a name="l00516"></a>00516    <span class="keywordtype">double</span> invDist = (dist!=0) ? 1.0/dist : 0.0;
<a name="l00517"></a>00517 
<a name="l00518"></a>00518    start.<a class="code" href="classPoint.html#a4">setDim</a>( d );
<a name="l00519"></a>00519    <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;d; i++ )
<a name="l00520"></a>00520    {
<a name="l00521"></a>00521       start[i] = s[i];
<a name="l00522"></a>00522       scale[i] = (e[i] - s[i]) * invDist;
<a name="l00523"></a>00523    }
<a name="l00524"></a>00524 
<a name="l00525"></a>00525    <span class="keywordflow">return</span> 0;
<a name="l00526"></a>00526 }
<a name="l00527"></a>00527 
<a name="l00528"></a>00528 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00529"></a>00529 <span class="comment">/**</span>
<a name="l00530"></a>00530 <span class="comment">Start a new move using this trajectory.  The trajectory must have already</span>
<a name="l00531"></a>00531 <span class="comment">been calculated when this function is called.</span>
<a name="l00532"></a>00532 <span class="comment">@return A pointer to an error object, or NULL on success.</span>
<a name="l00533"></a>00533 <span class="comment">*/</span>
<a name="l00534"></a>00534 <span class="comment">/***************************************************************************/</span>
<a name="l00535"></a>00535 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classLinkTrjScurve.html#a3">LinkTrjScurve::StartNew</a>( <span class="keywordtype">void</span> )
<a name="l00536"></a>00536 {
<a name="l00537"></a>00537    <span class="keywordflow">return</span> trj.<a class="code" href="classTrjScurve.html#a5">StartNew</a>();
<a name="l00538"></a>00538 }
<a name="l00539"></a>00539 
<a name="l00540"></a>00540 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00541"></a>00541 <span class="comment">/**</span>
<a name="l00542"></a>00542 <span class="comment">Retrieve the next segment of this trajectory.  The positions &amp; velocities for</span>
<a name="l00543"></a>00543 <span class="comment">all axes are returned in the passed arrays.</span>
<a name="l00544"></a>00544 <span class="comment"></span>
<a name="l00545"></a>00545 <span class="comment">@param pos An array which will be filled with position information.  </span>
<a name="l00546"></a>00546 <span class="comment">@param vel An array which will be filled with velocity information.</span>
<a name="l00547"></a>00547 <span class="comment">@param time A reference to a variable where the time (milliseconds) will be</span>
<a name="l00548"></a>00548 <span class="comment">       returned.</span>
<a name="l00549"></a>00549 <span class="comment">@return A pointer to an error object, or NULL on success.</span>
<a name="l00550"></a>00550 <span class="comment">*/</span>
<a name="l00551"></a>00551 <span class="comment">/***************************************************************************/</span>
<a name="l00552"></a>00552 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classLinkTrjScurve.html#a5">LinkTrjScurve::NextSegment</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> pos[], <a class="code" href="CML__Utils_8h.html#a11">uunit</a> vel[], <a class="code" href="CML__Utils_8h.html#a6">uint8</a> &amp;time )
<a name="l00553"></a>00553 {
<a name="l00554"></a>00554    <a class="code" href="CML__Utils_8h.html#a11">uunit</a> p, v;
<a name="l00555"></a>00555 
<a name="l00556"></a>00556    <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = trj.<a class="code" href="classTrjScurve.html#a7">NextSegment</a>( p, v, time );
<a name="l00557"></a>00557    <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;
<a name="l00558"></a>00558 
<a name="l00559"></a>00559    <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;start.<a class="code" href="classPoint.html#a2">getDim</a>(); i++ )
<a name="l00560"></a>00560    {
<a name="l00561"></a>00561       pos[i] = start[i] + p*scale[i];
<a name="l00562"></a>00562       vel[i] = v*scale[i];
<a name="l00563"></a>00563    }
<a name="l00564"></a>00564    <span class="keywordflow">return</span> 0;
<a name="l00565"></a>00565 }
<a name="l00566"></a>00566 
<a name="l00567"></a>00567 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00568"></a>00568 <span class="comment">/**</span>
<a name="l00569"></a>00569 <span class="comment">Finish this trajectory. </span>
<a name="l00570"></a>00570 <span class="comment">*/</span>
<a name="l00571"></a>00571 <span class="comment">/***************************************************************************/</span>
<a name="l00572"></a>00572 <span class="keywordtype">void</span> <a class="code" href="classLinkTrjScurve.html#a4">LinkTrjScurve::Finish</a>( <span class="keywordtype">void</span> )
<a name="l00573"></a>00573 {
<a name="l00574"></a>00574    trj.<a class="code" href="classTrjScurve.html#a6">Finish</a>();
<a name="l00575"></a>00575 }
<a name="l00576"></a>00576 
<a name="l00577"></a>00577 <span class="preprocessor">#endif</span>
</pre></div><hr>
<address style="align: right;">
<small>Copley Motion Library, Copyright (c) 2002-2003
<a href="http://www.copleycontrols.com">
<img src="CCC_logo.gif" alt="Copley Controls Corp." align="middle" border=0 ></a>
</small>
</address>
</body>
</html>