Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

tc.c File Reference

#include <stdio.h>
#include "rcs_prnt.hh"
#include <math.h>
#include "posemath.h"
#include "emcpos.h"
#include "tc.h"
#include "rcsvers.hh"

Include dependency graph for tc.c:

Include dependency graph

Go to the source code of this file.

Defines

#define TC_VEL_EPSILON   0.0001
#define TC_SCALE_EPSILON   0.0001
#define TC_QUEUE_MARGIN   10

Functions

char __attribute__ ((unused)) ident[]="$Id
int tcSetCycleTime (TC_STRUCT *tc, double secs)
int tcSetLine (TC_STRUCT *tc, PmLine line, PmLine line_abc)
int tcSetCircle (TC_STRUCT *tc, PmCircle circle, PmLine line_abc)
int tcSetTVmax (TC_STRUCT *tc, double _vMax)
int tcSetRVmax (TC_STRUCT *tc, double _Rvmax)
int tcSetRAmax (TC_STRUCT *tc, double _WDotMax)
int tcSetVscale (TC_STRUCT *tc, double _vScale)
int tcSetTAmax (TC_STRUCT *tc, double _aMax)
int tcSetPremax (TC_STRUCT *tc, double vMax, double aMax)
int tcSetVlimit (TC_STRUCT *tc, double vLimit)
int tcSetId (TC_STRUCT *tc, int _id)
int tcGetId (TC_STRUCT *tc)
int tcSetTermCond (TC_STRUCT *tc, int cond)
int tcGetTermCond (TC_STRUCT *tc)
int tcRunCycle (TC_STRUCT *tc)
EmcPose tcGetPos (TC_STRUCT *tc)
EmcPose tcGetGoalPos (TC_STRUCT *tc)
double tcGetVel (TC_STRUCT *tc)
double tcGetAccel (TC_STRUCT *tc)
int tcGetTcFlag (TC_STRUCT *tc)
int tcIsDone (TC_STRUCT *tc)
int tcIsAccel (TC_STRUCT *tc)
int tcIsConst (TC_STRUCT *tc)
int tcIsDecel (TC_STRUCT *tc)
int tcIsPaused (TC_STRUCT *tc)
void tcPrint (TC_STRUCT *tc)
int tcqCreate (TC_QUEUE_STRUCT *tcq, int _size, TC_STRUCT *tcSpace)
int tcqDelete (TC_QUEUE_STRUCT *tcq)
int tcqInit (TC_QUEUE_STRUCT *tcq)
int tcqPut (TC_QUEUE_STRUCT *tcq, TC_STRUCT tc)
TC_STRUCT tcqGet (TC_QUEUE_STRUCT *tcq, int *status)
int tcqRemove (TC_QUEUE_STRUCT *tcq, int n)
int tcqLen (TC_QUEUE_STRUCT *tcq)
TC_STRUCT * tcqItem (TC_QUEUE_STRUCT *tcq, int n, int *status)
TC_STRUCT * tcqLast (TC_QUEUE_STRUCT *tcq, int *status)
int tcqFull (TC_QUEUE_STRUCT *tcq)
double tcRunPreCycle (const TC_STRUCT *tc)
int tcForceCycle (TC_STRUCT *tc, double ratio)
PmCartesian tcGetUnitCart (TC_STRUCT *tc)
int tcSetDout (TC_STRUCT *tc, unsigned char douts, unsigned char starts, unsigned char ends)

Define Documentation

#define TC_QUEUE_MARGIN   10
 

Definition at line 941 of file tc.c.

#define TC_SCALE_EPSILON   0.0001
 

#define TC_VEL_EPSILON   0.0001
 

Function Documentation

char __attribute__ (  (unused)    )  [static]
 

Definition at line 75 of file tc.c.

