tc.h Source File

00001 #ifndef TC_H
00002 #define TC_H
00003 
00004 /*
00005   tc.h
00006 
00007   Discriminate-based trajectory planning
00008 
00009   Modification history:
00010 
00011   17-Aug-2001  FMP added aout,dout motion IO
00012   17-Nov-2000 WPS added tcGetUnitCart(), wMax,wDotMax,rmag,tmag, etc.
00013   13-Mar-2000 WPS added unused attribute to ident to avoid 'defined but 
00014   not used' compiler warning.
00015   8-Jun-1999  FMP added vLimit, tcSetVLimit()
00016   8-Mar-1999  FMP added tcSpace arg to tcqCreate()
00017   25-Feb-1999  FMP removed changed 'full' to 'allFull' in TC_QUEUE_STRUCT,
00018   and added tcqFull(). This was to ameliorate the race condition between
00019   the full state and the appending process seeing it, where a couple of
00020   motions may be appended before the earlier full state was seen. tcqFull()
00021   returns true if the queue is into a margin of safety, but the queue will
00022   still work until the allFull limit is reached.
00023   25-Jun-1998  FMP added v to premax
00024   15-Jun-1998  FMP added termFlag, TC_TERM_COND_STOP,BLEND, tcSet/GetTerm()
00025   18-Dec-1997  FMP took out C++ interface
00026   18-Dec-1997  FMP changed to PmPose
00027   16-Jul-1997  FMP added id
00028   14-Jul-1997  FMP added C posemath changes (PM_POSE -> PmPose)
00029   17-Jun-1997  FMP added type, TC_LINEAR, TC_CIRCULAR
00030   13-Jun-1997  FMP added TC_IS_PAUSED, vScale
00031   16-Apr-1997  FMP created from C and C++ versions
00032 */
00033 
00034 #include "posemath.h"
00035 #include "emcpos.h"
00036 
00037 /* ident tag */
00038 #ifndef __GNUC__
00039 #ifndef __attribute__
00040 #define __attribute__(x)
00041 #endif
00042 #endif
00043 
00044 static char __attribute__((unused)) tc_h[] = "$Id: tc.h,v 1.6 2001/08/17 22:05:41 proctor Exp $";
00045 
00046 /* values for tcFlag */
00047 #define TC_IS_UNSET -1
00048 #define TC_IS_DONE 1
00049 #define TC_IS_ACCEL 2
00050 #define TC_IS_CONST 3
00051 #define TC_IS_DECEL 4
00052 #define TC_IS_PAUSED 5
00053 
00054 /* values for endFlag */
00055 #define TC_TERM_COND_STOP 1
00056 #define TC_TERM_COND_BLEND 2
00057 
00058 #define TC_LINEAR 1
00059 #define TC_CIRCULAR 2
00060 
00061 /* structure for individual trajectory elements */
00062 
00063 typedef struct
00064 {
00065   double cycleTime;
00066   double targetPos;             /* positive motion progession */
00067   double vMax;                  /* max velocity */
00068   double vScale;                /* scale factor for vMax */
00069   double aMax;                  /* max accel */
00070   double preVMax;               /* vel from previous blend */
00071   double preAMax;               /* decel (negative) from previous blend */
00072   double vLimit;                /* abs vel limit, including scale */
00073   double toGo;
00074   double currentPos;
00075   double currentVel;
00076   double currentAccel;
00077   int tcFlag;                   /* TC_IS_DONE,ACCEL,CONST,DECEL*/
00078   int type;                     /* TC_LINEAR, TC_CIRCULAR */
00079   int id;                       /* id for motion segment */
00080   int termCond;                 /* TC_END_STOP,BLEND */
00081   PmLine line;
00082   PmLine line_abc;
00083   PmCircle circle;
00084   double tmag;                  /* magnitude of translation */
00085   double abc_mag;               /* magnitude of rotation  */
00086   double tvMax;                 /* maximum translational velocity */
00087   double taMax;                 /* maximum translational accelleration */
00088   double abc_vMax;              /* maximum rotational velocity */
00089   double abc_aMax;              /* maximum rotational accelleration */
00090   PmCartesian unitCart;
00091   unsigned char douts;          /* mask for douts to set */
00092   unsigned char doutstarts;     /* mask for dout start vals */
00093   unsigned char doutends;       /* mask for dout end vals */
00094 } TC_STRUCT;
00095 
00096 /* TC_STRUCT functions */
00097 
00098 extern int tcInit(TC_STRUCT *tc);
00099 extern int tcSetCycleTime(TC_STRUCT *tc, double secs);
00100 extern int tcSetLine(TC_STRUCT *tc, PmLine line, PmLine line_abc);
00101 extern int tcSetCircle(TC_STRUCT *tc, PmCircle circle, PmLine line_abc);
00102 extern int tcSetTVmax(TC_STRUCT *tc, double vmax);
00103 extern int tcSetRVmax(TC_STRUCT *tc, double vmax);
00104 extern int tcSetVscale(TC_STRUCT *tc, double vscale);
00105 extern int tcSetTAmax(TC_STRUCT *tc, double amax);
00106 extern int tcSetRAmax(TC_STRUCT *tc, double wmax);
00107 extern int tcSetPremax(TC_STRUCT *tc, double vmax, double amax);
00108 extern int tcSetVlimit(TC_STRUCT *tc, double vlimit);
00109 extern int tcSetId(TC_STRUCT *tc, int id);
00110 extern int tcGetId(TC_STRUCT *tc);
00111 extern int tcSetTermCond(TC_STRUCT *tc, int cond);
00112 extern int tcGetTermCond(TC_STRUCT *tc);
00113 extern int tcRunCycle(TC_STRUCT *tc);
00114 extern EmcPose tcGetPos(TC_STRUCT *tc);
00115 extern EmcPose tcGetGoalPos(TC_STRUCT *tc);
00116 extern double tcGetVel(TC_STRUCT *tc);
00117 extern double tcGetAccel(TC_STRUCT *tc);
00118 extern PmCartesian tcGetUnitCart(TC_STRUCT *tc);
00119 extern int tcGetTcFlag(TC_STRUCT *tc);
00120 extern int tcIsDone(TC_STRUCT *tc);
00121 extern int tcIsAccel(TC_STRUCT *tc);
00122 extern int tcIsConst(TC_STRUCT *tc);
00123 extern int tcIsDecel(TC_STRUCT *tc);
00124 extern int tcIsPaused(TC_STRUCT *tc);
00125 extern void tcPrint(TC_STRUCT *tc);
00126 extern double tcRunPreCycle(const TC_STRUCT *tc);
00127 extern int tcForceCycle(TC_STRUCT *tc, double ratio);
00128 extern int tcSetDout(TC_STRUCT *tc, unsigned char douts, unsigned char starts, unsigned char ends);
00129 
00130 /* queue of TC_STRUCT elements*/
00131 
00132 typedef struct
00133 {
00134   TC_STRUCT *queue;             /* ptr to the tcs */
00135   int size;                     /* size of queue */
00136   int _len;                     /* number of tcs now in queue */
00137   int start, end;               /* indices to next to get, next to put */
00138   int allFull;                  /* flag meaning it's actually full */
00139 } TC_QUEUE_STRUCT;
00140 
00141 /* TC_QUEUE_STRUCT functions */
00142 
00143 /* create queue of _size */
00144 extern int tcqCreate(TC_QUEUE_STRUCT *tcq, int _size, TC_STRUCT *tcSpace);
00145 
00146 /* free up queue */
00147 extern int tcqDelete(TC_QUEUE_STRUCT *tcq);
00148 
00149 /* reset queue to empty */
00150 extern int tcqInit(TC_QUEUE_STRUCT *tcq);
00151 
00152 /* put tc on end */
00153 extern int tcqPut(TC_QUEUE_STRUCT *tcq, TC_STRUCT tc);
00154 
00155 /* get tcq from front */
00156 extern TC_STRUCT tcqGet(TC_QUEUE_STRUCT *tcq, int *status);
00157 
00158 /* remove n tcs from front */
00159 extern int tcqRemove(TC_QUEUE_STRUCT *tcq, int n);
00160 
00161 /* how many tcs on queue */
00162 extern int tcqLen(TC_QUEUE_STRUCT *tcq);
00163 
00164 /* look at nth item, first is 0 */
00165 extern TC_STRUCT * tcqItem(TC_QUEUE_STRUCT *tcq, int n, int *status);
00166 
00167 /* look at last item */
00168 extern TC_STRUCT * tcqLast(TC_QUEUE_STRUCT *tcq, int *status);
00169 
00170 /* get full status */
00171 extern int tcqFull(TC_QUEUE_STRUCT *tcq);
00172 
00173 #endif /* TC_H */
00174