amppvt_8cpp-source.html

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

<a name="l00560"></a>00560 
<a name="l00561"></a>00561     <span class="keywordflow">if</span>( ++pvtCacheID == pvtSegID )
<a name="l00562"></a>00562        pvtUseCache = <span class="keyword">false</span>;
<a name="l00563"></a>00563       }
<a name="l00564"></a>00564 
<a name="l00565"></a>00565       <span class="comment">// We need to get a new segment from the trajectory generator.</span>
<a name="l00566"></a>00566       <span class="comment">// If there isn't one then it means that we're done sending</span>
<a name="l00567"></a>00567       <span class="comment">// segments out, so we can just return.</span>
<a name="l00568"></a>00568       <span class="keywordflow">else</span> <span class="keywordflow">if</span>( !pvtTrj ) 
<a name="l00569"></a>00569     <span class="keywordflow">return</span>;
<a name="l00570"></a>00570 
<a name="l00571"></a>00571       <span class="keywordflow">else</span>
<a name="l00572"></a>00572       {
<a name="l00573"></a>00573     <span class="comment">// If the trajectory generator returns an error then I'll </span>
<a name="l00574"></a>00574     <span class="comment">// just return and try again next time.</span>
<a name="l00575"></a>00575     <a class="code" href="CML__Utils_8h.html#a6">uint8</a> time;
<a name="l00576"></a>00576     <a class="code" href="CML__Utils_8h.html#a11">uunit</a> p, v;
<a name="l00577"></a>00577 
<a name="l00578"></a>00578     <span class="comment">// See if this is a PVT or PT segment</span>
<a name="l00579"></a>00579     <span class="keywordtype">bool</span> useVel = pvtTrj-&gt;<a class="code" href="classTrajectory.html#a2">UseVelocityInfo</a>();
<a name="l00580"></a>00580 
<a name="l00581"></a>00581     <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = pvtTrj-&gt;<a class="code" href="classTrajectory.html#a3">NextSegment</a>( p, v, time );
<a name="l00582"></a>00582     <span class="keywordflow">if</span>( err )
<a name="l00583"></a>00583     {
<a name="l00584"></a>00584        <a class="code" href="CML_8h.html#a0">cml</a>.<a class="code" href="classCopleyMotionLibrary.html#a9">Warn</a>( <span class="stringliteral">"Amp %d PVT NextSegment returned %s\n"</span>, err-&gt;<a class="code" href="classError.html#a0">toString</a>() );
<a name="l00585"></a>00585        <span class="keywordflow">return</span>;
<a name="l00586"></a>00586     }
<a name="l00587"></a>00587 
<a name="l00588"></a>00588     <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="l00589"></a>00589     <a class="code" href="CML__Utils_8h.html#a9">int32</a> vel = 0;
<a name="l00590"></a>00590     <span class="keywordflow">if</span>( useVel ) vel = <a class="code" href="classAmp.html#z15_11">VelUser2Load</a>(v);
<a name="l00591"></a>00591 
<a name="l00592"></a>00592     <span class="comment">// Format the PVT segment</span>
<a name="l00593"></a>00593     <span class="keywordflow">if</span>( useVel )
<a name="l00594"></a>00594        err = <a class="code" href="classAmp.html#b5">FormatPvtSeg</a>( pos, vel, time, buff );
<a name="l00595"></a>00595     <span class="keywordflow">else</span>
<a name="l00596"></a>00596        err = <a class="code" href="classAmp.html#b6">FormatPtSeg</a>( pos, time, buff );
<a name="l00597"></a>00597 
<a name="l00598"></a>00598     <span class="comment">// If the format failed, this is a critical error.</span>
<a name="l00599"></a>00599     <span class="comment">// I'll flush the trajectory buffer which causes the</span>
<a name="l00600"></a>00600     <span class="comment">// trajectory to be aborted.</span>
<a name="l00601"></a>00601     <span class="keywordflow">if</span>( err )
<a name="l00602"></a>00602     {
<a name="l00603"></a>00603        <a class="code" href="classAmp.html#b8">PvtBufferFlush</a>( <span class="keyword">false</span> );
<a name="l00604"></a>00604        pvtTrj-&gt;<a class="code" href="classTrajectory.html#a1">Finish</a>();
<a name="l00605"></a>00605        pvtTrj = 0;
<a name="l00606"></a>00606        <span class="keywordflow">return</span>;
<a name="l00607"></a>00607     }
<a name="l00608"></a>00608 
<a name="l00609"></a>00609     <span class="comment">// Add this segment to my cache for later error handling</span>
<a name="l00610"></a>00610     pvtCache.<a class="code" href="classPvtSegCache.html#a2">AddSegment</a>( buff, pvtSegID, p );
<a name="l00611"></a>00611 
<a name="l00612"></a>00612     pvtSegID++;
<a name="l00613"></a>00613     pvtLastPos = pos;
<a name="l00614"></a>00614 
<a name="l00615"></a>00615     <span class="comment">// If this was the last segment in the trajectory (i.e. the</span>
<a name="l00616"></a>00616     <span class="comment">// time was zero), then discard the trajectory object.</span>
<a name="l00617"></a>00617     <span class="keywordflow">if</span>( !time )
<a name="l00618"></a>00618     {
<a name="l00619"></a>00619        pvtTrj-&gt;<a class="code" href="classTrajectory.html#a1">Finish</a>();
<a name="l00620"></a>00620        pvtTrj = 0;
<a name="l00621"></a>00621     }
<a name="l00622"></a>00622       }
<a name="l00623"></a>00623 
<a name="l00624"></a>00624       <span class="comment">// Send the segment using a PDO</span>
<a name="l00625"></a>00625       pvtPdo.<a class="code" href="classRPDO__Pvt.html#a2">Transmit</a>( buff );
<a name="l00626"></a>00626    }
<a name="l00627"></a>00627 
<a name="l00628"></a>00628    <span class="keywordflow">return</span>;
<a name="l00629"></a>00629 }
<a name="l00630"></a>00630 
<a name="l00631"></a>00631 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00632"></a>00632 <span class="comment">/**</span>
<a name="l00633"></a>00633 <span class="comment">  Add the passed segment to the cache.  Segments must be passed in order and </span>
<a name="l00634"></a>00634 <span class="comment">  with no gaps between ID numbers.  If this segment doesn't follow those rules</span>
<a name="l00635"></a>00635 <span class="comment">  then the cache will be cleared before the segment is added.</span>
<a name="l00636"></a>00636 <span class="comment">  @param seg Points to an array of 8 bytes which make up the segment to be added.</span>
<a name="l00637"></a>00637 <span class="comment">  The segment data is copied into the cache.  No copy of the pointer is </span>
<a name="l00638"></a>00638 <span class="comment">  kept locally.</span>
<a name="l00639"></a>00639 <span class="comment">  @param id The ID number of the passed segment.</span>
<a name="l00640"></a>00640 <span class="comment">  @param p The position corresponding to this segment.</span>
<a name="l00641"></a>00641 <span class="comment">  */</span>
<a name="l00642"></a>00642 <span class="comment">/***************************************************************************/</span>
<a name="l00643"></a><a class="code" href="classPvtSegCache.html#a2">00643</a> <span class="keywordtype">void</span> <a class="code" href="classPvtSegCache.html#a2">PvtSegCache::AddSegment</a>( <a class="code" href="CML__Utils_8h.html#a6">uint8</a> *seg, <a class="code" href="CML__Utils_8h.html#a8">uint16</a> <span class="keywordtype">id</span>, <a class="code" href="CML__Utils_8h.html#a11">uunit</a> p )
<a name="l00644"></a>00644 {
<a name="l00645"></a>00645    <span class="comment">// If the ID is not one greater then the last ID</span>
<a name="l00646"></a>00646    <span class="comment">// added to the cache, then clear it.  </span>
<a name="l00647"></a>00647    <span class="comment">// Segments must be added to the cache in order </span>
<a name="l00648"></a>00648    <span class="comment">// with no gaps.</span>
<a name="l00649"></a>00649 
<a name="l00650"></a>00650    <span class="keywordflow">if</span>( <span class="keywordtype">id</span> != oldest+ct )
<a name="l00651"></a>00651    {
<a name="l00652"></a>00652       <a class="code" href="classPvtSegCache.html#a1">Clear</a>();
<a name="l00653"></a>00653       oldest = id;
<a name="l00654"></a>00654    }
<a name="l00655"></a>00655 
<a name="l00656"></a>00656    <span class="comment">// Figure out where to put this one</span>
<a name="l00657"></a>00657    <span class="keywordtype">int</span> index;
<a name="l00658"></a>00658 
<a name="l00659"></a>00659    <span class="keywordflow">if</span>( ct &lt; <a class="code" href="CML__Amp_8h.html#a0">PVTCACHESIZE</a> )
<a name="l00660"></a>00660       index = ct++;
<a name="l00661"></a>00661    <span class="keywordflow">else</span>
<a name="l00662"></a>00662    {
<a name="l00663"></a>00663       oldest++;
<a name="l00664"></a>00664       index = top++;
<a name="l00665"></a>00665       <span class="keywordflow">if</span>( top == <a class="code" href="CML__Amp_8h.html#a0">PVTCACHESIZE</a> )
<a name="l00666"></a>00666     top = 0;
<a name="l00667"></a>00667    }
<a name="l00668"></a>00668 
<a name="l00669"></a>00669    <span class="comment">// Add the new segment</span>
<a name="l00670"></a>00670    <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;8; i++ )
<a name="l00671"></a>00671       cache[index][i] = seg[i];
<a name="l00672"></a>00672    pos[index] = p;
<a name="l00673"></a>00673 }
<a name="l00674"></a>00674 
<a name="l00675"></a>00675 <span class="comment">/***************************************************************************/</span><span class="comment"></span>
<a name="l00676"></a>00676 <span class="comment">/**</span>
<a name="l00677"></a>00677 <span class="comment">  Get the specified segment from the cache.  If the requested segment is </span>
<a name="l00678"></a>00678 <span class="comment">  available, it's contents will be copied to the passed pointer.</span>
<a name="l00679"></a>00679 <span class="comment">  @param seg A pointer to an array of 8 bytes where the segment data </span>
<a name="l00680"></a>00680 <span class="comment">  will be copied on success.</span>
<a name="l00681"></a>00681 <span class="comment">  @param id The ID number of the segment being requested</span>
<a name="l00682"></a>00682 <span class="comment">  @return true on success, false if the requested segment isn't available.</span>
<a name="l00683"></a>00683 <span class="comment">  */</span>
<a name="l00684"></a>00684 <span class="comment">/***************************************************************************/</span>
<a name="l00685"></a><a class="code" href="classPvtSegCache.html#a3">00685</a> <span class="keywordtype">bool</span> <a class="code" href="classPvtSegCache.html#a3">PvtSegCache::GetSegment</a>( <a class="code" href="CML__Utils_8h.html#a6">uint8</a> *seg, <a class="code" href="CML__Utils_8h.html#a8">uint16</a> <span class="keywordtype">id</span> )
<a name="l00686"></a>00686 {
<a name="l00687"></a>00687    <a class="code" href="CML__Utils_8h.html#a7">int16</a> index = <span class="keywordtype">id</span> - oldest;
<a name="l00688"></a>00688 
<a name="l00689"></a>00689    <span class="comment">// Fail if the index is outside my range</span>
<a name="l00690"></a>00690    <span class="keywordflow">if</span>( index &lt; 0 || index &gt;= ct )
<a name="l00691"></a>00691       <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00692"></a>00692 
<a name="l00693"></a>00693    index += top;
<a name="l00694"></a>00694    <span class="keywordflow">if</span>( index &gt;= <a class="code" href="CML__Amp_8h.html#a0">PVTCACHESIZE</a> )
<a name="l00695"></a>00695       index -= <a class="code" href="CML__Amp_8h.html#a0">PVTCACHESIZE</a>;
<a name="l00696"></a>00696 
<a name="l00697"></a>00697    <span class="keywordflow">for</span>( <span class="keywordtype">int</span> i=0; i&lt;8; i++ )
<a name="l00698"></a>00698       seg[i] = cache[index][i];
<a name="l00