00075                                                   : tc.c,v 1.9 2001/08/17 22:05:41 proctor Exp $";
00076 
00077 /* FIXME-- this should be made part of TC_STRUCT, and extMotDout() added
00078    in external interface */
00079 static unsigned char doutbyte = 0;
00080 extern void extMotDout(unsigned char byte);
00081 
00082 #define TC_VEL_EPSILON 0.0001   /* number below which v is considered 0 */
00083 #define TC_SCALE_EPSILON 0.0001 /* number below which scale is considered 0 */
00084 
00085 int tcInit(TC_STRUCT *tc)
00086 {
00087   PmPose zero;
00088 
00089   if (0 == tc)
00090   {
00091     return -1;
00092   }
00093 
00094   tc->cycleTime = 0.0;
00095   tc->targetPos = 0.0;
00096   tc->vMax = 0.0;
00097   tc->vScale = 1.0;
00098   tc->aMax = 0.0;
00099   tc->preVMax = 0.0;
00100   tc->preAMax = 0.0;
00101   tc->vLimit = 0.0;
00102   tc->toGo = 0.0;
00103   tc->currentPos = 0.0;
00104   tc->currentVel = 0.0;
00105   tc->currentAccel = 0.0;
00106   tc->tcFlag = TC_IS_UNSET;
00107   tc->type = TC_LINEAR;         /* default is linear interpolation */
00108   tc->id = 0;
00109   tc->termCond = TC_TERM_COND_BLEND;
00110 
00111   tc->tmag=0.0; 
00112   tc->abc_mag=0.0;
00113   tc->tvMax=0.0;
00114   tc->taMax=0.0;
00115   tc->abc_vMax=0.0;
00116   tc->abc_aMax=0.0;
00117   tc->unitCart.x = tc->unitCart.y = tc->unitCart.z = 0.0;
00118   zero.tran.x = zero.tran.y = zero.tran.z = 0.0;
00119   zero.rot.s=1.0;
00120   zero.rot.x = zero.rot.y = zero.rot.z = 0.0;
00121 
00122   pmLineInit(&tc->line, zero, zero);
00123   pmLineInit(&tc->line_abc, zero, zero);
00124   /* since type is TC_LINEAR, don't need to set circle params */
00125 
00126   tc->douts = 0;
00127   tc->doutstarts = 0;
00128   tc->doutends = 0;
00129 
00130   return 0;
00131 }

int tcForceCycle (  TC_STRUCT *    tc,
double    ratio
) 
 

Definition at line 1022 of file tc.c.

01023 {
01024   double newPos;
01025   double newToGo;
01026   double newVel;
01027   double newAccel;
01028   int newTcFlag;
01029 
01030   if (0 == tc)
01031     {
01032       return -1;
01033     }
01034 
01035   newPos = ratio * tc->targetPos;
01036   newToGo = newPos - tc->currentPos;
01037 
01038   if (ratio >= 1.0)
01039   {
01040     newVel = 0.0;
01041     newAccel = 0.0;
01042     newTcFlag = TC_IS_DONE;
01043   }
01044   else
01045   {
01046     newVel = (newPos - tc->currentPos) / tc->cycleTime;
01047     newAccel = (newVel - tc->currentVel) / tc->cycleTime;
01048     if (newAccel > 0.0)
01049     {
01050       newTcFlag = TC_IS_ACCEL;
01051     }
01052     else if (newAccel < 0.0)
01053     {
01054       newTcFlag = TC_IS_DECEL;
01055     }
01056     else
01057     {
01058       newTcFlag = TC_IS_CONST;
01059     }
01060   }
01061 
01062   tc->currentPos = newPos;
01063   tc->toGo = newToGo;
01064   tc->currentVel = newVel;
01065   tc->currentAccel = newAccel;
01066   tc->tcFlag = newTcFlag;
01067 
01068   return 0;
01069 }

double tcGetAccel (  TC_STRUCT *    tc ) 
 

Definition at line 641 of file tc.c.

00642 {
00643   if (0 == tc)
00644   {
00645     return 0.0;
00646   }
00647 
00648   return tc->currentAccel;
00649 }

EmcPose tcGetGoalPos (  TC_STRUCT *    tc ) 
 

Definition at line 591 of file tc.c.

00592 {
00593   PmPose v;
00594   EmcPose ev;
00595 
00596   if (0 == tc)
00597   {
00598     ev.tran.x = ev.tran.y = ev.tran.z = 0.0;
00599     ev.a = ev.b = ev.c = 0.0;
00600     return ev;
00601   }
00602 
00603   if (tc->type == TC_LINEAR)
00604     {
00605       v = tc->line.end;
00606     }
00607   else if (tc->type == TC_CIRCULAR)
00608     {
00609       /* we don't save start or end vector in TC_STRUCT to save space.
00610          To get end, call pmCirclePoint with final angle. tcGetGoalPos()
00611          is called infrequently so this space-time tradeoff is done. If
00612          this function is called often, we should save end point in the
00613          PM_CIRCLE struct. This will increase all TC_STRUCTS but make
00614          tcGetGoalPos() run faster. */
00615       pmCirclePoint(&tc->circle, tc->circle.angle, &v);
00616     }
00617   else
00618     {
00619       v.tran.x = v.tran.y = v.tran.z = 0.0;
00620       v.rot.s = 1.0;
00621       v.rot.x = v.rot.y = v.rot.z = 0.0;
00622     }
00623   
00624   ev.tran  = v.tran;
00625   ev.a = tc->line_abc.end.tran.x;
00626   ev.b = tc->line_abc.end.tran.y;
00627   ev.c = tc->line_abc.end.tran.z;
00628   return ev;
00629 }

int tcGetId (  TC_STRUCT *    tc ) 
 

Definition at line 345 of file tc.c.

00346 {
00347   if (0 == tc)
00348     {
00349       return -1;
00350     }
00351 
00352   return tc->id;
00353 }

