trjscurve_8cpp-source.html

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

<a name="l00142"></a>00142    V = maxVel * 0.1;
<a name="l00143"></a>00143    A = maxAcc * 10.0;
<a name="l00144"></a>00144    D = maxDec * 10.0;
<a name="l00145"></a>00145    J = maxJrk * 100.0;
<a name="l00146"></a>00146 <span class="preprocessor">#endif</span>
<a name="l00147"></a>00147 <span class="preprocessor"></span>
<a name="l00148"></a>00148    <span class="comment">/**************************************************</span>
<a name="l00149"></a>00149 <span class="comment">    * Assume positive moves for simplicity.  We'll fix</span>
<a name="l00150"></a>00150 <span class="comment">    * this in the end if necessary.</span>
<a name="l00151"></a>00151 <span class="comment">    **************************************************/</span>
<a name="l00152"></a>00152    <span class="keywordtype">bool</span> negMove = (P&lt;0);
<a name="l00153"></a>00153    <span class="keywordflow">if</span>( negMove )
<a name="l00154"></a>00154       P *= -1.0;
<a name="l00155"></a>00155 
<a name="l00156"></a>00156    <span class="keywordflow">if</span>( P==0 )
<a name="l00157"></a>00157    {
<a name="l00158"></a>00158       tj = tk = ta = td = tv = 0;
<a name="l00159"></a>00159       init = <span class="keyword">true</span>;
<a name="l00160"></a>00160       <span class="keywordflow">return</span> 0;
<a name="l00161"></a>00161    }
<a name="l00162"></a>00162 
<a name="l00163"></a>00163    <span class="comment">/**************************************************</span>
<a name="l00164"></a>00164 <span class="comment">    * Make sure A &lt;= D.  This reduces the number of </span>
<a name="l00165"></a>00165 <span class="comment">    * tests I need to do later.  I'll fix this at the</span>
<a name="l00166"></a>00166 <span class="comment">    * end of the calculation.</span>
<a name="l00167"></a>00167 <span class="comment">    **************************************************/</span>
<a name="l00168"></a>00168    <span class="keywordtype">bool</span> swapAD = (A &gt; D);
<a name="l00169"></a>00169 
<a name="l00170"></a>00170    <span class="keywordflow">if</span>( swapAD )
<a name="l00171"></a>00171    {
<a name="l00172"></a>00172       <span class="keywordtype">double</span> tmp;
<a name="l00173"></a>00173       tmp = A; A = D; D = tmp;
<a name="l00174"></a>00174    }
<a name="l00175"></a>00175 
<a name="l00176"></a>00176    <span class="comment">/**************************************************</span>
<a name="l00177"></a>00177 <span class="comment">    * I'll lower jerk to ensure that my jerk segments</span>
<a name="l00178"></a>00178 <span class="comment">    * are at least 1 millisecond long.</span>
<a name="l00179"></a>00179 <span class="comment">    **************************************************/</span>
<a name="l00180"></a>00180    <span class="keywordflow">if</span>( J &gt; A*1e3 ) J = A * 1e3;
<a name="l00181"></a>00181    <span class="keywordflow">if</span>( J &gt; V*1e6 ) J = V * 1e6;
<a name="l00182"></a>00182    <span class="keywordflow">if</span>( J &gt; P*5e8 ) J = P * 5e8;
<a name="l00183"></a>00183 
<a name="l00184"></a>00184    <span class="comment">/**************************************************</span>
<a name="l00185"></a>00185 <span class="comment">    * These are the key variables I'll need to find.</span>
<a name="l00186"></a>00186 <span class="comment">    *   tj = time to increase/decrease acceleration</span>
<a name="l00187"></a>00187 <span class="comment">    *   ta = time to run at constant accel</span>
<a name="l00188"></a>00188 <span class="comment">    *   tv = time to run at constant velocity</span>
<a name="l00189"></a>00189 <span class="comment">    *   td = time to run at constant decel</span>
<a name="l00190"></a>00190 <span class="comment">    *   tk = time to increase/decrease deceleration</span>
<a name="l00191"></a>00191 <span class="comment">    **************************************************/</span>
<a name="l00192"></a>00192 
<a name="l00193"></a>00193    <span class="comment">/**************************************************</span>
<a name="l00194"></a>00194 <span class="comment">    * See if a simple jerk limited move will handle </span>
<a name="l00195"></a>00195 <span class="comment">    * this.  In this case, the move will be 4 segments</span>
<a name="l00196"></a>00196 <span class="comment">    * each of the same time.</span>
<a name="l00197"></a>00197 <span class="comment">    **************************************************/</span>
<a name="l00198"></a>00198    tj = pow( P/(2*J), 1.0/3.0 );
<a name="l00199"></a>00199    <span class="keywordflow">if</span>( (J*tj &lt; A) &amp;&amp; (J*tj*tj &lt; V) )
<a name="l00200"></a>00200    {
<a name="l00201"></a>00201       ta = td = tv = 0;
<a name="l00202"></a>00202 
<a name="l00203"></a>00203       <span class="comment">// Adjust time &amp; Jerk to the next higher millisecond</span>
<a name="l00204"></a>00204       tk = tj = 0.001 * ceil( tj*1000 );
<a name="l00205"></a>00205 
<a name="l00206"></a>00206       J  = P / (2 * tj*tj*tj);
<a name="l00207"></a>00207    }
<a name="l00208"></a>00208 
<a name="l00209"></a>00209    <span class="comment">/**************************************************</span>
<a name="l00210"></a>00210 <span class="comment">    * We know we'll hit either the accel or velocity </span>
<a name="l00211"></a>00211 <span class="comment">    * limit.  See if the accel limit is too high.</span>
<a name="l00212"></a>00212 <span class="comment">    * If so, this must be a Jerk &amp; Velocity move.</span>
<a name="l00213"></a>00213 <span class="comment">    **************************************************/</span>
<a name="l00214"></a>00214    <span class="keywordflow">else</span> <span class="keywordflow">if</span>( J*V &lt; A*A )
<a name="l00215"></a>00215    {
<a name="l00216"></a>00216       ta = td = 0;
<a name="l00217"></a>00217       tj = sqrt( V/J );
<a name="l00218"></a>00218 
<a name="l00219"></a>00219       <span class="comment">// Adjust the times so they are integer multiples</span>
<a name="l00220"></a>00220       <span class="comment">// of milliseconds.  I'll adjust J &amp; V to compensate</span>
<a name="l00221"></a>00221       tk = tj = 0.001 * ceil( tj*1000 );
<a name="l00222"></a>00222       tv = P/V - 2*tj;
<a name="l00223"></a>00223 
<a name="l00224"></a>00224       tv = 0.001 * ceil( tv*1000 );
<a name="l00225"></a>00225 
<a name="l00226"></a>00226       V = P / (tv + 2*tj);
<a name="l00227"></a>00227       J = V / (tj*tj);
<a name="l00228"></a>00228    }
<a name="l00229"></a>00229 
<a name="l00230"></a>00230    <span class="keywordflow">else</span> 
<a name="l00231"></a>00231    {
<a name="l00232"></a>00232       <span class="comment">/**************************************************</span>
<a name="l00233"></a>00233 <span class="comment">       * At this point we know we will hit the accel limit.</span>
<a name="l00234"></a>00234 <span class="comment">       * We may or may not hit the velocity &amp; decel limits.</span>
<a name="l00235"></a>00235 <span class="comment">       * I'll start by assuming that I'll hit the velocity</span>
<a name="l00236"></a>00236 <span class="comment">       * limit.</span>
<a name="l00237"></a>00237 <span class="comment">       **************************************************/</span>
<a name="l00238"></a>00238       <span class="keywordtype">double</span> vj, vk;
<a name="l00239"></a>00239 
<a name="l00240"></a>00240       tj = A/J;
<a name="l00241"></a>00241       vj = A*tj / 2.0;
<a name="l00242"></a>00242 
<a name="l00243"></a>00243       ta = (V - 2*vj) / A;
<a name="l00244"></a>00244 
<a name="l00245"></a>00245       <span class="keywordflow">if</span>( J*V &lt; D*D )
<a name="l00246"></a>00246       {
<a name="l00247"></a>00247     td = 0.0;
<a name="l00248"></a>00248     tk = sqrt(V/J);
<a name="l00249"></a>00249     vk = V/2;
<a name="l00250"></a>00250       }
<a name="l00251"></a>00251       <span class="keywordflow">else</span>
<a name="l00252"></a>00252       {
<a name="l00253"></a>00253     tk = D/J;
<a name="l00254"></a>00254     td = (V-J*tk*tk) / D;
<a name="l00255"></a>00255     vk = D*tk / 2.0;
<a name="l00256"></a>00256       }
<a name="l00257"></a>00257 
<a name="l00258"></a>00258       <span class="comment">// Find the distance moved getting up to </span>
<a name="l00259"></a>00259       <span class="comment">// and down from V</span>
<a name="l00260"></a>00260       <span class="keywordtype">double</span> pa = tj*vj*2 + ta*vj + A*ta*tj + ta*ta*A/2.0;
<a name="l00261"></a>00261       <span class="keywordtype">double</span> pd = tk*vk*2 + td*vk + D*td*tk + td*td*D/2.0;
<a name="l00262"></a>00262 
<a name="l00263"></a>00263       <span class="comment">// If this distance is less then the total move,</span>
<a name="l00264"></a>00264       <span class="comment">// then I've found my solution.  Otherwise, the</span>
<a name="l00265"></a>00265       <span class="comment">// velocity limit isn't reached.</span>
<a name="l00266"></a>00266       <span class="keywordflow">if</span>( pa+pd &lt;= P )
<a name="l00267"></a>00267     tv = (P-pa-pd) / V;
<a name="l00268"></a>00268 
<a name="l00269"></a>00269       <span class="keywordflow">else</span>
<a name="l00270"></a>00270       {
<a name="l00271"></a>00271     <span class="comment">/**************************************************</span>
<a name="l00272"></a>00272 <span class="comment">     * At this point, we know we will hit the accel </span>
<a name="l00273"></a>00273 <span class="comment">     * limit, but not the velocity limit.  The only </span>
<a name="l00274"></a>00274 <span class="comment">     * question now is whether the decel limit will </span>
<a name="l00275"></a>00275 <span class="comment">     * be reached.</span>
<a name="l00276"></a>00276 <span class="comment">     *</span>
<a name="l00277"></a>00277 <span class="comment">     * I'll try no decel limit first.</span>
<a name="l00278"></a>00278 <span class="comment">     **************************************************/</span>
<a name="l00279"></a>00279     tv = 0.0;
<a name="l00280"></a>00280     tk = (sqrt( sqrt(2*P*A)*4*J +A*A ) - A) / (2*J);
<a name="l00281"></a>00281 
<a name="l00282"></a>00282     <span class="keywordflow">if</span>( J*tk &lt;= D )
<a name="l00283"></a>00283     {
<a name="l00284"></a>00284        ta = (J*tk*tk - J*tj*tj) / A;
<a name="l00285"></a>00285        td = 0.0;
<a name="l00286"></a>00286     }
<a name="l00287"></a>00287 
<a name="l00288"></a>00288     <span class="keywordflow">else</span>
<a name="l00289"></a>00289     {
<a name="l00290"></a>00290        tk = D/J;
<a name="l00291"></a>00291