path_8cpp-source.html

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

<a name="l00318"></a>00318    }
<a name="l00319"></a>00319 <span class="comment"></span>
<a name="l00320"></a>00320 <span class="comment">   /**</span>
<a name="l00321"></a>00321 <span class="comment">     Get the position and velocity along this segment's path at </span>
<a name="l00322"></a>00322 <span class="comment">     the specified time into the segment.</span>
<a name="l00323"></a>00323 <span class="comment">     @param t The time (seconds) into the segment.  Must not be</span>
<a name="l00324"></a>00324 <span class="comment">              greater then the segment's duration</span>
<a name="l00325"></a>00325 <span class="comment">     @param pos The position along the path is returned here.</span>
<a name="l00326"></a>00326 <span class="comment">     @param vel The velocity along the path is returned here.</span>
<a name="l00327"></a>00327 <span class="comment">    */</span>
<a name="l00328"></a>00328    <span class="keyword">virtual</span> <span class="keywordtype">void</span> getPathPos( <span class="keywordtype">double</span> t, <span class="keywordtype">double</span> &amp;pos, <span class="keywordtype">double</span> &amp;vel )
<a name="l00329"></a>00329    {
<a name="l00330"></a>00330       Calculate();
<a name="l00331"></a>00331       <span class="keywordtype">double</span> vs = getVelStart();
<a name="l00332"></a>00332 
<a name="l00333"></a>00333       <span class="keywordflow">if</span>( t &lt;= ta )
<a name="l00334"></a>00334       {
<a name="l00335"></a>00335     <span class="keywordtype">double</span> A = getMaxAcc();
<a name="l00336"></a>00336     vel = vs + t*A;
<a name="l00337"></a>00337     pos = vs*t + t*t*A/2;
<a name="l00338"></a>00338       }
<a name="l00339"></a>00339       <span class="keywordflow">else</span> <span class="keywordflow">if</span>( t &lt;= (ta+tv) )
<a name="l00340"></a>00340       {
<a name="l00341"></a>00341     t -= ta;
<a name="l00342"></a>00342     vel = va;
<a name="l00343"></a>00343     pos = pa + va*t;
<a name="l00344"></a>00344       }
<a name="l00345"></a>00345       <span class="keywordflow">else</span>
<a name="l00346"></a>00346       {
<a name="l00347"></a>00347     <span class="keywordtype">double</span> D = getMaxDec();
<a name="l00348"></a>00348     t -= (ta+tv);
<a name="l00349"></a>00349 
<a name="l00350"></a>00350     vel = va - t*D;
<a name="l00351"></a>00351     pos = pv + va*t - t*t*D/2;
<a name="l00352"></a>00352       }
<a name="l00353"></a>00353    }
<a name="l00354"></a>00354 
<a name="l00355"></a>00355    <span class="keyword">virtual</span> <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> pos[], <a class="code" href="CML__Utils_8h.html#a11">uunit</a> vel[] ) = 0;
<a name="l00356"></a>00356 };
<a name="l00357"></a>00357 <span class="comment"></span>
<a name="l00358"></a>00358 <span class="comment">/**</span>
<a name="l00359"></a>00359 <span class="comment"> * Line segment path element.</span>
<a name="l00360"></a>00360 <span class="comment"> */</span>
<a name="l00361"></a>00361 <span class="keyword">class </span>LineSeg: <span class="keyword">public</span> PathElement
<a name="l00362"></a>00362 {
<a name="l00363"></a>00363    <a class="code" href="classPoint.html">Point&lt;PATH_MAX_DIMENSIONS&gt;</a> start;
<a name="l00364"></a>00364    <span class="keywordtype">double</span> direction, sinDir, cosDir;
<a name="l00365"></a>00365 <span class="keyword">public</span>:
<a name="l00366"></a>00366 
<a name="l00367"></a>00367    LineSeg( <a class="code" href="classPointN.html">PointN</a> &amp;start, <span class="keywordtype">double</span> dir, <span class="keywordtype">double</span> len )
<a name="l00368"></a>00368    {
<a name="l00369"></a>00369       this-&gt;start = start;
<a name="l00370"></a>00370       direction = dir;
<a name="l00371"></a>00371       setLength( len );
<a name="l00372"></a>00372 
<a name="l00373"></a>00373       sinDir = sin(direction);
<a name="l00374"></a>00374       cosDir = cos(direction);
<a name="l00375"></a>00375    }
<a name="l00376"></a>00376 
<a name="l00377"></a>00377    <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="l00378"></a>00378    {
<a name="l00379"></a>00379       <span class="keywordtype">double</span> pos, vel;
<a name="l00380"></a>00380 
<a name="l00381"></a>00381       getPathPos( t, pos, vel );
<a name="l00382"></a>00382 
<a name="l00383"></a>00383       p[0] = start[0] + cosDir * pos;
<a name="l00384"></a>00384       v[0] = cosDir * vel;
<a name="l00385"></a>00385 
<a name="l00386"></a>00386       <span class="keywordflow">if</span>( start.getDim() &gt; 1 )
<a name="l00387"></a>00387       {
<a name="l00388"></a>00388     p[1] = start[1] + sinDir * pos;
<a name="l00389"></a>00389     v[1] = sinDir * vel;
<a name="l00390"></a>00390       }
<a name="l00391"></a>00391       <span class="keywordflow">return</span> 0;
<a name="l00392"></a>00392    }
<a name="l00393"></a>00393 };
<a name="l00394"></a>00394 
<a name="l00395"></a>00395 <span class="keyword">class </span>ArcSeg: <span class="keyword">public</span> PathElement
<a name="l00396"></a>00396 {
<a name="l00397"></a>00397    <a class="code" href="classPoint.html">Point&lt;PATH_MAX_DIMENSIONS&gt;</a> center;
<a name="l00398"></a>00398    <span class="keywordtype">double</span> radius, startAng, totAng;
<a name="l00399"></a>00399    <span class="keywordtype">double</span> velCentrip;
<a name="l00400"></a>00400 <span class="keyword">public</span>:
<a name="l00401"></a>00401    ArcSeg( <a class="code" href="classPointN.html">PointN</a> &amp;ctr, <span class="keywordtype">double</span> r, <span class="keywordtype">double</span> start, <span class="keywordtype">double</span> tot )
<a name="l00402"></a>00402    {
<a name="l00403"></a>00403       center = ctr;
<a name="l00404"></a>00404       radius = r;
<a name="l00405"></a>00405       startAng = start;
<a name="l00406"></a>00406       totAng = tot;
<a name="l00407"></a>00407       setLength( radius * fabs(tot) );
<a name="l00408"></a>00408       velCentrip = -1;
<a name="l00409"></a>00409    }
<a name="l00410"></a>00410 
<a name="l00411"></a>00411    <span class="comment">// Limit max acceleration around a curve based on </span>
<a name="l00412"></a>00412    <span class="comment">// centripetal acceleration.</span>
<a name="l00413"></a>00413    <span class="keywordtype">double</span> getMaxVel( <span class="keywordtype">void</span> )
<a name="l00414"></a>00414    {
<a name="l00415"></a>00415       <span class="comment">// Calculate a velocity limit based on centripetal accel</span>
<a name="l00416"></a>00416       <span class="keywordflow">if</span>( velCentrip &lt;= 0 )
<a name="l00417"></a>00417       {
<a name="l00418"></a>00418     <span class="keywordtype">double</span> A = getMaxAcc();
<a name="l00419"></a>00419     <span class="keywordtype">double</span> D = getMaxDec();
<a name="l00420"></a>00420     <span class="keywordflow">if</span>( D &lt; A ) A = D;
<a name="l00421"></a>00421 
<a name="l00422"></a>00422     velCentrip = sqrt( A * radius );
<a name="l00423"></a>00423       }
<a name="l00424"></a>00424 
<a name="l00425"></a>00425       <span class="keywordtype">double</span> max = PathElement::getMaxVel();
<a name="l00426"></a>00426       <span class="keywordflow">if</span>( velCentrip &lt; max )
<a name="l00427"></a>00427     <span class="keywordflow">return</span> velCentrip;
<a name="l00428"></a>00428       <span class="keywordflow">return</span> max;
<a name="l00429"></a>00429    }
<a name="l00430"></a>00430 
<a name="l00431"></a>00431 
<a name="l00432"></a>00432    <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="l00433"></a>00433    {
<a name="l00434"></a>00434       <span class="keywordtype">double</span> pos, vel;
<a name="l00435"></a>00435 
<a name="l00436"></a>00436       getPathPos( t, pos, vel );
<a name="l00437"></a>00437 
<a name="l00438"></a>00438       pos /= radius;
<a name="l00439"></a>00439 
<a name="l00440"></a>00440       <span class="keywordtype">double</span> ang;
<a name="l00441"></a>00441       <span class="keywordflow">if</span>( totAng &lt; 0 )
<a name="l00442"></a>00442       {
<a name="l00443"></a>00443     vel *= -1.0;
<a name="l00444"></a>00444     ang = startAng + pos;
<a name="l00445"></a>00445       }
<a name="l00446"></a>00446       <span class="keywordflow">else</span>  
<a name="l00447"></a>00447     ang = startAng - pos;
<a name="l00448"></a>00448 
<a name="l00449"></a>00449       <span class="keywordtype">double</span> sinAng = sin(ang);
<a name="l00450"></a>00450       <span class="keywordtype">double</span> cosAng = cos(ang);
<a name="l00451"></a>00451 
<a name="l00452"></a>00452       p[0] = center[0] + cosAng * radius;
<a name="l00453"></a>00453       p[1] = center[1] + sinAng * radius;
<a name="l00454"></a>00454       v[0] =  vel * sinAng;
<a name="l00455"></a>00455       v[1] = -vel * cosAng;
<a name="l00456"></a>00456 
<a name="l00457"></a>00457       <span class="keywordflow">return</span> 0;
<a name="l00458"></a>00458    }
<a name="l00459"></a>00459 
<a name="l00460"></a>00460    <span class="keywordtype">bool</span> getNextSegTime( <span class="keywordtype">double</span> &amp;t )
<a name="l00461"></a>00461    {
<a name="l00462"></a>00462       <span class="keywordtype">double</span> oldT = t;
<a name="l00463"></a>00463       <span class="keywordtype">bool</span> ret = PathElement::getNextSegTime(t);
<a name="l00464"></a>00464 
<a name="l00465"></a>00465       <span class="comment">// Force arc updates to happen at least every 10ms.</span>
<a name="l00466"></a>00466       <span class="comment">// I should optimize this based on radius and speed.</span>
<a name="l00467"></a>00467       <span class="keywordflow">if</span>( t-oldT &gt; 0.01 )
<a name="l00468"></a>00468       {
<a name="l00469"></a>00469     t = oldT + 0.01;
<a name="l00470"></a>00470     <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00471"></a>00471       }
<a name="l00472"></a>00472       <span class="keywordflow">return</span> ret;
<a name="l00473"></a>00473    }
<a name="l00474"></a>00474 };
<a name="l00475"></a>00475 <span class="comment"></span>
<a name="l00476"></a>00476 <span class="comment">/**</span>
<a name="l00477"></a>00477 <span class="comment"> * Path segment used to delay for a specified amount of time.</span>
<a name="l00478"></a>00478 <span class="comment"> */</span>
<a name="l00479"></a>00479 <span class="keyword">class </span>DelaySeg: <span class="keyword">public</span> PathElement
<a name="l00480"></a>00480 {