EmcPose tcGetPos (  TC_STRUCT *    tc ) 
 

Definition at line 536 of file tc.c.

00537 {
00538   EmcPose v;
00539   PmPose v1;
00540   PmPose v2;
00541 
00542   if (0 == tc)
00543     {
00544       v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
00545       v1.rot.s = 1.0;
00546       v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
00547       return v;
00548     }
00549 
00550   /* note: was if tc->targetPos <= 0.0 return basePos */
00551   if (tc->type == TC_LINEAR)
00552     {
00553       pmLinePoint(&tc->line, tc->currentPos, &v1);
00554     }
00555   else if (tc->type == TC_CIRCULAR)
00556     {
00557       pmCirclePoint(&tc->circle, tc->currentPos / tc->circle.radius, &v1);
00558     }
00559   else
00560     {
00561       v1.tran.x = v1.tran.y = v1.tran.z = 0.0;
00562       v1.rot.s = 1.0;
00563       v1.rot.x = v1.rot.y = v1.rot.z = 0.0;
00564     }
00565   v.tran = v1.tran;
00566   if(tc->abc_mag > 1e-6) 
00567     {
00568       if(tc->tmag > 1e-6 )
00569         {
00570           pmLinePoint(&tc->line_abc,(tc->currentPos *tc->abc_mag /tc->tmag),&v2);
00571           v.a = v2.tran.x; 
00572           v.b = v2.tran.y; 
00573           v.c = v2.tran.z; 
00574         }
00575       else
00576         {
00577           pmLinePoint(&tc->line_abc,tc->currentPos,&v2);
00578           v.a = v2.tran.x; 
00579           v.b = v2.tran.y; 
00580           v.c = v2.tran.z; 
00581         }
00582     }
00583   else
00584     {
00585       v.a = v.b = v.c = 0.0;
00586     }
00587           
00588   return v;
00589 }

int tcGetTcFlag (  TC_STRUCT *    tc ) 
 

Definition at line 651 of file tc.c.

00652 {
00653   if (0 == tc)
00654   {
00655     return TC_IS_UNSET;
00656   }
00657 
00658   return tc->tcFlag;
00659 }

int tcGetTermCond (  TC_STRUCT *    tc ) 
 

Definition at line 373 of file tc.c.

00374 {
00375   if (0 == tc)
00376     {
00377       return -1;
00378     }
00379 
00380   return tc->termCond;
00381 }

PmCartesian tcGetUnitCart (  TC_STRUCT *    tc ) 
 

Definition at line 1072 of file tc.c.

01073 {
01074   PmPose currentPose;
01075   PmCartesian radialCart;
01076   static const PmCartesian fake= {1.0,0.0,0.0};
01077 
01078   if(tc->type == TC_LINEAR)
01079     {
01080       pmCartCartSub(tc->line.end.tran,tc->line.start.tran,&tc->unitCart);
01081 #ifdef USE_PM_CART_NORM
01082       pmCartNorm(tc->unitCart,&tc->unitCart);
01083 #else    
01084       pmCartUnit(tc->unitCart,&tc->unitCart);
01085 #endif
01086       return(tc->unitCart);
01087     }
01088   else if(tc->type == TC_CIRCULAR)
01089     {
01090       pmCirclePoint(&tc->circle,tc->currentPos,&currentPose);
01091       pmCartCartSub(currentPose.tran, tc->circle.center, &radialCart);
01092       pmCartCartCross(tc->circle.normal, radialCart,&tc->unitCart);
01093 #ifdef USE_PM_CART_NORM
01094       pmCartNorm(tc->unitCart,&tc->unitCart);
01095 #else    
01096       pmCartUnit(tc->unitCart,&tc->unitCart);
01097 #endif
01098       return(tc->unitCart);
01099     }
01100   // It should never really get here.
01101   return fake;
01102 }

double tcGetVel (  TC_STRUCT *    tc ) 
 

Definition at line 631 of file tc.c.

00632 {
00633   if (0 == tc)
00634   {
00635     return 0.0;
00636   }
00637 
00638   return tc->currentVel;
00639 }

int tcIsAccel (  TC_STRUCT *    tc ) 
 

Definition at line 671 of file tc.c.

00672 {
00673   if (0 == tc)
00674   {
00675     return 0;
00676   }
00677 
00678   return (tc->tcFlag == TC_IS_ACCEL);
00679 }

int tcIsConst (  TC_STRUCT *    tc ) 
 

Definition at line 681 of file tc.c.

00682 {
00683   if (0 == tc)
00684   {
00685     return 0;
00686   }
00687 
00688   return (tc->tcFlag == TC_IS_CONST);
00689 }

int tcIsDecel (  TC_STRUCT *    tc ) 
 

Definition at line 691 of file tc.c.

