amppvt_8cpp-source.html

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

<a name="l00276"></a>00276 
<a name="l00277"></a>00277    <span class="comment">// We expect the buffer to be empty at the start of the move.</span>
<a name="l00278"></a>00278    <span class="comment">// If it isn't we'll clear it.</span>
<a name="l00279"></a>00279    <span class="keywordflow">if</span>( !(stat &amp; <a class="code" href="AmpPVT_8cpp.html#a6">PVTSTAT_EMPTY</a>) )
<a name="l00280"></a>00280    {
<a name="l00281"></a>00281       err = <a class="code" href="classAmp.html#b8">PvtBufferFlush</a>();
<a name="l00282"></a>00282       <span class="keywordflow">if</span>( !err ) err = <a class="code" href="classAmp.html#z13_4">GetPvtBuffStat</a>( stat );
<a name="l00283"></a>00283       <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;
<a name="l00284"></a>00284    }
<a name="l00285"></a>00285 
<a name="l00286"></a>00286    <span class="comment">// See how many segments will fit in the buffer (which should be</span>
<a name="l00287"></a>00287    <span class="comment">// empty at this point).  I need at least 2 and would normally</span>
<a name="l00288"></a>00288    <span class="comment">// expect many more.  If for some reason it's less then 2, then</span>
<a name="l00289"></a>00289    <span class="comment">// I'll just return an error.  This really should never happen.</span>
<a name="l00290"></a>00290    <a class="code" href="CML__Utils_8h.html#a6">uint8</a> n = (<a class="code" href="CML__Utils_8h.html#a6">uint8</a>)((stat &amp; <a class="code" href="AmpPVT_8cpp.html#a4">PVTSTAT_FREECT</a>) &gt;&gt; 16);
<a name="l00291"></a>00291    <span class="keywordflow">if</span>( n &lt; 2 )
<a name="l00292"></a>00292       <span class="keywordflow">return</span> &amp;<a class="code" href="classAmpError.html#s12">AmpError::pvtBufferFull</a>;
<a name="l00293"></a>00293 
<a name="l00294"></a>00294    pvtBuffSize = n;
<a name="l00295"></a>00295 <span class="comment"></span>
<a name="l00296"></a>00296 <span class="comment">   /// Clear the PVT segment cache</span>
<a name="l00297"></a>00297 <span class="comment"></span>   pvtUseCache = <span class="keyword">false</span>;
<a name="l00298"></a>00298    pvtCache.<a class="code" href="classPvtSegCache.html#a1">Clear</a>();
<a name="l00299"></a>00299 <span class="comment"></span>
<a name="l00300"></a>00300 <span class="comment">   /// Make sure the trajectory object is ready to go</span>
<a name="l00301"></a>00301 <span class="comment"></span>   err = trj.<a class="code" href="classTrajectory.html#a0">StartNew</a>();
<a name="l00302"></a>00302    <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;
<a name="l00303"></a>00303 
<a name="l00304"></a>00304    <span class="comment">// Keep track of the first segment being sent</span>
<a name="l00305"></a>00305    pvtSegActive = pvtSegID = (stat &amp; <a class="code" href="AmpPVT_8cpp.html#a3">PVTSTAT_NEXTID</a>);
<a name="l00306"></a>00306 
<a name="l00307"></a>00307    <span class="comment">/**************************************************</span>
<a name="l00308"></a>00308 <span class="comment">    * Upload as many segments as possible.  I use an</span>
<a name="l00309"></a>00309 <span class="comment">    * SDO to upload the segments here, but will use a </span>
<a name="l00310"></a>00310 <span class="comment">    * PDO to upload the rest of the profile once we</span>
<a name="l00311"></a>00311 <span class="comment">    * start moving.  This simplifies my error handling</span>
<a name="l00312"></a>00312 <span class="comment">    * here when I can afford the overhead of the SDO.</span>
<a name="l00313"></a>00313 <span class="comment">    **************************************************/</span>
<a name="l00314"></a>00314    <a class="code" href="CML__Utils_8h.html#a6">uint8</a> segBuff[8];
<a name="l00315"></a>00315    <a class="code" href="CML__Utils_8h.html#a6">uint8</a> time;
<a name="l00316"></a>00316    <span class="keywordflow">for</span>( <a class="code" href="CML__Utils_8h.html#a6">uint8</a> i=0; i&lt;n; i++ )
<a name="l00317"></a>00317    {
<a name="l00318"></a>00318       <a class="code" href="CML__Utils_8h.html#a11">uunit</a> p,v;
<a name="l00319"></a>00319 
<a name="l00320"></a>00320       <span class="comment">// See if this is a PVT or PT segment</span>
<a name="l00321"></a>00321       <span class="keywordtype">bool</span> useVel = trj.<a class="code" href="classTrajectory.html#a2">UseVelocityInfo</a>();
<a name="l00322"></a>00322 
<a name="l00323"></a>00323       err = trj.<a class="code" href="classTrajectory.html#a3">NextSegment</a>( p, v, time );
<a name="l00324"></a>00324       <span class="keywordflow">if</span>( err )
<a name="l00325"></a>00325     <span class="keywordflow">break</span>;
<a name="l00326"></a>00326 
<a name="l00327"></a>00327       <a class="code" href="CML__Utils_8h.html#a9">int32</a> pos = <a class="code" href="classAmp.html#z15_10">PosUser2Load</a>(p);
<a name="l00328"></a>00328 
<a name="l00329"></a>00329       <a class="code" href="CML__Utils_8h.html#a9">int32</a> vel = 0;
<a name="l00330"></a>00330       <span class="keywordflow">if</span>( useVel ) vel = <a class="code" href="classAmp.html#z15_11">VelUser2Load</a>(v);
<a name="l00331"></a>00331 
<a name="l00332"></a>00332       <span class="comment">// If this is the first segment of the move, then </span>
<a name="l00333"></a>00333       <span class="comment">// make sure my initial position is known.  I really</span>
<a name="l00334"></a>00334       <span class="comment">// only need to do this if the position value is</span>
<a name="l00335"></a>00335       <span class="comment">// larger then 24 bits, otherwise I'll be sending it</span>
<a name="l00336"></a>00336       <span class="comment">// as an absolute value and won't need the previous</span>
<a name="l00337"></a>00337       <span class="comment">// position anyway.</span>
<a name="l00338"></a>00338       <span class="keywordflow">if</span>( useVel &amp;&amp; !i &amp;&amp; (pos &gt; 0x007FFFFF || -pos &gt; 0x007FFFFF) )
<a name="l00339"></a>00339       {
<a name="l00340"></a>00340     err = <a class="code" href="classAmp.html#b7">SetPvtInitialPos</a>( pos );
<a name="l00341"></a>00341     <span class="keywordflow">if</span>( err ) <span class="keywordflow">break</span>;
<a name="l00342"></a>00342     pvtSegID++;
<a name="l00343"></a>00343     n--;
<a name="l00344"></a>00344       }
<a name="l00345"></a>00345 
<a name="l00346"></a>00346       <span class="comment">// Format the segment</span>
<a name="l00347"></a>00347       <span class="keywordflow">if</span>( useVel )
<a name="l00348"></a>00348     err = <a class="code" href="classAmp.html#b5">FormatPvtSeg</a>( pos, vel, time, segBuff );
<a name="l00349"></a>00349       <span class="keywordflow">else</span>
<a name="l00350"></a>00350     err = <a class="code" href="classAmp.html#b6">FormatPtSeg</a>( pos, time, segBuff );
<a name="l00351"></a>00351       <span class="keywordflow">if</span>( err ) <span class="keywordflow">break</span>;
<a name="l00352"></a>00352 
<a name="l00353"></a>00353       <span class="comment">// Send it using an SDO</span>
<a name="l00354"></a>00354       <span class="comment">//    err = sdo.Download( OBJID_PVT_DATA, 0, 8, segBuff );</span>
<a name="l00355"></a>00355       <span class="comment">//    if( err ) break;</span>
<a name="l00356"></a>00356 
<a name="l00357"></a>00357       <span class="comment">// Send it using a PDO</span>
<a name="l00358"></a>00358       pvtPdo.<a class="code" href="classRPDO__Pvt.html#a2">Transmit</a>( segBuff );
<a name="l00359"></a>00359       pvtCache.<a class="code" href="classPvtSegCache.html#a2">AddSegment</a>( segBuff, pvtSegID, p );
<a name="l00360"></a>00360 
<a name="l00361"></a>00361       <span class="comment">// Update the segment ID counter and the last segment</span>
<a name="l00362"></a>00362       <span class="comment">// position info.</span>
<a name="l00363"></a>00363       pvtSegID++;
<a name="l00364"></a>00364       pvtLastPos = pos;
<a name="l00365"></a>00365 
<a name="l00366"></a>00366       <span class="comment">// If time is zero, this is the last segment in the move.</span>
<a name="l00367"></a>00367       <span class="keywordflow">if</span>( !time ) <span class="keywordflow">break</span>;
<a name="l00368"></a>00368    }
<a name="l00369"></a>00369 
<a name="l00370"></a>00370    <span class="comment">// Get the buffer status.  We'll process this later</span>
<a name="l00371"></a>00371    <span class="keywordflow">if</span>( !err ) err = <a class="code" href="classAmp.html#z13_4">GetPvtBuffStat</a>( stat );
<a name="l00372"></a>00372 
<a name="l00373"></a>00373    <span class="comment">// If an error occurred during the download, flush </span>
<a name="l00374"></a>00374    <span class="comment">// the buffer and return the error code.</span>
<a name="l00375"></a>00375    <span class="keywordflow">if</span>( err )
<a name="l00376"></a>00376    {
<a name="l00377"></a>00377       trj.<a class="code" href="classTrajectory.html#a1">Finish</a>();
<a name="l00378"></a>00378       <a class="code" href="classAmp.html#b8">PvtBufferFlush</a>();
<a name="l00379"></a>00379       <span class="keywordflow">return</span> err;
<a name="l00380"></a>00380    }
<a name="l00381"></a>00381 
<a name="l00382"></a>00382    <span class="comment">// If the whole profile hasn't been sent, keep a pointer</span>
<a name="l00383"></a>00383    <span class="comment">// to the trajectory object so I can spool it up to the </span>
<a name="l00384"></a>00384    <span class="comment">// amp as buffer space becomes available.</span>
<a name="l00385"></a>00385    <span class="keywordflow">if</span>( time )
<a name="l00386"></a>00386       pvtTrj = &amp;trj;
<a name="l00387"></a>00387    <span class="keywordflow">else</span>
<a name="l00388"></a>00388    {
<a name="l00389"></a>00389       trj.<a class="code" href="classTrajectory.html#a1">Finish</a>();
<a name="l00390"></a>00390       pvtTrj = 0;
<a name="l00391"></a>00391    }
<a name="l00392"></a>00392 
<a name="l00393"></a>00393    <span class="comment">// Process the current buffer status to handle any lost messages.</span>
<a name="l00394"></a>00394    PvtStatusUpdate( stat );
<a name="l00395"></a>00395 
<a name="l00396"></a>00396    <span class="comment">// Start the move if so requested</span>
<a name="l00397"></a>00397    <span class="keywordflow">if</span>( start )
<a name="l00398"></a>00398       err = <a class="code" href="classAmp.html#z13_1">StartPVT</a>();
<a name="l00399"></a>00399 
<a name="l00400"></a>00400    <span class="keywordflow">return</span> err;
<a name="l00401"></a>00401 }
<a name="l00402"></a>00402 
<a name="l00403"></a>00403 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00404"></a>00404 <span class="comment">/**</span>
<a name="l00405"></a>00405 <span class="comment">  Start a PVT move that has already been uploaded.</span>
<a name="l00406"></a>00406 <span class="comment">  @return A pointer to an error object, or NULL on success.</span>
<a name="l00407"></a>00407 <span class="comment">  */</span>
<a name="l00408"></a>00408 <span class="comment">/***************************************************************************/</span>
<a name="l00409"></a><a class="code" href="classAmp.html#z13_1">00409</a> <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classAmp.html#z13_1">Amp::StartPVT</a>( <span class="keywordtype">void</span> )
<a name="l00410"></a>00410 {
<a name="l00411"></a>00411    <span class="comment">// Start the move</span>
<a name="l00412"></a>00412    <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = <a class="code" href="classAmp.html#z10_7">StartMove</a>();
<a name="l00413"></a>00413 
<a name="l00414"></a>00414    <span class="keywordflow">if</span>( err &amp;&amp; pvtTrj )
<a name="l00415"></a>00415    {
<a name="l00416"></a>00416       pvtTrj-&gt;<a class="code" href="classTrajectory.html#a1">Finish</a>();
<a name="l00417"></a>00417       pvtTrj = 0;