trjscurve_8cpp-source.html

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

<a name="l00292"></a>00292        <span class="keywordtype">double</span> a = J*A*(D+A);
<a name="l00293"></a>00293        <span class="keywordtype">double</span> b = 3*A*A*D -2*A*D*D +2*A*A*A +3*A*D*D;
<a name="l00294"></a>00294        <span class="keywordtype">double</span> c = (A*A + D*D + 2*A*D) * A*A/J - 2*P*J*D;
<a name="l00295"></a>00295 
<a name="l00296"></a>00296        ta = (-b + sqrt(b*b -4*a*c)) / (2*a);
<a name="l00297"></a>00297        td = (J*tj*tj + A*ta - J*tk*tk)/D;
<a name="l00298"></a>00298     }
<a name="l00299"></a>00299       }
<a name="l00300"></a>00300    }
<a name="l00301"></a>00301 
<a name="l00302"></a>00302    <span class="comment">/**************************************************</span>
<a name="l00303"></a>00303 <span class="comment">    * If I previously swapped A &amp; D, fix that now.</span>
<a name="l00304"></a>00304 <span class="comment">    **************************************************/</span>
<a name="l00305"></a>00305    <span class="keywordflow">if</span>( swapAD )
<a name="l00306"></a>00306    {
<a name="l00307"></a>00307       <span class="keywordtype">double</span> tmp;
<a name="l00308"></a>00308       tmp = ta; ta = td; td = tmp;
<a name="l00309"></a>00309       tmp = tj; tj = tk; tk = tmp;
<a name="l00310"></a>00310       tmp =  A;  A =  D;  D = tmp;
<a name="l00311"></a>00311    }
<a name="l00312"></a>00312 
<a name="l00313"></a>00313    <span class="comment">/**************************************************</span>
<a name="l00314"></a>00314 <span class="comment">    * Adjust for negative moves as necessary</span>
<a name="l00315"></a>00315 <span class="comment">    **************************************************/</span>
<a name="l00316"></a>00316    <span class="keywordflow">if</span>( negMove )
<a name="l00317"></a>00317    {
<a name="l00318"></a>00318       P  *= -1.0;
<a name="l00319"></a>00319       J  *= -1.0;
<a name="l00320"></a>00320    }
<a name="l00321"></a>00321 
<a name="l00322"></a>00322    init = <span class="keyword">true</span>;
<a name="l00323"></a>00323    <span class="keywordflow">return</span> 0;
<a name="l00324"></a>00324 }
<a name="l00325"></a>00325 
<a name="l00326"></a>00326 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00327"></a>00327 <span class="comment">/**</span>
<a name="l00328"></a>00328 <span class="comment">Reset this object so it may be passed to an amplifier.  This will return an</span>
<a name="l00329"></a>00329 <span class="comment">error if the trajectory has not yet been calculated, or if it is currently </span>
<a name="l00330"></a>00330 <span class="comment">being sent to another amp.</span>
<a name="l00331"></a>00331 <span class="comment"></span>
<a name="l00332"></a>00332 <span class="comment">@return A pointer to an error object, or NULL on success</span>
<a name="l00333"></a>00333 <span class="comment">*/</span>
<a name="l00334"></a>00334 <span class="comment">/***************************************************************************/</span>
<a name="l00335"></a>00335 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classTrjScurve.html#a5">TrjScurve::StartNew</a>( <span class="keywordtype">void</span> )
<a name="l00336"></a>00336 {
<a name="l00337"></a>00337    <span class="keywordflow">if</span>( !init ) <span class="keywordflow">return</span> &amp;<a class="code" href="classScurveError.html#s1">ScurveError::NoCalc</a>;
<a name="l00338"></a>00338    <span class="keywordflow">if</span>( inUse ) <span class="keywordflow">return</span> &amp;<a class="code" href="classScurveError.html#s2">ScurveError::InUse</a>;
<a name="l00339"></a>00339 
<a name="l00340"></a>00340    inUse = <span class="keyword">true</span>;
<a name="l00341"></a>00341 
<a name="l00342"></a>00342    p = start;
<a name="l00343"></a>00343    v = a = 0;
<a name="l00344"></a>00344    j = J;
<a name="l00345"></a>00345    remain = tj;
<a name="l00346"></a>00346    seg = 0;
<a name="l00347"></a>00347 
<a name="l00348"></a>00348    <span class="keywordflow">return</span> 0;
<a name="l00349"></a>00349 }
<a name="l00350"></a>00350 
<a name="l00351"></a>00351 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00352"></a>00352 <span class="comment">/**</span>
<a name="l00353"></a>00353 <span class="comment">Notify the trajectory object that it is no longer in use.</span>
<a name="l00354"></a>00354 <span class="comment">*/</span>
<a name="l00355"></a>00355 <span class="comment">/***************************************************************************/</span>
<a name="l00356"></a>00356 <span class="keywordtype">void</span> <a class="code" href="classTrjScurve.html#a6">TrjScurve::Finish</a>( <span class="keywordtype">void</span> )
<a name="l00357"></a>00357 {
<a name="l00358"></a>00358    inUse = <span class="keyword">false</span>;
<a name="l00359"></a>00359 }
<a name="l00360"></a>00360 
<a name="l00361"></a>00361 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00362"></a>00362 <span class="comment">/**</span>
<a name="l00363"></a>00363 <span class="comment">Get the next PVT segment for this s-curve profile.</span>
<a name="l00364"></a>00364 <span class="comment">*/</span>
<a name="l00365"></a>00365 <span class="comment">/***************************************************************************/</span>
<a name="l00366"></a>00366 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classTrjScurve.html#a7">TrjScurve::NextSegment</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> &amp;pout, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> &amp;vout, <a class="code" href="CML__Utils_8h.html#a6">uint8</a> &amp;tout )
<a name="l00367"></a>00367 {
<a name="l00368"></a>00368    <span class="keywordflow">if</span>( !inUse ) <span class="keywordflow">return</span> &amp;<a class="code" href="classScurveError.html#s3">ScurveError::NotInUse</a>;
<a name="l00369"></a>00369 
<a name="l00370"></a>00370    <span class="comment">/**************************************************</span>
<a name="l00371"></a>00371 <span class="comment">    * We have previously calculate the position &amp; </span>
<a name="l00372"></a>00372 <span class="comment">    * velocity for this segment.  I'll set the output</span>
<a name="l00373"></a>00373 <span class="comment">    * values now.</span>
<a name="l00374"></a>00374 <span class="comment">    **************************************************/</span>
<a name="l00375"></a>00375 <span class="preprocessor">#ifdef CML_ENABLE_USER_UNITS</span>
<a name="l00376"></a>00376 <span class="preprocessor"></span>   pout = p;
<a name="l00377"></a>00377    vout = v;
<a name="l00378"></a>00378 <span class="preprocessor">#else</span>
<a name="l00379"></a>00379 <span class="preprocessor"></span>   <span class="keywordflow">if</span>( p &gt;= 0 ) pout = (<a class="code" href="CML__Utils_8h.html#a11">uunit</a>)(p + 0.5);
<a name="l00380"></a>00380    <span class="keywordflow">else</span>         pout = (<a class="code" href="CML__Utils_8h.html#a11">uunit</a>)(p - 0.5);
<a name="l00381"></a>00381 
<a name="l00382"></a>00382    <span class="keywordflow">if</span>( v &gt;= 0 ) vout = (<a class="code" href="CML__Utils_8h.html#a11">uunit</a>)(10.0 * v + 0.5);
<a name="l00383"></a>00383    <span class="keywordflow">else</span>         vout = (<a class="code" href="CML__Utils_8h.html#a11">uunit</a>)(10.0 * v - 0.5);
<a name="l00384"></a>00384 <span class="preprocessor">#endif</span>
<a name="l00385"></a>00385 <span class="preprocessor"></span>
<a name="l00386"></a>00386    <span class="keywordflow">if</span>( seg == 7 )
<a name="l00387"></a>00387    {
<a name="l00388"></a>00388       tout = 0;
<a name="l00389"></a>00389       <span class="keywordflow">return</span> 0;
<a name="l00390"></a>00390    }
<a name="l00391"></a>00391 
<a name="l00392"></a>00392    <span class="keywordtype">bool</span> eos = <span class="keyword">false</span>;
<a name="l00393"></a>00393    <span class="keywordtype">double</span> t;
<a name="l00394"></a>00394 
<a name="l00395"></a>00395    <span class="keywordflow">if</span>( remain &gt; 0.510 )
<a name="l00396"></a>00396    {
<a name="l00397"></a>00397       t = 0.255;
<a name="l00398"></a>00398       tout = 255;
<a name="l00399"></a>00399    }
<a name="l00400"></a>00400 
<a name="l00401"></a>00401    <span class="keywordflow">else</span> <span class="keywordflow">if</span>( remain &gt; 0.254 )
<a name="l00402"></a>00402    {
<a name="l00403"></a>00403       tout = (<a class="code" href="CML__Utils_8h.html#a6">uint8</a>)floor(500*remain);
<a name="l00404"></a>00404       t = 0.001 * tout;
<a name="l00405"></a>00405    }
<a name="l00406"></a>00406 
<a name="l00407"></a>00407    <span class="keywordflow">else</span>
<a name="l00408"></a>00408    {
<a name="l00409"></a>00409       eos = <span class="keyword">true</span>;
<a name="l00410"></a>00410       tout = (<a class="code" href="CML__Utils_8h.html#a6">uint8</a>)floor(remain*1000 + 0.01);
<a name="l00411"></a>00411       t = remain;
<a name="l00412"></a>00412    }
<a name="l00413"></a>00413 
<a name="l00414"></a>00414    p += v*t + a*t*t/2 + j*t*t*t/6;
<a name="l00415"></a>00415    v += a*t + j*t*t/2;
<a name="l00416"></a>00416    a += j*t;
<a name="l00417"></a>00417 
<a name="l00418"></a>00418    remain -= t;
<a name="l00419"></a>00419    <span class="keywordflow">if</span>( remain &lt; 0 ) remain = 0;
<a name="l00420"></a>00420 
<a name="l00421"></a>00421    <span class="comment">// We're done if that wasn't the end of a segment.</span>
<a name="l00422"></a>00422    <span class="keywordflow">if</span>( !eos )
<a name="l00423"></a>00423       <span class="keywordflow">return</span> 0;
<a name="l00424"></a>00424 
<a name="l00425"></a>00425    <span class="comment">// There's a good chance that the previous segment time </span>
<a name="l00426"></a>00426    <span class="comment">// wasn't an even number of milliseconds.  Find the amount</span>
<a name="l00427"></a>00427    <span class="comment">// of time I went over the millisecond mark.</span>
<a name="l00428"></a>00428    <span class="keywordtype">double</span> ms;
<a name="l00429"></a>00429    <span class="keywordtype">double</span> f = modf( 1000*t, &amp;ms );
<a name="l00430"></a>00430    <span class="keywordtype">double</span> tf = 0;
<a name="l00431"></a>00431 
<a name="l00432"></a>00432    <span class="comment">// Increment my output time if I didn't end on an even millisecond.</span>
<a name="l00433"></a>00433    <span class="keywordflow">if</span>( f &gt; 0.001 )
<a name="l00434"></a>00434    {
<a name="l00435"></a>00435       tf = 0.001 * (1.0 - f);
<a name="l00436"></a>00436       tout++;
<a name="l00437"></a>00437    }
<a name="l00438"></a>00438 
<a name="l00439"></a>00439    <span class="comment">// Now, keep advancing to the next segment until I either come</span>
<a name="l00440"></a>00440    <span class="comment">// to the end of the move, or find a segment with more then 1ms</span>
<a name="l00441"></a>00441    <span class="comment">// available.</span>