00692 {
00693   if (0 == tc)
00694   {
00695     return 0;
00696   }
00697 
00698   return (tc->tcFlag == TC_IS_DECEL);
00699 }

int tcIsDone (  TC_STRUCT *    tc ) 
 

Definition at line 661 of file tc.c.

00662 {
00663   if (0 == tc)
00664   {
00665     return 0;
00666   }
00667 
00668   return (tc->tcFlag == TC_IS_DONE);
00669 }

int tcIsPaused (  TC_STRUCT *    tc ) 
 

Definition at line 701 of file tc.c.

00702 {
00703   if (0 == tc)
00704   {
00705     return 0;
00706   }
00707 
00708   return (tc->tcFlag == TC_IS_PAUSED);
00709 }

void tcPrint (  TC_STRUCT *    tc ) 
 

Definition at line 711 of file tc.c.

00712 {
00713 #ifndef NO_STDIO_SUPPORT
00714   if (0 == tc)
00715   {
00716     rcs_print("(null)\n");
00717     return;
00718   }
00719 
00720   rcs_print("cycleTime:    %f\n", tc->cycleTime);
00721   rcs_print("targetPos:    %f\n", tc->targetPos);
00722   rcs_print("vMax:         %f\n", tc->vMax);
00723   rcs_print("vLimit:       %f\n", tc->vLimit);
00724   rcs_print("vScale        %f\n", tc->vScale);
00725   rcs_print("aMax:         %f\n", tc->aMax);
00726   rcs_print("toGo:         %f\n", tc->toGo);
00727   rcs_print("currentPos:   %f\n", tc->currentPos);
00728   rcs_print("currentVel:   %f\n", tc->currentVel);
00729   rcs_print("currentAccel: %f\n", tc->currentAccel);
00730   rcs_print("tcFlag:       %s\n", tc->tcFlag == TC_IS_UNSET ? "UNSET" :
00731          tc->tcFlag == TC_IS_DONE ? "DONE" :
00732          tc->tcFlag == TC_IS_ACCEL ? "ACCEL" :
00733          tc->tcFlag == TC_IS_CONST ? "CONST" :
00734          tc->tcFlag == TC_IS_DECEL ? "DECEL" :
00735          tc->tcFlag == TC_IS_PAUSED ? "PAUSED" : "?");
00736   rcs_print("type:         %s\n", tc->type == TC_LINEAR ? "LINEAR" :
00737          tc->type == TC_CIRCULAR ? "CIRCULAR" : "?");
00738   rcs_print("id:           %d\n", tc->id);
00739 #endif /* STDIO_SUPPORT */
00740 }

int tcRunCycle (  TC_STRUCT *    tc ) 
 

Definition at line 383 of file tc.c.

