home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 15 / 15.iso / s / s300 / 1.ddi / CHAP3 / QUAD1.C < prev    next >
Encoding:
C/C++ Source or Header  |  1988-05-02  |  11.1 KB  |  408 lines
  1. /***********************************************************************
  2.     
  3. FILE
  4.     quad1.c  -  quadrature position and velocity measurement routines
  5.  
  6. ENTRY ROUTINE
  7.     quad_getp  -  get position
  8.  
  9. PRIVATE ROUTINE
  10.     pulse  -  parallel port interrupt service routine
  11.  
  12. INITIALIZATION ROUTINE
  13.     quad_init  -  initialize this module
  14.  
  15. REMARKS
  16.     This module extends quad0.c by measuring velocity as well as 
  17.     position using quadrature signals.
  18.  
  19.     For simplicity, this module assumes that quad_getv() will be
  20.     called at fixed intervals corresponding to the sampling period.
  21.     The velocity is then calculated by counting pulses over a sample
  22.     interval.  Unless the velocity is very slow, calculating velocity
  23.     at every quadrature state transistion may incur too much overhead.
  24.  
  25.     Note also that the pulse count variables are integers instead of
  26.     long integers.  On an IBM-PC, ints are faster and access can be
  27.     assume to be atomic with most compilers.  However, there is a
  28.     greater chance of overflow.  Whether to use ints or longs depends
  29.     on the requirements of your application (and how paranoid you
  30.     are obout overflows).
  31.  
  32.     You may want to use an interval timer instead of assuming a
  33.     fixed sampling interval.  See pfm0.c and time.c for an example
  34.     of how a software interval timer in time.c may be used.
  35.  
  36.     This module requires the xignal package to set up the parallel
  37.     port interrupt handler.
  38.  
  39. LAST UPDATE
  40.     27 January 1988
  41.  
  42.     Copyright(c) 1985-1988  D.M. Auslander and C.H. Tham
  43.  
  44. ***********************************************************************/
  45.  
  46. /***********************************************************************
  47.                             I M P O R T S
  48. ***********************************************************************/
  49.  
  50. #include "envir.h"
  51.  
  52. #include "inout.h"
  53. #include "xignal.h"
  54.  
  55.  
  56. /***********************************************************************
  57.               F O R W A R D    D E C L A R A T I O N S
  58. ***********************************************************************/
  59.  
  60. #ifdef ANSI
  61.  
  62. void incp(void);
  63. void decp(void);
  64.  
  65. #else
  66.  
  67. void incp();
  68. void decp();
  69.  
  70. #endif
  71.  
  72. /***********************************************************************
  73.                         P R I V A T E     D A T A
  74. ***********************************************************************/
  75.  
  76. #define PHASEMASK   0x03    /* mask for bits 0 and 1 on parallel port */
  77.  
  78. #define STATE00     0x00    /* quadrature states */
  79. #define STATE01     0x01
  80. #define STATE10     0x02
  81. #define STATE11     0x03
  82.  
  83. #define INPORT      0x3BE   /* parallel input port address */
  84.  
  85. #define FORWARD     1       /* forward direction flag value */
  86. #define REVERSE     0       /* reverse direction flag value */
  87.  
  88. static int oldstate;    /* previous state of quadrature input */
  89. static int curstate;    /* current quadrature state */
  90. static int pcount;      /* current pulse count */
  91. static int rcount;      /* reference count, for computing velocity */
  92. static int error;       /* error flag, initialized to false */
  93. static int revflag;     /* direction reversal flag */
  94. static int direction;   /* direction flag */
  95. static double pscale;   /* displacement per count */
  96. static double tsamp;    /* sampling period */
  97.  
  98.  
  99. /***********************************************************************
  100.                     E N T R Y    R O U T I N E S
  101. ***********************************************************************/
  102.  
  103. /*----------------------------------------------------------------------
  104. FUNCTION
  105.     QUAD_GETP  -  read position value (same as in quad0.c)
  106.  
  107. SYNOPSIS
  108.     double quad_getp()
  109.  
  110. RETURNS
  111.     incremental position in inches
  112.  
  113. LAST UPDATE
  114.     26 January 1988
  115.         change to double return type
  116. ----------------------------------------------------------------------*/
  117.  
  118. double quad_getp()
  119. {
  120.     int istat;      /* interrupt status */
  121.     int count;      /* copy of pcount */
  122.  
  123.  
  124.     if (error)
  125.     {
  126.         printf("quadrature state transistion error\n");
  127.  
  128.         error = 0;                  /* reset error flag */
  129.     }
  130.     
  131.     istat = disable();              /* begin critical section */
  132.  
  133.     count = pcount;
  134.  
  135.     if (istat)                      /* exit critical section */
  136.         enable();
  137.  
  138.     return(count * pscale);
  139. }
  140.  
  141.  
  142.  
  143. /*----------------------------------------------------------------------
  144. FUNCTION
  145.     QUAD_GETV  -  derive velocity from quadrature pulses
  146.  
  147. SYNOPSIS
  148.     double quad_getv()
  149.  
  150. RETURNS
  151.     velocity in rev/sec
  152.  
  153. REMARKS
  154.     For simplicity, we assume getv() is called at intervals of tsamp
  155.     seconds.  However, you may wish to use an interval timer instead
  156.     of making this assumption.
  157.  
  158.     The velocity is derived from the diffrence between the current
  159.     pulse count and the pulse count when this routine is last called
  160.     in the previous sampling instance.  If there is a change in
  161.     direction between the current and last call (as indicated by the
  162.     revflag), velocity is derived using the pulse count when the direction
  163.     reversal takes place (rcount) instead of the count when this routine
  164.     is last called (lcount).  This gives a reasonable estimate of the
  165.     actual velocity.
  166.  
  167.     If you are using an interval timer instead of assuming fixed
  168.     sampling time, substitute calls to read and reset the interval
  169.     timer instead of using tsamp.
  170.  
  171. LAST UPDATE
  172.     27 January 1988
  173.         use rcount when direction changes.
  174. ----------------------------------------------------------------------*/
  175.  
  176. double quad_getv()
  177. {
  178.     static int last_count = 0;      /* pcount at last call */
  179.     int cur_count;                  /* current value of pcount */
  180.     int rev_count;                  /* copy of rcount */
  181.     int istat;                      /* interrupt status */
  182.     double velocity;                /* computed velocity */
  183.  
  184.  
  185.     if (error)
  186.     {
  187.         printf("quadrature state transistion error\n");
  188.  
  189.         error = 0;              /* reset error flag */
  190.     }
  191.  
  192.     /*------------------------------------------------------------
  193.         Copies of pcount and rcount are made with interrupts
  194.         disabled to prevent their being asynchronously changed by
  195.         pulse() during velocity computation.  This precaution
  196.         may be omitted to reduce overheads if the resultant error
  197.         in velocity is not significant.  Real time programming
  198.         is full of such trade-offs.
  199.     ------------------------------------------------------------*/
  200.      
  201.     istat = disable();
  202.  
  203.     cur_count = pcount;
  204.     rev_count = rcount;
  205.  
  206.     if (revflag)    /* a direction reversal has taken place */
  207.     {
  208.         revflag = 0;
  209.  
  210.         if (istat)      /* interrupts enabled after clearing revflag */
  211.             enable();
  212.  
  213.         velocity = (cur_count - rev_count) * pscale / tsamp;
  214.     }
  215.     else
  216.     {
  217.         if (istat)
  218.             enable();
  219.  
  220.         velocity = (cur_count - last_count) * pscale / tsamp;
  221.     }
  222.  
  223.     last_count = cur_count;
  224.  
  225.     return(velocity);
  226. }
  227.  
  228.  
  229.  
  230. /***********************************************************************
  231.                 P R I V A T E     R O U T I N E S
  232. ***********************************************************************/
  233.  
  234. /*---------------------------------------------------------------------
  235. PROCEDURE
  236.     PULSE  -  quadrature pulse input interrupt service routine
  237.  
  238. SYNOPSIS
  239.     static void pulse(), called by parallel port interrupt service routine
  240.  
  241. REMARKS
  242.     Basically the same as that in quad0.c, except that it calls
  243.     procedures decp() and incp() to update the pulse count and
  244.     set revflag if there is a change in direction.
  245.  
  246. LAST UPDATE
  247.     26 January 1988
  248.         use new void type
  249. ----------------------------------------------------------------------*/
  250.  
  251. static void pulse()
  252. {
  253.  
  254.     oldstate = curstate;                /* update state variables */
  255.  
  256.     curstate = in(INPORT) & PHASEMASK;  /* read quadrature signal */
  257.  
  258.     switch (oldstate)
  259.     {
  260.         case STATE00:
  261.  
  262.             if (curstate == STATE01)
  263.                 decp();
  264.             else if (curstate == STATE10)
  265.                 incp();
  266.             else
  267.                 error = 1;
  268.             
  269.             break;
  270.         
  271.         case STATE01:
  272.  
  273.             if (curstate == STATE11)
  274.                 decp();
  275.             else if (curstate == STATE00)
  276.                 incp();
  277.             else
  278.                 error = 1;
  279.             
  280.             break;
  281.  
  282.         case STATE11:
  283.  
  284.             if (curstate == STATE10)
  285.                 decp();
  286.             else if (curstate == STATE01)
  287.                 incp();
  288.             else
  289.                 error = 1;
  290.             
  291.             break;
  292.         
  293.         case STATE10:
  294.  
  295.             if (curstate == STATE00)
  296.                 decp();
  297.             else if (curstate == STATE11)
  298.                 incp();
  299.             else
  300.                 error = 1;
  301.     }
  302.  
  303. }
  304.  
  305.  
  306.  
  307. /*---------------------------------------------------------------------
  308. PROCEDURE
  309.     INCP  -  increment pulse count
  310.  
  311. SYNOPSIS
  312.     static void incp()
  313.  
  314. REMARKS
  315.     Updates direction indicator and mark current pulse count
  316.  
  317. LAST UPDATE
  318.     27 January 1988
  319.         mark current pcount instead of using flag
  320. ----------------------------------------------------------------------*/
  321.  
  322. static void incp()
  323. {
  324.     
  325.     ++pcount;           /* increment pulse count */
  326.  
  327.     if (direction == REVERSE)   /* direction reversal is indicated */
  328.     {
  329.         direction = FORWARD;
  330.         rcount = pcount;
  331.         revflag = 1;
  332.     }
  333.  
  334. }
  335.  
  336.  
  337.  
  338. /*---------------------------------------------------------------------
  339. PROCEDURE
  340.     DECP  -  decrement pulse count
  341.  
  342. SYNOPSIS
  343.     static void decp()
  344.  
  345. REMARKS
  346.     Updates direction indicator and mark current pulse count
  347.  
  348. LAST UPDATE
  349.     27 January 1988
  350.         mark current pcount instead of using flag
  351. ----------------------------------------------------------------------*/
  352.  
  353. static void decp()
  354. {
  355.     
  356.     --pcount;           /* decrement pulse count */
  357.  
  358.     if (direction == FORWARD)   /* direction reversal is indicated */
  359.     {
  360.         direction = REVERSE;
  361.         rcount = pcount;
  362.         revflag = 1;
  363.     }
  364.  
  365. }
  366.  
  367.  
  368. /***********************************************************************
  369.             I N I T I A L I Z A T I O N    R O U T I N E S
  370. ***********************************************************************/
  371.  
  372. /*---------------------------------------------------------------------
  373. PROCEDURE
  374.     QUAD_INIT  -  initialize
  375.  
  376. SYNOPSIS
  377.     void quad_init(scale, period)
  378.     double scale, period;
  379.  
  380. PARAMETERS
  381.     scale  -  displacement per quadrature count
  382.     period -  sampling period
  383.  
  384. LAST UPDATE
  385.     26 January 1988
  386.         calling routine specifies scaling factor
  387. ----------------------------------------------------------------------*/
  388.  
  389. void quad_init(scale, period)
  390. double scale, period;
  391. {
  392.  
  393.     pscale = scale;
  394.     tsamp = period;
  395.  
  396.     pcount = 0;
  397.     rcount = 0;
  398.     error = 0;
  399.     revflag = 0;
  400.     direction = FORWARD;        /* arbitrary choice */
  401.  
  402.     curstate = oldstate = in(INPORT) & PHASEMASK;
  403.  
  404.     xignal(XIGPRN, pulse);  /* install pulse() as service routine */
  405.  
  406. }
  407.  
  408.