path_8cpp-source.html

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

<a name="l00155"></a>00155 <span class="comment"></span>
<a name="l00156"></a>00156 <span class="comment">   /**</span>
<a name="l00157"></a>00157 <span class="comment">    * Initialize some internal parameters based on </span>
<a name="l00158"></a>00158 <span class="comment">    * segment length and various limits (velocity,</span>
<a name="l00159"></a>00159 <span class="comment">    * accel, decel).</span>
<a name="l00160"></a>00160 <span class="comment">    */</span>
<a name="l00161"></a>00161    <span class="keywordtype">void</span> Init( <span class="keywordtype">double</span> V, <span class="keywordtype">double</span> A, <span class="keywordtype">double</span> D )
<a name="l00162"></a>00162    {
<a name="l00163"></a>00163       <span class="comment">// Save the constraints</span>
<a name="l00164"></a>00164       velMax = V; accMax = A;
<a name="l00165"></a>00165       decMax = D;
<a name="l00166"></a>00166 
<a name="l00167"></a>00167       <span class="comment">// Find the maximum amount that the velocity could increase</span>
<a name="l00168"></a>00168       <span class="comment">// during this segment based only on the acceleration and</span>
<a name="l00169"></a>00169       <span class="comment">// length of the segment.</span>
<a name="l00170"></a>00170       velUp = sqrt( 2.0 * A * length );
<a name="l00171"></a>00171 
<a name="l00172"></a>00172       <span class="comment">// Find the maximum amount that the velocity could decrease</span>
<a name="l00173"></a>00173       <span class="comment">// based only on decel and length.</span>
<a name="l00174"></a>00174       <span class="keywordflow">if</span>( D &lt;= 0.0 )
<a name="l00175"></a>00175     velDn = velUp;
<a name="l00176"></a>00176       <span class="keywordflow">else</span>
<a name="l00177"></a>00177     velDn = sqrt( 2.0 * D * length );
<a name="l00178"></a>00178    }
<a name="l00179"></a>00179 <span class="comment"></span>
<a name="l00180"></a>00180 <span class="comment">   /// Return the length of the path element</span>
<a name="l00181"></a>00181 <span class="comment"></span>   <span class="keyword">virtual</span> <span class="keywordtype">double</span> getLength( <span class="keywordtype">void</span> ){ <span class="keywordflow">return</span> length; }
<a name="l00182"></a>00182 <span class="comment"></span>
<a name="l00183"></a>00183 <span class="comment">   /// Get the maximum velocity limit used during this </span>
<a name="l00184"></a>00184 <span class="comment">   /// segment.</span>
<a name="l00185"></a>00185 <span class="comment"></span>   <span class="keyword">virtual</span> <span class="keywordtype">double</span> getMaxVel( <span class="keywordtype">void</span> ){ <span class="keywordflow">return</span> velMax; }
<a name="l00186"></a>00186 <span class="comment"></span>
<a name="l00187"></a>00187 <span class="comment">   /// Get the acceleration limit used during this segment.</span>
<a name="l00188"></a>00188 <span class="comment"></span>   <span class="keyword">virtual</span> <span class="keywordtype">double</span> getMaxAcc( <span class="keywordtype">void</span> ){ <span class="keywordflow">return</span> accMax; }
<a name="l00189"></a>00189 <span class="comment"></span>
<a name="l00190"></a>00190 <span class="comment">   /// Get the deceleration limit used during this segment</span>
<a name="l00191"></a>00191 <span class="comment"></span>   <span class="keyword">virtual</span> <span class="keywordtype">double</span> getMaxDec( <span class="keywordtype">void</span> )
<a name="l00192"></a>00192    {
<a name="l00193"></a>00193       <span class="keywordflow">return</span> (decMax &lt;= 0.0) ? accMax : decMax;
<a name="l00194"></a>00194    }
<a name="l00195"></a>00195 <span class="comment"></span>
<a name="l00196"></a>00196 <span class="comment">   /// Return the maximum starting velocity that this segment</span>
<a name="l00197"></a>00197 <span class="comment">   /// could handle based on it's current end velocity requirement,</span>
<a name="l00198"></a>00198 <span class="comment">   /// it's internal velocity limits, and it's acceleration.</span>
<a name="l00199"></a>00199 <span class="comment"></span>   <span class="keyword">virtual</span> <span class="keywordtype">double</span> getMaxStartVel( <span class="keywordtype">void</span> )
<a name="l00200"></a>00200    {
<a name="l00201"></a>00201       <span class="keywordtype">double</span> v = velEnd + velDn;
<a name="l00202"></a>00202       <span class="keywordtype">double</span> max = getMaxVel();
<a name="l00203"></a>00203 
<a name="l00204"></a>00204       <span class="keywordflow">if</span>( v &gt; max ) v = max;
<a name="l00205"></a>00205       <span class="keywordflow">return</span> v;
<a name="l00206"></a>00206    }
<a name="l00207"></a>00207 <span class="comment"></span>
<a name="l00208"></a>00208 <span class="comment">   /// Adjust the ending velocity limit for this segment.</span>
<a name="l00209"></a>00209 <span class="comment">   /// This is called as new segments are added after this one.</span>
<a name="l00210"></a>00210 <span class="comment">   /// @return true if the ending velocity changed, false otherwise.</span>
<a name="l00211"></a>00211 <span class="comment"></span>   <span class="keywordtype">bool</span> adjustEndVel( <span class="keywordtype">double</span> v )
<a name="l00212"></a>00212    {
<a name="l00213"></a>00213       <span class="comment">// If my running times have already been calculated, then</span>
<a name="l00214"></a>00214       <span class="comment">// my ending velocity can no longer be adjusted.  This can </span>
<a name="l00215"></a>00215       <span class="comment">// happen if segments are being added to a running trajectory.</span>
<a name="l00216"></a>00216       <span class="keywordflow">if</span>( calculated )
<a name="l00217"></a>00217     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00218"></a>00218 
<a name="l00219"></a>00219       <span class="comment">// Limit the passed ending velocity to my local peak</span>
<a name="l00220"></a>00220       <span class="keywordflow">if</span>( v &gt; velPeak ) v = velPeak;
<a name="l00221"></a>00221 
<a name="l00222"></a>00222       <span class="comment">// If the new ending velocity is greater then my current</span>
<a name="l00223"></a>00223       <span class="comment">// value, then adjust it and return true.</span>
<a name="l00224"></a>00224       <span class="keywordflow">if</span>( v &gt; velEnd )
<a name="l00225"></a>00225       {
<a name="l00226"></a>00226     velEnd = v;
<a name="l00227"></a>00227     <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00228"></a>00228       }
<a name="l00229"></a>00229 
<a name="l00230"></a>00230       <span class="comment">// If the new ending velocity isn't any greater then my current</span>
<a name="l00231"></a>00231       <span class="comment">// one, then I now have enough information to calculate my running</span>
<a name="l00232"></a>00232       <span class="comment">// times.</span>
<a name="l00233"></a>00233       Calculate();
<a name="l00234"></a>00234       <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00235"></a>00235    }
<a name="l00236"></a>00236 
<a name="l00237"></a>00237    <span class="comment">// Calculate the running times used by this segment.  The running times</span>
<a name="l00238"></a>00238    <span class="comment">// define the velocity profile of the segment.  They are calculated when</span>
<a name="l00239"></a>00239    <span class="comment">// starting and ending velocities of the segment have been found.</span>
<a name="l00240"></a>00240    <span class="keyword">virtual</span> <span class="keywordtype">bool</span> Calculate( <span class="keywordtype">void</span> )
<a name="l00241"></a>00241    {
<a name="l00242"></a>00242       <span class="keywordflow">if</span>( calculated ) <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00243"></a>00243       calculated = <span class="keyword">true</span>;
<a name="l00244"></a>00244 
<a name="l00245"></a>00245       <span class="keywordtype">double</span> ve = velEnd;
<a name="l00246"></a>00246       <span class="keywordtype">double</span> vs = getVelStart();
<a name="l00247"></a>00247       <span class="keywordtype">double</span> P = length;
<a name="l00248"></a>00248       <span class="keywordtype">double</span> A = getMaxAcc();
<a name="l00249"></a>00249       <span class="keywordtype">double</span> D = getMaxDec();
<a name="l00250"></a>00250       <span class="keywordtype">double</span> V = getMaxVel();
<a name="l00251"></a>00251 
<a name="l00252"></a>00252       <span class="comment">// See if we would exceed our velocity limit if we used used</span>
<a name="l00253"></a>00253       <span class="comment">// an acceleration and deceleration time with no constant velocity.</span>
<a name="l00254"></a>00254       <span class="keywordflow">if</span>( A*ve*ve + D*vs*vs + 2*A*D*P &gt; V*V*(D+A) )
<a name="l00255"></a>00255       {
<a name="l00256"></a>00256     V = getMaxVel();
<a name="l00257"></a>00257     ta = (V-vs) / A;
<a name="l00258"></a>00258     td = (V-ve) / D;
<a name="l00259"></a>00259 
<a name="l00260"></a>00260     <span class="keywordtype">double</span> remain = P - (vs*ta + ta*ta*A/2) - (ve*td + td*td*D/2);
<a name="l00261"></a>00261     tv = remain / V;
<a name="l00262"></a>00262       }
<a name="l00263"></a>00263       <span class="keywordflow">else</span>
<a name="l00264"></a>00264       {
<a name="l00265"></a>00265     <span class="keywordtype">double</span> x = sqrt(D+A) * sqrt( A*ve*ve + D*vs*vs + 2*A*D*P );
<a name="l00266"></a>00266     ta = (x - (D+A)*vs) / (A*D+A*A);
<a name="l00267"></a>00267     td = (x - (D+A)*ve) / (A*D+D*D);
<a name="l00268"></a>00268     tv = 0.0;
<a name="l00269"></a>00269       }
<a name="l00270"></a>00270 
<a name="l00271"></a>00271       <span class="comment">// Calculate positions and velocities along path</span>
<a name="l00272"></a>00272       pa = vs * ta + ta*ta*A/2;
<a name="l00273"></a>00273       va = vs + ta*A;
<a name="l00274"></a>00274       pv = pa + tv*va;
<a name="l00275"></a>00275       pd = pv + td*va - td*td*D/2;
<a name="l00276"></a>00276 
<a name="l00277"></a>00277       <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00278"></a>00278    }
<a name="l00279"></a>00279 <span class="comment"></span>
<a name="l00280"></a>00280 <span class="comment">   /**</span>
<a name="l00281"></a>00281 <span class="comment">     Return the total amount of time it will take me to run through this segment.</span>
<a name="l00282"></a>00282 <span class="comment">     @return time in seconds.</span>
<a name="l00283"></a>00283 <span class="comment">    */</span>
<a name="l00284"></a>00284    <span class="keyword">virtual</span> <span class="keywordtype">double</span> getDuration( <span class="keywordtype">void</span> )
<a name="l00285"></a>00285    {
<a name="l00286"></a>00286       Calculate();
<a name="l00287"></a>00287       <span class="keywordflow">return</span> ta+tv+td;
<a name="l00288"></a>00288    }
<a name="l00289"></a>00289 <span class="comment"></span>
<a name="l00290"></a>00290 <span class="comment">   /**</span>
<a name="l00291"></a>00291 <span class="comment">    * Give a current time into this segment (less then the </span>
<a name="l00292"></a>00292 <span class="comment">    * segment duration), return the next largest time into the</span>
<a name="l00293"></a>00293 <span class="comment">    * segment with a fixed acceleration.</span>
<a name="l00294"></a>00294 <span class="comment">    *</span>
<a name="l00295"></a>00295 <span class="comment">    * @param t The starting time into the segment is passed in.</span>
<a name="l00296"></a>00296 <span class="comment">    *          The new time value is passed out.</span>
<a name="l00297"></a>00297 <span class="comment">    * @return true if a new time within the segment could be found</span>
<a name="l00298"></a>00298 <span class="comment">    */</span>
<a name="l00299"></a>00299    <span class="keyword">virtual</span> <span class="keywordtype">bool</span> getNextSegTime( <span class="keywordtype">double</span> &amp;t )
<a name="l00300"></a>00300    {
<a name="l00301"></a>00301       Calculate();
<a name="l00302"></a>00302       t += MIN_PVT_TIME;
<a name="l00303"></a>00303 
<a name="l00304"></a>00304       <span class="keywordflow">if</span>( t &lt; ta )
<a name="l00305"></a>00305       {
<a name="l00306"></a>00306     t = ta;
<a name="l00307"></a>00307     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00308"></a>00308       }
<a name="l00309"></a>00309 
<a name="l00310"></a>00310       <span class="keywordflow">else</span> <span class="keywordflow">if</span>( t &lt; (ta+tv) )
<a name="l00311"></a>00311       {
<a name="l00312"></a>00312     t = ta+tv;
<a name="l00313"></a>00313     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00314"></a>00314       }
<a name="l00315"></a>00315 
<a name="l00316"></a>00316       t = ta+tv+td;
<a name="l00317"></a>00317       <span class="keywordflow">return</span> <span class="keyword">true</span>;