00384 {
00385   double newPos;
00386   double newVel;
00387   double newAccel;
00388   double discr;
00389   int isScaleDecel;
00390   int oldTcFlag;
00391 
00392   if (0 == tc) {
00393     return -1;
00394   }
00395 
00396   /* save the old flag-- we'll use it when deciding to flag deceleration
00397      if we're scaling back velocity */
00398   oldTcFlag = tc->tcFlag;
00399 
00400   if (tc->tcFlag == TC_IS_DONE) {
00401     tc->currentVel = 0.0;
00402     tc->currentAccel = 0.0;
00403     return 0;
00404   }
00405 
00406   tc->toGo = tc->targetPos - tc->currentPos;
00407 
00408   if (tc->aMax <= 0.0 || tc->vMax <= 0.0 || tc->cycleTime <= 0.0) {
00409     return -1;
00410   }
00411 
00412   /* compute newvel = 0 limit first */
00413   discr = 0.5 * tc->cycleTime * tc->currentVel - tc->toGo;
00414   if (discr > 0.0) {
00415     newVel = 0.0;
00416   }
00417   else {
00418     discr = 0.25 * tc->cycleTime * tc->cycleTime - 2.0 / tc->aMax * discr;
00419     newVel = - 0.5 * tc->aMax * tc->cycleTime + tc->aMax * sqrt(discr);
00420   }
00421 
00422   if (tc->tcFlag == TC_IS_UNSET) {
00423     /* it's the start of this segment, so set any start output bits */
00424     if (tc->douts) {
00425       doutbyte |= (tc->douts & tc->doutstarts);
00426       doutbyte &= (~tc->douts | tc->doutstarts);
00427       extMotDout(doutbyte);
00428     }
00429   }
00430 
00431   if (newVel <= 0.0) {
00432     newVel = 0.0;
00433     newAccel = 0;
00434     newPos = tc->targetPos;
00435     tc->tcFlag = TC_IS_DONE;
00436     /* set any end output bits */
00437     if (tc->douts) {
00438       doutbyte |= (tc->douts & tc->doutends);
00439       doutbyte &= (~tc->douts | tc->doutends);
00440       extMotDout(doutbyte);
00441     }
00442   }
00443   else {
00444     /* clamp velocity to scaled max, and note if it's scaled
00445        back. This will cause a deceleration which we will NOT
00446        flag as a TC_IS_DECEL unless we were previously in
00447        a decel mode, since that would cause the next
00448        motion in the queue to begin to be planned. Make it
00449        TC_IS_CONST instead. */
00450     isScaleDecel = 0;
00451     if (newVel > (tc->vMax - tc->preVMax) * tc->vScale) {
00452       newVel = (tc->vMax - tc->preVMax) * tc->vScale;
00453       isScaleDecel = 1;
00454     }
00455 
00456     /* clamp scaled velocity against absolute limit */
00457     if (newVel > tc->vLimit) {
00458       newVel = tc->vLimit;
00459     }
00460 
00461     if (tc->type == TC_CIRCULAR) {
00462       if (newVel > pmSqrt(tc->aMax*tc->circle.radius)) {
00463         newVel = pmSqrt(tc->aMax*tc->circle.radius);
00464       }
00465     }
00466 
00467     /* calc resulting accel */
00468     newAccel = (newVel - tc->currentVel) / tc->cycleTime;
00469 
00470     /* clamp accel if necessary, and recalc velocity */
00471     /* also give credit for previous segment's decel, in preAMax,
00472        which is a negative value since it's a decel */
00473     if (newAccel > 0.0) {
00474       if (newAccel > tc->aMax - tc->preAMax) {
00475         newAccel = tc->aMax - tc->preAMax;
00476         /* if tc->preMax was calculated correctly this check is
00477            redundant.
00478            (Just because I'm paranoid doesn't mean they are not out to get me!) */ 
00479         if (newAccel < 0.0) {
00480           newAccel = 0.0;
00481         }
00482         newVel = tc->currentVel + newAccel * tc->cycleTime;
00483       }
00484     }
00485     else {
00486       if (newAccel < -tc->aMax) {
00487         newAccel = -tc->aMax;
00488         newVel = tc->currentVel + newAccel * tc->cycleTime;
00489       }
00490     }
00491 
00492 #ifdef A_CHANGE_MAX
00493     if (newAccel > A_CHANGE_MAX*tc->cycleTime+tc->currentAccel) {
00494       newAccel = A_CHANGE_MAX*tc->cycleTime + tc->currentAccel;
00495       newVel = tc->currentVel + newAccel * tc->cycleTime;
00496     }
00497     else if(newAccel < -A_CHANGE_MAX*tc->cycleTime+tc->currentAccel) {
00498       newAccel = -A_CHANGE_MAX*tc->cycleTime + tc->currentAccel;
00499       newVel = tc->currentVel + newAccel * tc->cycleTime;
00500     }
00501 #endif
00502 
00503     tc->toGo = (newVel + tc->currentVel) * 0.5 * tc->cycleTime;
00504     newPos = tc->currentPos + tc->toGo;
00505 
00506     if (newAccel < 0.0) {
00507       if (isScaleDecel && oldTcFlag != TC_IS_DECEL) {
00508         /* we're decelerating, but blending next move is not
00509            being done yet, so don't flag a decel. This will
00510            prevent premature blending */
00511         tc->tcFlag = TC_IS_ACCEL;
00512       }
00513       else {
00514         tc->tcFlag = TC_IS_DECEL;
00515       }
00516     }
00517     else if (newAccel > 0.0) {
00518       tc->tcFlag = TC_IS_ACCEL;
00519     }
00520     else if (newVel < TC_VEL_EPSILON &&
00521              tc->vScale < TC_SCALE_EPSILON) {
00522       tc->tcFlag = TC_IS_PAUSED;
00523     }
00524     else {
00525       tc->tcFlag = TC_IS_CONST;
00526     }
00527   }
00528 
00529   tc->currentPos = newPos;
00530   tc->currentVel = newVel;
00531   tc->currentAccel = newAccel;
00532 
00533   return 0;
00534 }

double tcRunPreCycle (  const TC_STRUCT *    tc ) 
 

Definition at line 979 of file tc.c.

00980 {
00981   TC_STRUCT preTc;
00982   double ratio;                 /* ratio of (new pos) / (target pos) */
00983 
00984   if (0 == tc)
00985     {
00986       return -1;
00987     }
00988 
00989   if (tc->targetPos <= 0.0)
00990   {
00991     /* already there */
00992     ratio = 1.0;
00993   }
00994   else
00995   {
00996     preTc = *tc;
00997     tcRunCycle(&preTc);
00998     ratio = preTc.currentPos / preTc.targetPos;
00999   }
01000 
01001   if (preTc.tcFlag == TC_IS_DONE)
01002   {
01003     /* if it'll be done, may as well return 100% */
01004     return 1.0;
01005   }
01006 
01007   return ratio;
01008 }

int tcSetCircle (  TC_STRUCT *    tc,
PmCircle    circle,
PmLine    line_abc
) 
 

Definition at line 219 of file tc.c.

00220 {
00221   if (0 == tc)
00222   {
00223     return -1;
00224   }
00225 
00226   tc->circle = circle;
00227   tc->line_abc = line_abc;
00228 
00229   /* for circular motion, path param is arc length */
00230   tc->tmag = tc->targetPos = circle.angle * circle.radius;
00231 
00232   tc->currentPos = 0.0;
00233   tc->type = TC_CIRCULAR;
00234 
00235   tc->aMax = tc->taMax;
00236   tc->vMax = tc->tvMax;
00237 
00238   pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &tc->abc_mag);
00239 
00240   return 0;
00241 }

int tcSetCycleTime (  TC_STRUCT *    tc,
double    secs
) 
 

Definition at line 133 of file tc.c.

00134 {
00135   if (secs <= 0.0 ||
00136       0 == tc)
00137   {
00138     return -1;
00139   }
00140 
00141   tc->cycleTime = secs;
00142 
00143   return 0;
00144 }

int tcSetDout (  TC_STRUCT *    tc,
unsigned char    douts,
unsigned char    starts,
unsigned char    ends
) 
 

Definition at line 1104 of file tc.c.

01105 {
01106   if (0 == tc) {
01107     return -1;
01108   }
01109 
01110   tc->douts = douts;
01111   tc->doutstarts = starts;
01112   tc->doutends = ends;
01113 
01114   return 0;
01115 }

int tcSetId (  TC_STRUCT *    tc,
int    id
) 
 

Definition at line 333 of file tc.c.

00334 {
00335   if (0 == tc)
00336     {
00337       return -1;
00338     }
00339 
00340   tc->id = _id;
00341 
00342   return 0;
00343 }

int tcSetLine (  TC_STRUCT *    tc,
PmLine    line,
PmLine    line_abc
) 
 

Definition at line 146 of file tc.c.

00147 {
00148   double tmag,abc_mag;
00149 
00150   if (0 == tc)
00151   {
00152     return -1;
00153   }
00154 
00155   tc->line = line;
00156   tc->line_abc = line_abc;
00157 
00158 
00159   /* set targetPos to be scalar difference */
00160   pmCartCartDisp(line.end.tran, line.start.tran, &tmag);
00161   pmCartCartDisp(line_abc.end.tran, line_abc.start.tran, &abc_mag);
00162 
00163   tc->abc_mag = abc_mag;
00164   tc->tmag = tmag;
00165   
00166   if(tc->abc_aMax <= 0.0 && tc->taMax > 0.0)
00167     {
00168       tc->abc_aMax = tc->taMax;
00169     }
00170 
00171   if(tc->abc_vMax <= 0.0 && tc->tvMax > 0.0)
00172     {
00173       tc->abc_vMax = tc->tvMax;
00174     }
00175 
00176   if(tc->tmag < 1e-6)
00177     {
00178       tc->aMax = tc->abc_aMax;
00179       tc->vMax = tc->abc_vMax;
00180       tc->targetPos = abc_mag;
00181     }
00182   else
00183     {
00184       if(0)
00185 #if 0
00186       if(tc->abc_mag > 1e-6)
00187 #endif
00188         {
00189           if(tc->abc_aMax * tmag/abc_mag < tc->taMax)
00190             {
00191               tc->aMax = tc->abc_aMax *tmag/abc_mag;
00192             }
00193           else
00194             {
00195               tc->aMax = tc->taMax;
00196             }      
00197           if(tc->abc_vMax * tmag/abc_mag < tc->tvMax)
00198             {
00199               tc->vMax = tc->abc_vMax * tmag /abc_mag;
00200             }
00201           else
00202             {
00203               tc->vMax = tc->tvMax;
00204             }
00205         }
00206       else
00207         {
00208           tc->aMax = tc->taMax;
00209           tc->vMax = tc->tvMax;
00210         }
00211       tc->targetPos = tmag;
00212     }
00213      
00214   tc->currentPos = 0.0;
00215   tc->type = TC_LINEAR;
00216   return 0;
00217 }

int tcSetPremax (  TC_STRUCT *    tc,
double    vmax,
double    amax
) 
 

Definition at line 308 of file tc.c.

00309 {
00310   if (0 == tc)
00311   {
00312     return -1;
00313   }
00314 
00315   tc->preVMax = vMax;
00316   tc->preAMax = aMax;
00317 
00318   return 0;
00319 }

int tcSetRAmax (  TC_STRUCT *    tc,
double    wmax
) 
 

Definition at line 269 of file tc.c.

00270 {
00271   if (_WDotMax < 0.0 ||
00272       0 == tc)
00273   {
00274     return -1;
00275   }
00276 
00277   tc->abc_aMax = _WDotMax;
00278 
00279   return 0;
00280 }

int tcSetRVmax (  TC_STRUCT *    tc,
double    vmax
) 
 

Definition at line 256 of file tc.c.

00257 {
00258   if (_Rvmax < 0.0 ||
00259       0 == tc)
00260   {
00261     return -1;
00262   }
00263 
00264   tc->abc_vMax = _Rvmax;
00265 
00266   return 0;
00267 }

int tcSetTAmax (  TC_STRUCT *    tc,
double    amax
) 
 

Definition at line 295 of file tc.c.

00296 {
00297   if (_aMax <= 0.0 ||
00298       0 == tc)
00299   {
00300     return -1;
00301   }
00302 
00303   tc->taMax = _aMax;
00304 
00305   return 0;
00306 }

int tcSetTVmax (  TC_STRUCT *    tc,
double    vmax
) 
 

Definition at line 243 of file tc.c.

00244 {
00245   if (_vMax < 0.0 ||
00246       0 == tc)
00247   {
00248     return -1;
00249   }
00250 
00251   tc->tvMax = _vMax;
00252 
00253   return 0;
00254 }

int tcSetTermCond (  TC_STRUCT *    tc,
int    cond
) 
 

Definition at line 355 of file tc.c.

00356 {
00357   if (0 == tc)
00358     {
00359       return -1;
00360     }
00361 
00362   if (cond != TC_TERM_COND_STOP &&
00363       cond != TC_TERM_COND_BLEND)
00364     {
00365       return -1;
00366     }
00367 
00368   tc->termCond = cond;
00369 
00370   return 0;
00371 }

int tcSetVlimit (  TC_STRUCT *    tc,
double    vlimit
) 
 

Definition at line 321 of file tc.c.

00322 {
00323   if (0 == tc)
00324   {
00325     return -1;
00326   }
00327 
00328   tc->vLimit = vLimit;
00329 
00330   return 0;
00331 }

int tcSetVscale (  TC_STRUCT *    tc,
double    vscale
) 
 

Definition at line 282 of file tc.c.

00283 {
00284   if (_vScale < 0.0 ||
00285       0 == tc)
00286   {
00287     return -1;
00288   }
00289 
00290   tc->vScale = _vScale;
00291 
00292   return 0;
00293 }

int tcqCreate (  TC_QUEUE_STRUCT *    tcq,
int    _size,
TC_STRUCT *    tcSpace
) 
 

Definition at line 744 of file tc.c.

00745 {
00746   if (_size <= 0 ||
00747       0 == tcq)
00748   {
00749     return -1;
00750   }
00751   else
00752   {
00753     tcq->queue = tcSpace;
00754     tcq->size = _size;
00755     tcq->_len = 0;
00756     tcq->start = tcq->end = 0;
00757     tcq->allFull = 0;
00758 
00759     if (0 == tcq->queue)
00760       {
00761         return -1;
00762       }
00763     return 0;
00764   }
00765 }

int tcqDelete (  TC_QUEUE_STRUCT *    tcq ) 
 

Definition at line 767 of file tc.c.

00768 {
00769   if (0 != tcq &&
00770       0 != tcq->queue)
00771   {
00772     /*    free(tcq->queue); */
00773     tcq->queue = 0;
00774   }
00775 
00776   return 0;
00777 }

int tcqFull (  TC_QUEUE_STRUCT *    tcq ) 
 

Definition at line 942 of file tc.c.

00943 {
00944   if (0 == tcq)
00945     {
00946       return 1;                 /* null queue is full, for safety */
00947     }
00948 
00949   /* call the queue full if the length is into the margin, so reduce
00950      the effect of a race condition where the appending process may
00951      not see the full status immediately and send another motion */
00952 
00953   if (tcq->size <= TC_QUEUE_MARGIN)
00954     {
00955       /* no margin available, so full means really all full */
00956       return tcq->allFull;
00957     }
00958 
00959   if (tcq->_len >= tcq->size - TC_QUEUE_MARGIN)
00960     {
00961       /* we're into the margin, so call it full */
00962       return 1;
00963     }
00964 
00965   /* we're not into the margin */
00966   return 0;
00967 }

TC_STRUCT tcqGet (  TC_QUEUE_STRUCT *    tcq,
int *    status
) 
 

Definition at line 826 of file tc.c.

00827 {
00828   TC_STRUCT tc;
00829 
00830   if ((0 == tcq) ||
00831       (0 == tcq->queue) ||              /* not initialized */
00832       ((tcq->start == tcq->end) && ! tcq->allFull)) /* empty queue */
00833   {
00834     if (0 != status)
00835     {
00836       *status = -1;             /* set status flag, if passed */
00837     }
00838     tc.cycleTime=0;
00839     tc.targetPos=0;             /* positive motion progession */
00840     tc.vMax=0;                  /* max velocity */
00841     tc.vScale=0;                /* scale factor for vMax */
00842     tc.aMax=0;                  /* max accel */
00843     tc.preVMax=0;               /* vel from previous blend */
00844     tc.preAMax=0;               /* decel (negative) from previous blend */
00845     tc.vLimit=0;                /* abs vel limit, including scale */
00846     tc.toGo=0;
00847     tc.currentPos=0;
00848     tc.currentVel=0;
00849     tc.currentAccel=0;
00850     tc.tcFlag=0;                        /* TC_IS_DONE,ACCEL,CONST,DECEL*/
00851     tc.type=0;                  /* TC_LINEAR, TC_CIRCULAR */
00852     tc.id=0;                    /* id for motion segment */
00853     tc.termCond=0;                      /* TC_END_STOP,BLEND */
00854     return tc;
00855   }
00856 
00857   /* get current int and set status for returning */
00858   if (0 != status)
00859   {
00860     *status = 0;
00861   }
00862   tc = tcq->queue[tcq->start];
00863 
00864   /* update start ptr and reset allFull flag and len */
00865   tcq->start = (tcq->start + 1) % tcq->size;
00866   tcq->allFull = 0;
00867   tcq->_len--;
00868 
00869   return tc;
00870 }

int tcqInit (  TC_QUEUE_STRUCT *    tcq ) 
 

Definition at line 779 of file tc.c.

00780 {
00781   if (0 == tcq)
00782   {
00783     return -1;
00784   }
00785 
00786   tcq->_len = 0;
00787   tcq->start = tcq->end = 0;
00788   tcq->allFull = 0;
00789 
00790   return 0;
00791 }

TC_STRUCT* tcqItem (  TC_QUEUE_STRUCT *    tcq,
int    n,
int *    status
) 
 

Definition at line 908 of file tc.c.

00909 {
00910   if ((0 == tcq) ||
00911       (0 == tcq->queue) ||      /* not initialized */
00912       (n < 0) ||
00913       (n >= tcq->_len))         /* n too large */
00914   {
00915     if (0 != status)
00916     {
00917       *status = -1;
00918     }
00919     return (TC_STRUCT *) 0;
00920   }
00921 
00922   if (0 != status)
00923   {
00924     *status = 0;
00925   }
00926   return &(tcq->queue[(tcq->start + n) % tcq->size]);
00927 }

TC_STRUCT* tcqLast (  TC_QUEUE_STRUCT *    tcq,
int *    status
) 
 

Definition at line 930 of file tc.c.

00931 {
00932   if (0 == tcq)
00933   {
00934     return (TC_STRUCT *) 0;
00935   }
00936 
00937   return tcqItem(tcq, tcq->_len - 1, status);
00938 }

int tcqLen (  TC_QUEUE_STRUCT *    tcq ) 
 

Definition at line 897 of file tc.c.

00898 {
00899   if (0 == tcq)
00900   {
00901     return -1;
00902   }
00903 
00904   return tcq->_len;
00905 }

int tcqPut (  TC_QUEUE_STRUCT *    tcq,
TC_STRUCT    tc
) 
 

Definition at line 794 of file tc.c.

00795 {
00796   /* check for initialized */
00797   if (0 == tcq ||
00798       0 == tcq->queue)
00799   {
00800     return -1;
00801   }
00802 
00803   /* check for allFull, so we don't overflow the queue */
00804   if (tcq->allFull)
00805   {
00806     return -1;
00807   }
00808 
00809   /* add it */
00810   tcq->queue[tcq->end] = tc;
00811   tcq->_len++;
00812 
00813   /* update end ptr, modulo size of queue */
00814   tcq->end = (tcq->end + 1) % tcq->size;
00815 
00816   /* set allFull flag if we're really full */
00817   if (tcq->end == tcq->start)
00818     {
00819       tcq->allFull = 1;
00820     }
00821 
00822   return 0;
00823 }

int tcqRemove (  TC_QUEUE_STRUCT *    tcq,
int    n
) 
 

Definition at line 873 of file tc.c.

00874 {
00875   if (n <= 0)
00876   {
00877     return 0;                   /* okay to remove 0 or fewer */
00878   }
00879 
00880   if ((0 == tcq) ||
00881       (0 == tcq->queue) ||              /* not initialized */
00882       ((tcq->start == tcq->end) && ! tcq->allFull) || /* empty queue */
00883       (n > tcq->_len))          /* too many requested */
00884   {
00885     return -1;
00886   }
00887 
00888   /* update start ptr and reset allFull flag and len */
00889   tcq->start = (tcq->start + n) % tcq->size;
00890   tcq->allFull = 0;
00891   tcq->_len -= n;
00892 
00893   return 0;
00894 }

Generated on Sun Dec 2 15:28:32 2001 for EMC by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001