stg2.c Source File

00001 /*
00002   stg2.c
00003 
00004   Servo To Go board functions for new boards
00005 
00006   Modification history:
00007 
00008   27-Sep-2001 WPS changed STG_8_AXES to STG_8_AXIS to match stg.c
00009   2-Aug-2001  FMP added stgAdcStart,Wait,Read functions
00010   7-Aug-2000 WPS made several changes. The Servo-To-Go 2 code had a
00011   bug where STG_BASE_ADDRESS was not set to match wStg->wBaseAddress or
00012   vice versa when the port was found automatically. This meant that
00013   digital IO might work but the encoder/motion stuff not or vice versa
00014   depending on which was correct.  Set FIND_STG_BASE_ADDRESS to 1 to
00015   disable automatic finding of the base address.
00016   15-Jun-2000 WPS changed include asm/io.h to sys/io.h for rtlinux_2_2.
00017   13-Mar-2000 WPS made several changes to eliminate compiler warnings.
00018   20-Jan-2000 WPS changed the scaling in stgWriteDac()
00019   18-Jan-2000 WPS modified to more closely match the Model 2 documentation:
00020   -- >
00021   3-Nov-1999  FMP created from STG DOS example source; took out
00022   extEncoderReadLevel()
00023 */
00024 
00025 /*
00026   Porting Notes:
00027 
00028   The code that writes and read the encoders to get the number of axes
00029   causes encoder values to be 0 until the axes moves around a bit.
00030 */
00031 
00032 /* ident tag */
00033 #ifndef __GNUC__
00034 #ifndef __attribute__
00035 #define __attribute__(x)
00036 #endif
00037 #endif
00038 
00039 static char __attribute__((unused)) ident[] = "$Id: stg2.c,v 1.12 2001/10/31 20:21:47 wshackle Exp $";
00040 
00041 
00042 #if !defined(rtlinux) && !defined(rtai)
00043 #include <stdio.h>
00044 #endif
00045 
00046 #include "stg2.h"               /* these decls */
00047 #include "extintf.h"            /* EXT_ENCODER_INDEX_MODEL */
00048 
00049 /* base address-- override default at compile time in stg.h,
00050    or at module load time with STG_BASE_ADDRESS=value */
00051 unsigned short STG_BASE_ADDRESS = DEFAULT_STG_BASE_ADDRESS;
00052 int FIND_STG_BASE_ADDRESS=1;
00053 
00054 #ifdef STG_8_AXES
00055 #define STG_MAX_AXIS 8
00056 #else
00057 #define STG_MAX_AXIS 4
00058 #endif
00059 
00060 #if defined(rtlinux) || defined(rtai)
00061 #ifndef MODULE
00062 #define MODULE
00063 #endif
00064 #define DO_IT
00065 #endif
00066 
00067 #ifdef LINUX
00068 #define DO_IT
00069 /*
00070   Compiling this for LINUX (not RTLINUX) means linking this into a Linux
00071   process, running non-real-time in user space. This is useful for debugging.
00072   If this is done, then the following stuff needs to be done to access the
00073   IO space.
00074   */
00075 
00076 /*
00077   Because of a limitation in gcc (present at least in 2.7.2.1 and below), you
00078   _have to_ compile any source code that uses these routines with optimisation
00079   turned on (gcc -O1 or higher), or alternatively #define extern to be
00080   empty before #including <asm/io.h>.
00081 
00082   Otherwise, you'll get errors at link time like:
00083 
00084   stg.c:475: undefined reference to `__inbc'
00085  */
00086 #ifdef DEFINE_EXTERN_BEFORE_IO
00087 #define extern
00088 #endif
00089 
00090 
00091 /*
00092   Need to access ports in range 0x200-0x21F and 0x600-0x61F, for default
00093   STG base address. ioperm() only enables up to 0x3FF, so we need to use
00094   iopl() to grant full access to IO space.
00095   */
00096 #include <unistd.h>             /* iopl() */
00097 
00098 #endif /* LINUX  */
00099 
00100 #ifdef DO_IT
00101 
00102 #if defined(__KERNEL__) && defined(linux)
00103 #include <linux/kernel.h>
00104 #endif
00105 
00106 #if defined(linux) || defined(rtlinux) || defined(rtai)
00107 #include <sys/io.h>             /* ioperm() */
00108 #endif
00109 
00110 /* asm/io.h already included by sys/io.h for 2.2, 2.3, 2.4 kernels  */
00111 
00112 #ifndef LINUX_VERSION_CODE
00113 #include <linux/version.h>
00114 #endif
00115 
00116 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,2,0)
00117 /* Linux or RT-Linux use io ports */
00118 #include <asm/io.h>             /* outb, inb */
00119 #endif
00120 
00121 #else
00122 
00123 /* otherwise do nothing */
00124 
00125 #include "rcs_prnt.hh"
00126 
00127 static int iopl(int level)
00128 {
00129   return 0;
00130 }
00131 
00132 static unsigned char inb(unsigned int port)
00133 {
00134   if (port >= STG_BASE_ADDRESS &&
00135       port <= STG_BASE_ADDRESS + 0x1f)
00136     {
00137       return 0;
00138     }
00139 
00140   if (port >= STG_BASE_ADDRESS + 0x400 &&
00141       port <= STG_BASE_ADDRESS + 0x400 + 0x1f)
00142     {
00143       return 0;
00144     }
00145 
00146   printf("inb: address out of bounds: %X\n", port);
00147   return 0;
00148 }
00149 
00150 static void outb(unsigned char byte, unsigned int port)
00151 {
00152   /* allow writes to 0x80 for 1 msec delay */
00153   if (port == 0x80)
00154     {
00155       return;
00156     }
00157 
00158   if (port >= STG_BASE_ADDRESS &&
00159       port <= STG_BASE_ADDRESS + 0x1f)
00160     {
00161       return;
00162     }
00163 
00164   if (port >= STG_BASE_ADDRESS + 0x400 &&
00165       port <= STG_BASE_ADDRESS + 0x400 + 0x1f)
00166     {
00167       return;
00168     }
00169 
00170   printf("outb: address out of bounds: %X\n", port);
00171 }
00172 
00173 static unsigned short inw(unsigned int port)
00174 {
00175   if (port % 2)
00176     {
00177       printf("inw: alignment error: %X\n", port);
00178       return 0;
00179     }
00180 
00181   if (port >= STG_BASE_ADDRESS &&
00182       port <= STG_BASE_ADDRESS + 0x1e)
00183     {
00184       return 0;
00185     }
00186 
00187   if (port >= STG_BASE_ADDRESS + 0x400 &&
00188       port <= STG_BASE_ADDRESS + 0x400 + 0x1e)
00189     {
00190       return 0;
00191     }
00192 
00193   printf("inw: address out of bounds: %X\n", port);
00194   return 0;
00195 }
00196 
00197 static void outw(unsigned short word, unsigned int port)
00198 {
00199   if (port % 2)
00200     {
00201       printf("outw: alignment error: %X\n", port);
00202       return;
00203     }
00204 
00205   if (port >= STG_BASE_ADDRESS &&
00206       port <= STG_BASE_ADDRESS + 0x1e)
00207     {
00208       return;
00209     }
00210 
00211   if (port >= STG_BASE_ADDRESS + 0x400 &&
00212       port <= STG_BASE_ADDRESS + 0x400 + 0x1e)
00213     {
00214       return;
00215     }
00216 
00217   printf("outw: address out of bounds: %X\n", port);
00218   return;
00219 }
00220 
00221 #endif /* don't DO_IT */
00222 
00223 /* map DOS funcs to Linux */
00224 #define _outp(port,val) outb(val,port)
00225 #define outp(port,val) outb(val,port)
00226 #define _inp(port) inb(port)
00227 #define inp(port) inb(port)
00228 #define _inpw(port) inw(port)
00229 #define _outpw(port,val) outw(val,port)
00230 
00231 #define fOutP(port,val) _outp(port,val)
00232 #define fOutPW(port,val) _outpw(port, val)
00233 #define fInP(port) _inp(port)
00234 #define fInPW(port) _inpw(port)
00235 
00236 static const int      aPortOffset_1[] = {DIO_A, DIO_B, DIO_C, DIO_D};
00237 static const int      aPortOffset_2[] = {PORT_A, PORT_B, PORT_C, PORT_D};
00238 
00239 /***************************************************************************/
00240 /*                                                                         */
00241 /*                           Constructor                                   */
00242 /*                                                                         */
00243 /***************************************************************************/
00244 void ServoToGoConstructor(STG_STRUCT *stg, unsigned short wRequestedIrq)
00245 {
00246   int t;
00247 
00248 #if defined (LINUX)
00249   iopl(3);  /* get access to I/O ports, must be root. */
00250 #endif
00251 
00252   /* BUG FIX-- init globals */
00253   stg->wAxesInSys = 0;
00254   stg->wBaseAddress = 0;
00255   stg->wIrq = 0;
00256   stg->wModel = MODEL_NO_ID;
00257   stg->wNoBoardFlag = NO_BOARD;
00258   stg->wAxesInSys = 0;
00259   stg->wSaveDirs = 0;
00260   stg->byIndexPollAxis = 0;
00261   stg->byIndexPulsePolarity = 1;
00262   for (t = 0; t < MAX_AXIS; t++) {
00263     stg->lSimDac[t] = 0;
00264     stg->lSimEnc[t] = 0;
00265     stg->byEncHighByte[t] = 0;
00266     stg->byOldByte2[t] = 0;
00267   }
00268 
00269   Initialize(stg, wRequestedIrq);   /* figure out board address, init irq controller */
00270   EncoderInit(stg);
00271   SelectInterruptPeriod(stg, _1_MILLISECOND);  /* initialize timer */
00272   stg->byIndexPulsePolarity = 1;               /* default active level high */
00273   if (stg->wModel == MODEL2)
00274     fOutP(stg->wBaseAddress + CNTRL1, CNTRL1_NOT_SLAVE);
00275 };
00276 
00277 /***************************************************************************/
00278 /*                                                                         */
00279 /*                            Destructor                                   */
00280 /*                                                                         */
00281 /***************************************************************************/
00282 void ServoToGoDestructor(STG_STRUCT *stg)
00283 {
00284   unsigned short nAxis;
00285 
00286   StopInterrupts(stg);
00287 
00288   /* set all the DAC outputs to 0. */
00289   for (nAxis = 0; nAxis < MAX_AXIS; nAxis++)
00290     RawDAC(stg, nAxis, 0);
00291 
00292   /* set all the digital I/O bits to input */
00293   DioDirection(stg, 0);
00294 }
00295 
00296 /***************************************************************************/
00297 /*                                                                         */
00298 /*                              SetIrq                                     */
00299 /*                                                                         */
00300 /***************************************************************************/
00301 void SetIrq(STG_STRUCT *stg, unsigned short wRequestedIrq)
00302 {
00303   unsigned char byIntReg;
00304 
00305   if (stg->wNoBoardFlag == NO_BOARD)
00306     return;
00307 
00308   stg->wIrq = wRequestedIrq;  /* assume it's OK for now, check later */
00309 
00310   if (stg->wModel == MODEL1)
00311     byIntReg = 0x80;       /* initial value for the high bits in the register */
00312   /* sets auto zero off */
00313   else   /* MODEL2 */
00314     byIntReg = 0x88;       /* cal high too, not calibrating ADC */
00315 
00316   /* now put low bits into byIntReg to select irq */
00317 
00318   switch (wRequestedIrq)
00319     {
00320     case 3: break;      /* add zero */
00321 
00322     case 5: byIntReg |= 4;
00323       break;
00324 
00325     case 7: byIntReg |= 2;
00326       break;
00327 
00328     case 9: byIntReg |= 6;
00329       break;
00330 
00331     case 10: byIntReg |= 5;
00332       break;
00333 
00334     case 11: byIntReg |= 7;
00335       break;
00336 
00337     case 12: byIntReg |= 3;
00338       break;
00339 
00340     case 15: byIntReg |= 1;
00341       break;
00342 
00343     default: stg->wIrq = 5;      /* ERROR, requested irq not valid, use 5 */
00344       byIntReg |= 4; /* There is no safe value, leaving zero */
00345       /* here would select IRQ 3 which is worse */
00346       /* than 5 because IRQ 3 is usually for COM 2 */
00347       break;
00348     }
00349 
00350   if (stg->wModel == MODEL1)
00351     fOutP(stg->wBaseAddress + INTC, byIntReg);  /* set irq */
00352   else   /* MODEL2 */
00353     fOutP(stg->wBaseAddress + CNTRL0, byIntReg);
00354 }
00355 /***************************************************************************/
00356 /*                                                                         */
00357 /*                             BaseFind                                    */
00358 /*                                                                         */
00359 /***************************************************************************/
00360 
00361 /* find the base address of the Servo To Go board */
00362 
00363 unsigned short BaseFind(STG_STRUCT *stg)
00364 {
00365   short i;
00366   unsigned short io_add;
00367 
00368   for (i = 15; i >= 0; i--)                    /* search all possible addresses */
00369     {
00370       io_add = i * 0x20 + 0x200;
00371       if ( BrdtstOK(stg,
00372 io_add) )
00373         return io_add;
00374     }
00375   return(0);
00376 }
00377 
00378 /***************************************************************************/
00379 /*                                                                         */
00380 /*                             BrdtstOK                                    */
00381 /*                                                                         */
00382 /***************************************************************************/
00383 
00384 /* check the board tests register, figure out board model */
00385 
00386 unsigned short BrdtstOK(STG_STRUCT *stg, unsigned short BaseAddress)
00387 {
00388   unsigned short BrdtstAddress;
00389   unsigned short SerSeq, HighNibble;
00390   int j;
00391 
00392   BrdtstAddress = BaseAddress + BRDTST;
00393 
00394   SerSeq = 0;
00395   for (j = 7; j >= 0; j--)
00396     {
00397       HighNibble = fInP(BrdtstAddress) >> 4;
00398       if (HighNibble & 8)     /* is SER set */
00399         {
00400           /* shift bit to position specifed by Q2, Q1, Q0 */
00401           /* which are the lower three bits.  Put bit in SerSeq. */
00402           SerSeq |= 1 << (HighNibble & 7);
00403         }
00404     }
00405   if (SerSeq == 0x75)        /* SER sequence is 01110101 */
00406     {
00407       stg->wModel = MODEL1;
00408       return 1;              /*true */
00409     }
00410   if (SerSeq == 0x74)        /* SER sequence is 01110100 */
00411     {
00412       stg->wModel = MODEL2;
00413       return 1;              /* true */
00414     }
00415   /*    stg->wModel = MODEL_NO_ID; */
00416   return 0;  /* false */
00417 }
00418 
00419 /***************************************************************************/
00420 /*                                                                         */
00421 /*                             Initialize                                  */
00422 /*                                                                         */
00423 /***************************************************************************/
00424 void Initialize(STG_STRUCT *stg, unsigned short wRequestedIrq)
00425 {
00426   /*
00427    * First find the the base I/O address of the board.
00428    *
00429    * only do this in the DOS example.  It's dangerous under 95, and can't be
00430    * done under NT
00431    */
00432   if(FIND_STG_BASE_ADDRESS)
00433     {
00434       stg->wBaseAddress = BaseFind(stg);
00435       if (stg->wBaseAddress == 0)
00436         {
00437           stg->wNoBoardFlag = NO_BOARD;
00438         }
00439       else                              /* BUG FIX-- need an else */
00440         {
00441           stg->wNoBoardFlag = BOARD_PRESENT;
00442         }
00443 #if !defined(rtlinux) && !defined(rtai)
00444       printf("Servo To Go Board IO address = 0x%X\n",stg->wBaseAddress);
00445 #endif
00446       STG_BASE_ADDRESS = stg->wBaseAddress;
00447     }
00448   else
00449     {
00450       stg->wBaseAddress = STG_BASE_ADDRESS;
00451     }
00452 
00453 
00454   /*
00455    * Initialize the interrupt controller
00456    */
00457   if (stg->wModel == MODEL1)
00458     {
00459       fOutP(stg->wBaseAddress + MIO_2, 0x92);  /* initialize INTC as output reg. */
00460       /* sets port D to input since we have */
00461       /* to set it to something. */
00462       SetIrq(stg, wRequestedIrq);              /* selects the IRQ in INTC. Now, if a stray */
00463       /* interrupt is issued (see below) it will */
00464       /* go to an interrupt that isn't enabled on */
00465       /* the motherboard yet (if your system is */
00466       /* set up correctly). */
00467       fOutP(stg->wBaseAddress + ICW1, 0x1a ); /* initialize 82C59 as single chip, */
00468       /* level triggered */
00469       fOutP(stg->wBaseAddress + ICW2, 0x00 ); /* icw2 - not used, must write */
00470       /* could issue stray interrupt here - danger! */
00471       fOutP(stg->wBaseAddress + OCW1, 0xff);  /* mask off all interrupt sources (the */
00472       /* interrupt on the motherboard isn't */
00473       /* enabled yet, you do that when you install */
00474       /* your interrupt handler.). */
00475     }
00476   else   /* must be a Model 2 */
00477     {
00478       fOutP(stg->wBaseAddress + MIO_2, 0x8b);  /* initialize CNTRL0 as output reg. */
00479       /* BRDTST to input. */
00480       /* sets port D, high and low, to input */
00481       /* since we have to set it to something. */
00482       SetIrq(stg, wRequestedIrq);              /* selects the IRQ in CNTRL0. Now, if a stray */
00483       /* interrupt is issued (see below) it will */
00484       /* go to an interrupt that isn't enabled on */
00485       /* the motherboard yet (if your system is */
00486       /* set up correctly). */
00487     }
00488 };
00489 
00490 /***************************************************************************/
00491 /*                                                                         */
00492 /*                              StartADC                                   */
00493 /*                                                                         */
00494 /***************************************************************************/
00495 void StartADC(STG_STRUCT *stg, unsigned short wAxis)
00496 {
00497   unsigned char Cntrl0;
00498 
00499   if (stg->wNoBoardFlag == NO_BOARD)
00500     {
00501       return;
00502     }
00503 
00504   if (wAxis > 7)
00505     return;
00506 
00507   if (stg->wModel == MODEL1)
00508     {
00509       /* do a dummy read from the ADC, just to set the input multiplexer to */
00510       /* the right channel */
00511       fInPW(stg->wBaseAddress + ADC_0 + (wAxis << 1));        /*lint !e534 */
00512 
00513       /* wait 4 uS for settling time on the multiplexer and ADC */
00514       /* you probably shouldn't really have a delay in */
00515       /* a driver. */
00516       Timer2Delay(stg, 28);
00517 
00518       /* now start conversion. */
00519       fOutPW(stg->wBaseAddress + ADC_0 + (wAxis << 1), 0);
00520     }
00521   else  /* Model 2 */
00522     {
00523       Cntrl0 = fInP(stg->wBaseAddress + CNTRL0) & 0x07;  /* save irq */
00524 
00525       Cntrl0 |= (wAxis << 4) | 0x88;  /* shift bits to AD2, AD1, AD0 */
00526       /* set bit 0x80 high for autozero */
00527       /* set bit 0x08 high, not calibrating */
00528       fOutP(stg->wBaseAddress + CNTRL0, Cntrl0);  /* select ADC channel */
00529 
00530       /* don't have to do a dummy read for a model 2 */
00531 
00532       /* wait 4 uS for settling time on the multiplexer and ADC */
00533       /* you probably shouldn't really have a delay in */
00534       /* a driver. */
00535       Timer2Delay(stg, 28);
00536 
00537       /* now start conversion. */
00538       fOutPW(stg->wBaseAddress + ADC, 0);
00539     }
00540 };
00541 
00542 /***************************************************************************/
00543 /*                                                                         */
00544 /*                               RawADC                                    */
00545 /*                                                                         */
00546 /***************************************************************************/
00547 short SpinReadADC(STG_STRUCT *stg, unsigned short wAxis)
00548 {
00549   short counts;
00550   short j;
00551 
00552   if (stg->wNoBoardFlag == NO_BOARD)
00553     {
00554       return 0;
00555     }
00556 
00557   if (wAxis > 7)
00558     return -1;
00559 
00560   if (stg->wModel == MODEL1)
00561     {
00562       /* make sure conversion is done, assume polling delay is done. */
00563       /* EOC (End Of Conversion) is bit 0x08 in IIR (Interrupt Request */
00564       /* Register) of Interrupt Controller.  Don't wait forever though */
00565       /* bail out eventually. */
00566 
00567       for (j = 0; (!(CurrentIRR(stg) & 0x08)) && (j < 10000); j++);
00568 
00569       counts = ( fInPW(stg->wBaseAddress + ADC_0 + (wAxis << 1)) );
00570     }
00571   else
00572     {
00573       /* is the conversion done? */
00574       for (j = 0; (fInP(stg->wBaseAddress + BRDTST) & BRDTST_EOC) && (j < 10000); j++);
00575 
00576       /* conversion is done, get counts. */
00577       counts = fInPW(stg->wBaseAddress + ADC);
00578     }
00579 
00580   if (counts & 0x1000)       /* is sign bit negative? */
00581     counts |= 0xf000;      /* sign extend */
00582   else
00583     counts &= 0xfff;       /* make sure high order bits are zero. */
00584 
00585   return counts;
00586 };
00587 
00588 long ReadADC(STG_STRUCT *stg, unsigned short wAxis, short * counts)
00589 {
00590   if (stg->wNoBoardFlag == NO_BOARD)
00591     {
00592       return STG_SUCCESS;
00593     }
00594 
00595   if (wAxis > 7)
00596     return STG_FAILURE;
00597 
00598   if (stg->wModel == MODEL1)
00599     {
00600       if ( !(CurrentIRR(stg) & 0x08) ) /* is the conversion done? */
00601         return STG_FAILURE;                  /* no, return failure */
00602 
00603       /* conversion is done, get counts. */
00604       /*        *counts = fInPW(stg->wBaseAddress + ADC_0 + (wAxis << 1)); */
00605       /*        *counts = fInPW(stg->wBaseAddress + ADC_1);  // debug */
00606     }
00607   else  /* Model 2 */
00608     {
00609       /* is the conversion done? */
00610       if ( fInP(stg->wBaseAddress + BRDTST) & BRDTST_EOC )
00611         return STG_FAILURE;                 /* no, return failure */
00612 
00613       /* conversion is done, get counts. */
00614       /*        *counts = fInPW(stg->wBaseAddress + ADC_1); */
00615     }
00616 
00617   *counts = fInPW(stg->wBaseAddress + ADC_0 + (wAxis << 1));
00618 
00619   if (*counts & 0x1000)       /* is sign bit negative? */
00620     *counts |= 0xf000;      /* sign extend */
00621   else
00622     *counts &= 0xfff;       /* make sure high order bits are zero. */
00623 
00624   return STG_SUCCESS;
00625 
00626 };
00627 
00628 /***************************************************************************/
00629 /*                                                                         */
00630 /*                             EncoderInit                                 */
00631 /*                                                                         */
00632 /***************************************************************************/
00633 /* */
00634 /* sets global variable: wAxesInSys */
00635 /* */
00636 void EncoderInit(STG_STRUCT *stg)
00637 {
00638   unsigned short wAdd, uplim;   /* for BUG FIX below */
00639 #if 0
00640   /* These variables are used only in the if 0'd out sections below. */
00641   unsigned short wA;
00642   unsigned short const wTestPat = 0x5aa5;
00643   LONGBYTE enc[MAX_AXIS];       /* BUG FIX-- was hard-coded 8 */
00644 #endif
00645 
00646   if (stg->wNoBoardFlag == NO_BOARD)
00647     {
00648       stg->wAxesInSys = MAX_AXIS;
00649       return;
00650     }
00651 
00652   /* It is possible that the encoder counts are being held by battery */
00653   /* backup, so we'll read the encoders, and save the values */
00654   /* Then we'll initialize the encoder chips, since it's more likely that */
00655   /* the ecoders were not kept alive by battery and need to be initialized */
00656 
00657 #if 0
00658   /* FIXME-- we need to read all the encoders here, so temporarily set
00659      wAxesInSys to MAX_AXIS */
00660   stg->wAxesInSys = MAX_AXIS;
00661   EncReadAll(stg, enc);
00662   stg->wAxesInSys = 0;
00663 #endif
00664 
00665   /* probably the right thing is to sign extend the 24 bits, so, instead */
00666   /* of a 24 bit unsigned count, we have +/- 23 bits. */
00667 
00668   /*    for ( i = 0; i < 8; i++) */
00669   /*    { */
00670   /*        byEncHighByte[i] = enc[i].Byte[2] & 0x80 ? 0xff : 0; */
00671   /*        byOldByte2[i] = enc[i].Byte[2]; */
00672   /*    } */
00673 
00674   /* FIXME-- we don't yet have wAxesInSys, so we'll have to go to the max */
00675   uplim = stg->wBaseAddress + CNT6_C;
00676 
00677   for (wAdd = stg->wBaseAddress + CNT0_C; wAdd <= uplim; wAdd +=4)
00678     {
00679       /* we're going to be slick and do two chips at a time, that's why */
00680       /* the registers are arranged data, data, control, control.  You */
00681       /* can do two at a time, by using word operations, instead of */
00682       /* byte operations.  Not a big deal for initializing, but reading is */
00683       /* done pretty often. */
00684 
00685       fOutPW(wAdd, 0x2020);   /* master reset */
00686 
00687       /* Set Counter Command Register - Input Control, OL Load (P3), */
00688       /* and Enable Inputs A and B (INA/B). */
00689 
00690       fOutPW(wAdd, 0x6868);
00691 
00692       /* Set Counter Command Register - Output Control */
00693 
00694       fOutPW(wAdd, 0x8080);
00695 
00696       /* Set Counter Command Register - Quadrature */
00697 
00698       fOutPW(wAdd, 0xc3c3);
00699 
00700       fOutPW(wAdd, 0x0404);  /* reset counter to zero */
00701     }
00702 
00703   /*  Figure out how many axes are on the card */
00704 
00705 #if 0
00706   /* FIXME-- we don't yet have wAxesInSys, so we'll have to go to the max */
00707   uplim = stg->wBaseAddress + CNT6_D;
00708 
00709   for (wA = stg->wBaseAddress + CNT0_D; wA <= uplim; wA +=4)
00710     {
00711       /* reset address pointer */
00712 
00713       fOutPW(wA + 2, 0x0101);
00714 
00715       /* write a pattern to the preset register */
00716 
00717       fOutPW(wA, wTestPat);
00718       fOutPW(wA, wTestPat);
00719       fOutPW(wA, wTestPat);
00720 
00721       /* transfer the preset register to the count register */
00722 
00723       fOutPW(wA + 2, 0x0909);
00724 
00725       /* transfer counter to output latch */
00726 
00727       fOutPW(wA + 2, 0x0202);
00728 
00729       /* read the output latch and see if it matches */
00730 
00731       if (fInPW(wA) != wTestPat)
00732         break;
00733       if (fInPW(wA) != wTestPat)
00734         break;
00735       if (fInPW(wA) != wTestPat)
00736         break;
00737 
00738       /* now replace the values that you saved previously, in case the */
00739       /* encoder was battery backed up */
00740 
00741       fOutP(wA, enc[stg->wAxesInSys].Byte[0]);
00742       fOutP(wA, enc[stg->wAxesInSys].Byte[1]);
00743       fOutP(wA, enc[stg->wAxesInSys].Byte[2]);
00744 
00745       fOutP(wA + 1, enc[stg->wAxesInSys + 1].Byte[0]);
00746       fOutP(wA + 1, enc[stg->wAxesInSys + 1].Byte[1]);
00747       fOutP(wA + 1, enc[stg->wAxesInSys + 1].Byte[2]);
00748 
00749       /* transfer the preset register to the count register */
00750 
00751       fOutPW(wA + 2, 0x0909);
00752 
00753       stg->wAxesInSys += 2;
00754 
00755       /* write zeros to preset register, we don't want to do a master reset */
00756       /* (MRST), because then we would need to re-initialize the counter */
00757 
00758       /*        fOutPW(wA, 0); */
00759       /*        fOutPW(wA, 0); */
00760       /*        fOutPW(wA, 0); */
00761 
00762       /* reset counter, BRW and CRY and address pointer (RADR) */
00763 
00764       /*        fOutPW(wA + 2, 0x0505); */
00765     }
00766 #else
00767   stg->wAxesInSys = 4;
00768 #endif
00769 };
00770 
00771 /***************************************************************************/
00772 /*                                                                         */
00773 /*                        SelectInterruptPeriod                            */
00774 /*                                                                         */
00775 /***************************************************************************/
00776 void SelectInterruptPeriod(STG_STRUCT *stg, long lPeriodSelect)
00777 {
00778   if (stg->wNoBoardFlag == NO_BOARD)
00779     {
00780       return;
00781     }
00782 
00783   if (lPeriodSelect != MAX_PERIOD)
00784     {
00785       fOutP(stg->wBaseAddress + TMRCMD, 0x56);   /* timer 1, read/load LSB (MSB is 0) */
00786       /* mode 3 (square wave) */
00787       fOutP(stg->wBaseAddress + TIMER_1, 0xb4);  /* 0xb4 = 180 -> 25 uSec period */
00788     }
00789   else
00790     {
00791       fOutP(stg->wBaseAddress + TMRCMD, 0x76);   /* timer 1, read/load LSB then MSB */
00792       /* mode 3 (square wave) */
00793       fOutP(stg->wBaseAddress + TIMER_1, 0xff);  /* LSB */
00794       fOutP(stg->wBaseAddress + TIMER_1, 0xff);  /* MSB */
00795     }
00796 
00797   switch (lPeriodSelect)
00798     {
00799     case _500_MICROSECONDS:
00800       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00801       /* MSB, mode 2 (real-time interrupt) */
00802       fOutP(stg->wBaseAddress + TIMER_0, 0x14);   /* 0x14 = 20 = .5 mS */
00803       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00804       break;
00805     case _1_MILLISECOND:
00806       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00807       /* MSB, mode 2 (real-time interrupt) */
00808       fOutP(stg->wBaseAddress + TIMER_0, 0x28);   /* 0x28 = 40 = 1 mS */
00809       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00810       break;
00811     case _2_MILLISECONDS:
00812       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00813       /* MSB, mode 2 (real-time interrupt) */
00814       fOutP(stg->wBaseAddress + TIMER_0, 0x50);   /* 0x50 = 80 = 2 mS */
00815       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00816       break;
00817     case _3_MILLISECONDS:
00818       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00819       /* MSB, mode 2 (real-time interrupt) */
00820       fOutP(stg->wBaseAddress + TIMER_0, 0x78);   /* 0x78 = 120 = 3 mS */
00821       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00822       break;
00823     case _4_MILLISECONDS:
00824       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00825       /* MSB, mode 2 (real-time interrupt) */
00826       fOutP(stg->wBaseAddress + TIMER_0, 0xA0);   /* 0xA0 = 160 = 4 mS */
00827       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00828       break;
00829     case _5_MILLISECONDS:
00830       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00831       /* MSB, mode 2 (real-time interrupt) */
00832       fOutP(stg->wBaseAddress + TIMER_0, 0xC8);   /* 0xC8 = 200 = 5 mS */
00833       fOutP(stg->wBaseAddress + TIMER_0, 0x00);
00834       break;
00835     case _10_MILLISECONDS:
00836       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00837       /* MSB, mode 2 (real-time interrupt) */
00838       fOutP(stg->wBaseAddress + TIMER_0, 0x90);   /* 0x0190 = 400 = 10 mS */
00839       fOutP(stg->wBaseAddress + TIMER_0, 0x01);
00840       break;
00841     case _100_MILLISECONDS:
00842       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00843       /* MSB, mode 2 (real-time interrupt) */
00844       fOutP(stg->wBaseAddress + TIMER_0, 0xA0);   /* 0x0FA0 = 4000 = 100 mS */
00845       fOutP(stg->wBaseAddress + TIMER_0, 0x0F);
00846       break;
00847     case _1_SECOND:
00848       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00849       /* MSB, mode 2 (real-time interrupt) */
00850       fOutP(stg->wBaseAddress + TIMER_0, 0x40);   /* 0x9C40 = 40000 = 1 S */
00851       fOutP(stg->wBaseAddress + TIMER_0, 0x9c);
00852       break;
00853     case MAX_PERIOD:
00854       fOutP(stg->wBaseAddress + TMRCMD, 0x34);    /* timer 0, read/load LSB followed by */
00855       /* MSB, mode 2 (real-time interrupt) */
00856       fOutP(stg->wBaseAddress + TIMER_0, 0xff);   /* LSB */
00857       fOutP(stg->wBaseAddress + TIMER_0, 0xff);   /* MSB */
00858       break;
00859     default:
00860       /* wrong input? then don't change it */
00861       break;
00862     }
00863 };
00864 
00865 /***************************************************************************/
00866 /*                                                                         */
00867 /*                               BaseAddress                               */
00868 /*                                                                         */
00869 /***************************************************************************/
00870 unsigned short BaseAddress(STG_STRUCT *stg)
00871 {
00872   return stg->wBaseAddress;
00873 };
00874 
00875 /***************************************************************************/
00876 /*                                                                         */
00877 /*                               RawDAC                                    */
00878 /*                                                                         */
00879 /***************************************************************************/
00880 void RawDAC(STG_STRUCT *stg, unsigned short nAxis, long lCounts)
00881 {
00882   if (stg->wNoBoardFlag == NO_BOARD)
00883     {
00884       return;
00885     }
00886 
00887   if ( nAxis > 7 )        /* is axis within range? */
00888     return;
00889 
00890 
00891   /* input / output: */
00892   /* */
00893   /*    lCounts (decimal) ... -lCounts ... +0x1000 ... volts */
00894   /* */
00895   /*     0x1000  (4096)     0xfffff000           0       +10 */
00896   /*out          0                      0      0x1000         0 */
00897   /* 0xfffff001 (-4095)          0xfff      0x1fff       -10 */
00898 
00899   /* So, the domain might be different than you expected. I expected: */
00900   /*     0xf000 (-4096)  to  0xfff (4095), rather than */
00901   /*     0xf001 (-4095)  to 0x1000 (4096) */
00902 
00903   /* reverse slope so positive counts give positive voltage */
00904   lCounts = - lCounts;
00905 
00906   /* shift for DAC */
00907   lCounts += 0x1000;
00908 
00909   if (lCounts > 0x1FFF)    /* clamp + output */
00910     {
00911       lCounts = 0x1FFF;
00912     }
00913   if (lCounts < 0)         /* clamp - output */
00914     {
00915       lCounts = 0;
00916     }
00917 
00918   if (stg->wNoBoardFlag == NO_BOARD)      /* are we simulating? */
00919     {
00920       stg->lSimDac[nAxis] = lCounts;
00921       return;
00922     }
00923 
00924   /*    nCounts = (USHORT) (lCounts + 0x1000); // correct range for DAC */
00925         
00926   fOutPW(stg->wBaseAddress + DAC_0 + (nAxis << 1), (unsigned short)lCounts);
00927 };
00928 
00929 /***************************************************************************/
00930 /*                                                                         */
00931 /*                             EncReadAll                                  */
00932 /*                                                                         */
00933 /***************************************************************************/
00934 void EncReadAll(STG_STRUCT *stg, LONGBYTE *lbEnc)
00935 {
00936   WORDBYTE wbTransfer;
00937   /*    static unsigned char byOldByte2[MAX_AXIS]; */
00938   /*    static unsigned char byEncHighByte[MAX_AXIS]; */
00939   short i;
00940 
00941   if (stg->wNoBoardFlag == NO_BOARD)
00942     {
00943       for (i = 0; i < 8; i++)
00944         {
00945           lbEnc[i].Long = stg->lSimEnc[i];
00946         }
00947       return;
00948     }
00949 
00950   /* Disable interrupts here?  No, the timer will latch new data in the */
00951   /* hardware anyway.  Maybe we should stop the timer?  In an interrupt */
00952   /* service routine, you're synchronized with the timer; so the readings */
00953   /* will never change while you're reading them.  If you're polling, you */
00954   /* would first latch the encoder counts with the EncoderLatch() function. */
00955   /* But, the timer could latch the counts again, in the middle of the read. */
00956   /* A critical section will help in some extreme cases. */
00957 
00958   /* reset counter internal addr ptr to point to first byte */
00959   /* BUG FIX-- don't go past 4 axes on 4 axis board */
00960   fOutPW(stg->wBaseAddress + CNT0_C, 0x0101);
00961   fOutPW(stg->wBaseAddress + CNT2_C, 0x0101);
00962   if (stg->wAxesInSys > 4) {
00963     fOutPW(stg->wBaseAddress + CNT4_C, 0x0101);
00964     fOutPW(stg->wBaseAddress + CNT6_C, 0x0101);
00965   }
00966 
00967   for (i = 0; i < 3; i++)            /* 24 bits means get 3 bytes each */
00968     {
00969       wbTransfer.Word = fInPW(stg->wBaseAddress + CNT0_D);
00970 
00971       lbEnc[0].Byte[i] = wbTransfer.Byte.high;
00972       lbEnc[1].Byte[i] = wbTransfer.Byte.low;
00973 
00974       wbTransfer.Word = fInPW(stg->wBaseAddress + CNT2_D);
00975 
00976       lbEnc[2].Byte[i] = wbTransfer.Byte.high;
00977       lbEnc[3].Byte[i] = wbTransfer.Byte.low;
00978 
00979       /* BUG FIX-- index rolls off end and kills stack if you
00980          have a 4 axis board after this point */
00981       if (stg->wAxesInSys < 8)
00982         {
00983           continue;
00984         }
00985 
00986       wbTransfer.Word = fInPW(stg->wBaseAddress + CNT4_D);
00987 
00988       lbEnc[4].Byte[i] = wbTransfer.Byte.high;
00989       lbEnc[5].Byte[i] = wbTransfer.Byte.low;
00990 
00991       wbTransfer.Word = fInPW(stg->wBaseAddress + CNT6_D);
00992 
00993       lbEnc[6].Byte[i] = wbTransfer.Byte.high;
00994       lbEnc[7].Byte[i] = wbTransfer.Byte.low;
00995     }
00996 
00997   /* maintain the high byte, to extend the counter to 32 bits */
00998   /* */
00999   /* base decisions to increment or decrement the high byte */
01000   /* on the highest 2 bits of the 24 bit value.  To get the */
01001   /* highest 2 bits, use 0xc0 as a mask on byte [2] (the third */
01002   /* byte). */
01003 
01004   for (i = 0; i < MAX_AXIS; i++)
01005     {
01006       /* check for -1 to 0 transition */
01007 
01008       if (    ( (stg->byOldByte2[i]    & 0xc0) == 0xc0 ) /* 11xxxxxx */
01009               && ( (lbEnc[i].Byte[2] & 0xc0) == 0 )    /* 00xxxxxx */
01010               )
01011         stg->byEncHighByte[i]++;
01012 
01013       /* check for 0 to -1 transition */
01014 
01015       if (    ( (stg->byOldByte2[i]    & 0xc0) == 0 )    /* 00xxxxxx */
01016               && ( (lbEnc[i].Byte[2] & 0xc0) == 0xc0 ) /* 11xxxxxx */
01017               )
01018         stg->byEncHighByte[i]--;
01019 
01020       lbEnc[i].Byte[3] = stg->byEncHighByte[i];
01021       stg->byOldByte2[i] = lbEnc[i].Byte[2];    /* current byte 2 becomes old one */
01022     }
01023 };
01024 
01025 /***************************************************************************/
01026 /*                                                                         */
01027 /*                          ResetIndexLatch(es)                            */
01028 /*                                                                         */
01029 /***************************************************************************/
01030 void ResetIndexLatch(STG_STRUCT *stg)
01031 {
01032   /* routine for Model 1 */
01033 
01034   fInP(stg->wBaseAddress + ODDRST);        /*lint !e534 reset index pulse latch for ODD axis */
01035   fInP(stg->wBaseAddress + BRDTST);        /*lint !e534 reset index pulse latch for EVEN axis */
01036 }
01037 
01038 void ResetIndexLatches(STG_STRUCT *stg, unsigned char byLatchBits)
01039 {
01040   /* routine for Model 2 */
01041   fOutP(stg->wBaseAddress + IDL, byLatchBits);
01042 }
01043 
01044 /***************************************************************************/
01045 /*                                                                         */
01046 /*                          SelectIndexAxis                                */
01047 /*                       SelectIndexOrExtLatch                             */
01048 /*                                                                         */
01049 /***************************************************************************/
01050 void SelectIndexAxis(STG_STRUCT *stg, unsigned char byAxis, unsigned char byPol)
01051 {
01052   /* routine for Model 1 */
01053 
01054   /* */
01055   /* initialize stuff to poll index pulse */
01056   /* */
01057   unsigned char byIntc;
01058 
01059   stg->byIndexPollAxis = byAxis;           /* save axis to check later */
01060   stg->byIndexPulsePolarity = byPol;       /* save polarity as new default */
01061   byAxis &= 0x6;                      /* ignore low bit, we check 2 axes at a time */
01062   byAxis <<= 3;                       /* shift into position for IXS1, IXS0 */
01063   byIntc = fInP(stg->wBaseAddress + INTC); /* get a copy of INTC, we'll change */
01064   /* some bits in it, not all */
01065   byIntc &= ~(IXLVL | IXS1 | IXS0);   /* zero bits for axis and polarity */
01066   byIntc |= byAxis;                   /* put axes address in INTC */
01067   if (byPol != 0)                     /* is index pulse active high? */
01068     byIntc |= IXLVL;
01069   fOutP(stg->wBaseAddress + INTC, byIntc);
01070   ResetIndexLatch(stg);
01071 
01072   /* The latched index pulse should be low now.  If it's not, either something's */
01073   /* wrong, or we happened to initialize it while the index pulse was active. */
01074 }
01075 
01076 void SelectIndexOrExtLatch(STG_STRUCT *stg, unsigned char bySelectBits)
01077 {
01078   /* routine for Model 2 */
01079 
01080   fOutP(stg->wBaseAddress + SELDI, bySelectBits);
01081 }
01082 
01083 void EnableCounterLatchOnIndexOrExt(STG_STRUCT *stg, unsigned char bySelectBits)
01084 {
01085   /* routine for Model 2 */
01086   fOutP(stg->wBaseAddress + IDLEN, bySelectBits);
01087 }
01088 
01089 /***************************************************************************/
01090 /*                                                                         */
01091 /*                             CurrentIRR                                  */
01092 /*                                                                         */
01093 /***************************************************************************/
01094 unsigned char CurrentIRR(STG_STRUCT *stg)
01095 {
01096   fOutP(stg->wBaseAddress + OCW3, 0x0a);           /* IRR on next read */
01097   return fInP(stg->wBaseAddress + IRRreg);
01098 }
01099 
01100 /***************************************************************************/
01101 /*                                                                         */
01102 /*                        (Get)IndexPulseLatch(es)                         */
01103 /*                                                                         */
01104 /***************************************************************************/
01105 unsigned short IndexPulseLatch(STG_STRUCT *stg)
01106 {
01107   /* routine for Model 1 board */
01108 
01109   /* poll the latched index pulse of the axis that was previously set up */
01110 
01111   unsigned char byIRR, byAxisMask;
01112 
01113   byIRR = CurrentIRR(stg);
01114   byAxisMask = (stg->byIndexPollAxis & 1) ? LIXODD : LIXEVN;  /* even or odd axis? */
01115   if (byIRR & byAxisMask)                          /* check latched index pulse */
01116     return 1;
01117   return 0;
01118 
01119   /* */
01120   /* a faster, but messier way */
01121   /* */
01122   /*fOutP(stg->wBaseAddress + OCW3, 0x0a);           // IRR on next read */
01123   /* */
01124   /*return (   fInP(stg->wBaseAddress + IRR) */
01125   /*         & ( (stg->byIndexPollAxis & 1) ? LIXODD : LIXEVN ) // mask for even or odd */
01126   /*       ); */
01127 }
01128 
01129 unsigned char GetIndexLatches(STG_STRUCT *stg)
01130 {
01131   /* routine for Model 2 board */
01132 
01133   return fInP(stg->wBaseAddress + IDL);
01134 }
01135 
01136 /***************************************************************************/
01137 /*                                                                         */
01138 /*                                RawDI                                    */
01139 /*                                                                         */
01140 /***************************************************************************/
01141 
01142 unsigned long RawDI(STG_STRUCT *stg)
01143 {
01144   IO32 xInBits;
01145 
01146   if (stg->wNoBoardFlag == NO_BOARD)
01147     {
01148       xInBits.all = 0;
01149       return(xInBits.all);
01150     }
01151 
01152   if (stg->wModel == MODEL1)
01153     {
01154       xInBits.port.A = fInP(stg->wBaseAddress + DIO_A);
01155       xInBits.port.B = fInP(stg->wBaseAddress + DIO_B);
01156       xInBits.port.C = fInP(stg->wBaseAddress + DIO_C);
01157       xInBits.port.D = fInP(stg->wBaseAddress + DIO_D);
01158     }
01159   else  /* Model 2 */
01160     {
01161       xInBits.port.A = fInP(stg->wBaseAddress + PORT_A);
01162       xInBits.port.B = fInP(stg->wBaseAddress + PORT_B);
01163       xInBits.port.C = fInP(stg->wBaseAddress + PORT_C);
01164       xInBits.port.D = fInP(stg->wBaseAddress + PORT_D);
01165     }
01166   return (xInBits.all);
01167 };
01168 
01169 /***************************************************************************/
01170 /*                                                                         */
01171 /*                                RawDO                                    */
01172 /*                                                                         */
01173 /***************************************************************************/
01174 
01175 void RawDO(STG_STRUCT *stg, unsigned long lOutBits)
01176 {
01177   if (stg->wNoBoardFlag == NO_BOARD)
01178     {
01179       return;
01180     }
01181   if (stg->wModel == MODEL1)
01182     {
01183       fOutP(stg->wBaseAddress + DIO_A, ((IO32 *)&lOutBits)->port.A);
01184       fOutP(stg->wBaseAddress + DIO_B, ((IO32 *)&lOutBits)->port.B);
01185       fOutP(stg->wBaseAddress + DIO_C, ((IO32 *)&lOutBits)->port.C);
01186       fOutP(stg->wBaseAddress + DIO_D, ((IO32 *)&lOutBits)->port.D);
01187     }
01188   else  /* Model 2 */
01189     {
01190       fOutP(stg->wBaseAddress + PORT_A, ((IO32 *)&lOutBits)->port.A);
01191       fOutP(stg->wBaseAddress + PORT_B, ((IO32 *)&lOutBits)->port.B);
01192       fOutP(stg->wBaseAddress + PORT_C, ((IO32 *)&lOutBits)->port.C);
01193       fOutP(stg->wBaseAddress + PORT_D, ((IO32 *)&lOutBits)->port.D);
01194     }
01195 };
01196 
01197 void RawDOPort(STG_STRUCT *stg, unsigned char byBitNumber, unsigned char bySet0or1, unsigned short nPort)
01198 {
01199   unsigned nOffset;
01200   unsigned char byData;
01201 
01202   if (stg->wNoBoardFlag == NO_BOARD)
01203     {
01204       return;
01205     }
01206 
01207   if (nPort > 3)
01208     return;
01209 
01210   if (stg->wModel == MODEL1)
01211     nOffset = aPortOffset_1[nPort];
01212   else  /* Model 2 */
01213     nOffset = aPortOffset_2[nPort];
01214 
01215   byData = fInP(stg->wBaseAddress + nOffset);
01216   if (bySet0or1 == 1)
01217     byData |= 1 << byBitNumber;
01218   else
01219     byData &= ~(1u << byBitNumber);
01220   fOutP(stg->wBaseAddress + nOffset, byData);
01221 };
01222 
01223 /* these next functions use the same constant that the direction functions, */
01224 /* DioDirection(...) use.  These are prefered, since the same constant can */
01225 /* be used in both situations, hopefully simplifying the code. */
01226 
01227 /* helper function */
01228 short PortBits2Index(short nPort)
01229 {
01230   int nPortIndex = 9;
01231 
01232   switch(nPort)
01233     {
01234     case STG_PORT_A:
01235       nPortIndex = 0;
01236       break;
01237     case STG_PORT_B:
01238       nPortIndex = 1;
01239       break;
01240     case STG_PORT_C_LO:
01241     case STG_PORT_C_HI:
01242     case STG_PORT_C:
01243       nPortIndex = 2;
01244       break;
01245     case STG_PORT_D_LO:
01246     case STG_PORT_D_HI:
01247     case STG_PORT_D:
01248       nPortIndex = 3;
01249       break;
01250     }
01251   return nPortIndex;
01252 }
01253 
01254 /***************************************************************************/
01255 /*                                                                         */
01256 /*                            DioDirection                                 */
01257 /*                                                                         */
01258 /***************************************************************************/
01259 
01260 /*                                        Don't care. */
01261 /*                                        |<------>| */
01262 /* bits in nSwDir set direction of ports: xxxxxxxxxxbbbbbb */
01263 /*                                                  |||||| */
01264 /*                                                  |||||Port A */
01265 /*       1 = Output                                 ||||PortB */
01266 /*       0 = Input                                  |||Port C, high nibble */
01267 /*                                                  ||Port C, low nibble */
01268 /*                                                  |Port D (port D, low nibble) */
01269 /*                                                  |(Port D, high nibble) */
01270 
01271 void DioDirection(STG_STRUCT *stg, const unsigned short nSwDir)
01272 {
01273   unsigned char byHwDir;                   /* direction bits for hardware */
01274   unsigned long lCurrentData;
01275 
01276   if (stg->wNoBoardFlag == NO_BOARD)
01277     {
01278       return;
01279     }
01280 
01281   /* get the current data in the I/O ports.  We'll replace it when we're */
01282   /* done.  When you change a port to output, it will start at what the */
01283   /* input was (usually high).  This way, bits won't change (except for a */
01284   /* glitch) when a port is set to output. */
01285 
01286   lCurrentData = RawDI(stg);
01287 
01288   byHwDir = 0x9b;                          /* initially all ports input */
01289 
01290   if (nSwDir & STG_PORT_A)                 /* check the bit for A out */
01291     byHwDir &= ~A_DIR_BIT;                  /* if output, set bit to 0 */
01292   if (nSwDir & STG_PORT_B)
01293     byHwDir &= ~B_DIR_BIT;
01294   if (nSwDir & STG_PORT_C_LO)
01295     byHwDir &= ~C_LOW_DIR_BIT;
01296   if (nSwDir & STG_PORT_C_HI)
01297     byHwDir &= ~C_HI_DIR_BIT;
01298 
01299   fOutP(stg->wBaseAddress + ABC_DIR, byHwDir); /* set direction for A, B and C */
01300 
01301   SetDDir(stg, nSwDir);
01302   stg->wSaveDirs = nSwDir;                     /* save, mostly for other function */
01303   RawDO(stg, lCurrentData);
01304 }
01305 
01306 void SetDDir(STG_STRUCT *stg, unsigned short nSwDir)
01307 {
01308   unsigned char byHwDir;                   /* direction bits for hardware */
01309   unsigned char bySaveReg, bySaveIMR, bySaveCntrl1, bySaveCntrl0;
01310 
01311   if (stg->wModel == MODEL1)
01312     {
01313       bySaveReg = fInP(stg->wBaseAddress + INTC);   /* INTC needs to be saved, because */
01314       /* MIO_2 reinitializes the 8255 which */
01315       /* implements the INTC register. */
01316       byHwDir = 0x92;                          /* initialize port D input */
01317       if (nSwDir & STG_PORT_D)                 /* is port D output? */
01318         byHwDir &= ~D_DIR_BIT;              /* if yes, set bit to 0, */
01319       bySaveIMR = fInP(stg->wBaseAddress + IMR);    /* get the current interrupt mask */
01320       fOutP(stg->wBaseAddress + OCW1, 0xff);        /* mask off all interrupts */
01321       fOutP(stg->wBaseAddress + MIO_2, byHwDir);    /* set direction for port D */
01322       fOutP(stg->wBaseAddress + INTC, bySaveReg);   /* restore interrupt control reg. */
01323       fOutP(stg->wBaseAddress + OCW1, bySaveIMR);   /* restore interrupt mask */
01324     }
01325   else  /* Model 2 */
01326     {
01327       bySaveCntrl0 = fInP(stg->wBaseAddress + CNTRL0); /* CNTRL0 needs to be saved, because */
01328       /* D_DIR reinitializes the 8255 which */
01329       /* implements the CNTRL0 register. */
01330       byHwDir = 0x8b;                          /* initialize CNTRL0 as output reg. */
01331       /* BRDTST to input. */
01332       /* sets port D, high and low, to input */
01333       if (nSwDir & STG_PORT_D_LO)              /* low nibble */
01334         byHwDir &= ~D_LOW_DIR_BIT;
01335       if (nSwDir & STG_PORT_D_HI)              /* high nibble */
01336         byHwDir &= ~D_HI_DIR_BIT;
01337 
01338       bySaveCntrl1 = fInP(stg->wBaseAddress+CNTRL1);/* save for interrupt enables */
01339 
01340       /* don't reset any latches; put in slave state; */
01341       /* disable interrupts, so the glitch in CTRL0 doesn't */
01342       /* cause an interrupt on wrong irq */
01343       fOutP(stg->wBaseAddress + CNTRL1, 0xf0);
01344 
01345       fOutP(stg->wBaseAddress + D_DIR, byHwDir);    /* set port D direction */
01346 
01347       /* restore CNTRL0, because it was re-initialized, which */
01348       /* lost any previous contents. */
01349       fOutP(stg->wBaseAddress + CNTRL0, bySaveCntrl0);
01350 
01351       /* re-enable interrupts, and restore slave state, don't */
01352       /* reset any latches. (1111xxxx) */
01353       fOutP(stg->wBaseAddress+CNTRL1, (bySaveCntrl1 & 0x0f) | 0xf0);
01354     }
01355 };
01356 
01357 /***************************************************************************/
01358 /*                                                                         */
01359 /*                               MotSim                                    */
01360 /*                                                                         */
01361 /***************************************************************************/
01362 /* a motor simulator */
01363 /* */
01364 /* takes it's input from stg->lSimDac[] */
01365 /* writes it's output to stg->lSimEnc[] */
01366 /* */
01367 /* This isn't used in the DOS program, it's only used in the Windows 95 driver */
01368 
01369 void MotSim(STG_STRUCT *stg)
01370 {
01371   static long lState_1[MAX_AXIS] = {0};                    /* state variables */
01372   long lScaledUp;
01373   const short nScale = 10;
01374   int nAxis;
01375 
01376   for (nAxis = 0; nAxis < MAX_AXIS; nAxis++)
01377     {
01378       /* The input is guaranteed to be +/- 12 bits */
01379       /* Scale up state so we don't loose resolution */
01380       lScaledUp = stg->lSimDac[nAxis] << nScale;
01381 
01382       /* note: I assume right shift is sign preserving (for signed value) */
01383 
01384       /* lag */
01385       lState_1[nAxis] += (lScaledUp - lState_1[nAxis]) >> (4 + nAxis); /*lint !e704 */
01386       /*   ^^^^^^^ time constant */
01387       /* is different for each axis  */
01388 
01389       /* integrator (shift out the scale factor and then some) */
01390       stg->lSimEnc[nAxis] += lState_1[nAxis] >> (nScale + 1);               /*lint !e704 */
01391     }
01392 }
01393 
01394 void AutoZeroAdc(STG_STRUCT *stg)
01395 {
01396   /* set the Analog to Digital converter to autozero on each conversion */
01397 
01398   if (stg->wModel == MODEL1)
01399     fOutP(stg->wBaseAddress + INTC, fInP(stg->wBaseAddress + INTC) & ~AUTOZERO);
01400   else   /* MODEL2 */
01401     fOutP(stg->wBaseAddress + CNTRL0, fInP(stg->wBaseAddress + CNTRL0) | CNTRL0_AZ);
01402 }
01403 
01404 void DontAutoZeroAdc(STG_STRUCT *stg)
01405 {
01406   /* set the Analog to Digital converter to NOT autozero */
01407 
01408   if (stg->wModel == MODEL1)
01409     fOutP(stg->wBaseAddress + INTC, fInP(stg->wBaseAddress + INTC) | AUTOZERO);
01410   else   /* MODEL2 */
01411     fOutP(stg->wBaseAddress + CNTRL0, fInP(stg->wBaseAddress + CNTRL0) & ~CNTRL0_AZ);
01412 }
01413 
01414 void CalADC(STG_STRUCT *stg)
01415 {
01416   /* Start calibration cycle on ADC chip */
01417 
01418   unsigned char Cntrl0;
01419 
01420   if (stg->wModel == MODEL2)   /* this function only in Model 2 board. */
01421     {
01422       Cntrl0 = fInP(stg->wBaseAddress + CNTRL0) & 0x07;       /* save irq */
01423       fOutP(stg->wBaseAddress + CNTRL0, Cntrl0);              /* cal is low */
01424       /* cal pulse should be 60 ns.  The ISA bus is 10 MHz. or 100 ns. */
01425       /* so we can just set it back high. */
01426       fOutP(stg->wBaseAddress + CNTRL0, Cntrl0 | 0x08);       /* cal is high */
01427     }
01428 }
01429 
01430 /***************************************************************************/
01431 /*                                                                         */
01432 /*                            SetEncoderCounts                             */
01433 /*                                                                         */
01434 /***************************************************************************/
01435 void SetEncoderCounts(STG_STRUCT *stg, unsigned short nAxis, long lCounts)
01436 {
01437   unsigned short wAddress;
01438   char *ByteUnion = (char *)&lCounts;  /* get pointer to lCounts, so you */
01439   /* can take it apart byte by byte */
01440 
01441   wAddress = stg->wBaseAddress + CNT0_D;
01442   wAddress += (nAxis & 0x6) << 1; /* shift to multiply by 2 */
01443   /* pairs of data regs seperated by pairs */
01444   /* of control regs, skip over control. */
01445   wAddress += nAxis & 1;
01446   fOutP(wAddress, ByteUnion[0]);
01447   fOutP(wAddress, ByteUnion[1]);
01448   fOutP(wAddress, ByteUnion[2]);
01449 
01450   /* transfer the preset register to the count register */
01451   fOutP(wAddress + 2, 0x09);
01452 
01453   /* set things for the part that extends the 24 bit counter */
01454   /* to 32 bits. */
01455   stg->byEncHighByte[nAxis] = ByteUnion[3];
01456   stg->byOldByte2[nAxis] = ByteUnion[2];
01457 }
01458 
01459 /***************************************************************************/
01460 /*                                                                         */
01461 /*                            EncoderLatch                                 */
01462 /*                                                                         */
01463 /***************************************************************************/
01464 void EncoderLatch(STG_STRUCT *stg)
01465 {
01466   if (stg->wNoBoardFlag == NO_BOARD)
01467     {
01468       return;
01469     }
01470 
01471   /* normally you'll have the timer latch the data in hardware, but */
01472   /* if the timer isn't running, we need to latch it ourselves. */
01473 
01474   /* BUG FIX-- don't go past 4 axes on 4 axis board */
01475   fOutPW(stg->wBaseAddress + CNT0_C, 0x0303);
01476   fOutPW(stg->wBaseAddress + CNT2_C, 0x0303);
01477   if (stg->wAxesInSys > 4) {
01478     fOutPW(stg->wBaseAddress + CNT4_C, 0x0303);
01479     fOutPW(stg->wBaseAddress + CNT6_C, 0x0303);
01480   }
01481 };
01482 
01483 /***************************************************************************/
01484 /*                                                                         */
01485 /*                          EncoderResetAddr                               */
01486 /*                                                                         */
01487 /***************************************************************************/
01488 void EncoderResetAddr(STG_STRUCT *stg)
01489 {
01490   if (stg->wNoBoardFlag == NO_BOARD)
01491     {
01492       return;
01493     }
01494 
01495   /* This function resets all the counter's internal address pointers to point */
01496   /* to the first byte in the 3 byte sequence */
01497 
01498   /* BUG FIX-- don't go past 4 axes on 4 axis board */
01499   fOutPW(stg->wBaseAddress + CNT0_C, 0x0101);
01500   fOutPW(stg->wBaseAddress + CNT2_C, 0x0101);
01501   if (stg->wAxesInSys > 4) {
01502     fOutPW(stg->wBaseAddress + CNT4_C, 0x0101);
01503     fOutPW(stg->wBaseAddress + CNT6_C, 0x0101);
01504   }
01505 };
01506 
01507 unsigned char GetSELDI(STG_STRUCT *stg)
01508 {
01509   return fInP(stg->wBaseAddress + SELDI);
01510 }
01511 
01512 unsigned char GetIDLEN(STG_STRUCT *stg)
01513 {
01514   return fInP(stg->wBaseAddress + IDLEN);
01515 }
01516 
01517 unsigned char GetCNTRL0(STG_STRUCT *stg)
01518 {
01519   return fInP(stg->wBaseAddress + CNTRL0);
01520 }
01521 
01522 unsigned char GetCNTRL1(STG_STRUCT *stg)
01523 {
01524   return fInP(stg->wBaseAddress + CNTRL1);
01525 }
01526 
01527 void ResetWatchdogLatch(STG_STRUCT *stg)
01528 {
01529   unsigned char byCntrl1 = fInP(stg->wBaseAddress + CNTRL1);
01530   byCntrl1 &= ~CNTRL1_WDTOUT;                   /*set bit low, to reset */
01531   /* don't reset other latches */
01532   byCntrl1 |= CNTRL1_INT_G2 | CNTRL1_INT_T2 | CNTRL1_INT_T0;
01533   fOutP(stg->wBaseAddress + CNTRL1, byCntrl1);
01534 }
01535 
01536 unsigned char GetBRDTST(STG_STRUCT *stg)
01537 {
01538   return fInP(stg->wBaseAddress + BRDTST);
01539 }
01540 
01541 unsigned short GetBoardPresence(STG_STRUCT *stg)
01542 {
01543   return stg->wNoBoardFlag;
01544 };
01545 
01546 unsigned short GetAxes(STG_STRUCT *stg)
01547 {
01548   return stg->wAxesInSys;
01549 };
01550 
01551 unsigned short GetIrq(STG_STRUCT *stg)
01552 {
01553   return stg->wIrq;
01554 };
01555 
01556 unsigned short GetAddr(STG_STRUCT *stg)
01557 {
01558   return stg->wBaseAddress;
01559 };
01560 
01561 unsigned short GetModel(STG_STRUCT *stg)
01562 {
01563   return stg->wModel;
01564 };
01565 
01566 /***************************************************************************/
01567 /*                                                                         */
01568 /*                            IndexPulse                                   */
01569 /*                                                                         */
01570 /***************************************************************************/
01571 short IndexPulse(STG_STRUCT *stg)
01572 {
01573   /* poll for the index pulse of the axis that was previously set up. */
01574   /* Normally you would look at the latched pulse.  This function will */
01575   /* probably only get used during testing. */
01576 
01577   unsigned char byIRR, byAxisMask;
01578 
01579   byIRR = CurrentIRR(stg);
01580   byAxisMask = (stg->byIndexPollAxis & 1) ? IXODD : IXEVN;  /* even or odd axis? */
01581 
01582     /* The raw index pulse isn't inverted by the hardware if the index pulse is */
01583     /* low active (only the latched pulse is).  For consistancy, we'll invert */
01584     /* the pulse in software. */
01585 
01586   if (stg->byIndexPulsePolarity == 0)                       /* if pulse is low true */
01587     byIRR ^= byAxisMask;                             /* flip bit */
01588 
01589   if (byIRR & byAxisMask)                              /* check index pulse */
01590     return 1;
01591   return 0;
01592 }
01593 
01594 /***************************************************************************/
01595 /*                                                                         */
01596 /*                             StopTimer                                   */
01597 /*                                                                         */
01598 /***************************************************************************/
01599 void StopTimer(STG_STRUCT *stg)
01600 {
01601   if (stg->wNoBoardFlag == NO_BOARD)
01602     {
01603       return;
01604     }
01605 
01606   /* stop the timer by putting it into one shot mode, it will never get */
01607   /* a trigger */
01608 
01609   /* bug bug this doesn't work */
01610 
01611   fOutP(stg->wBaseAddress + TMRCMD, 0x0a);    /* timer 0, mode 1 */
01612 }
01613 
01614 /***************************************************************************/
01615 /*                                                                         */
01616 /*                           Timer2 functions                              */
01617 /*                                                                         */
01618 /***************************************************************************/
01619 void Timer2Delay(STG_STRUCT *stg, unsigned short counts)
01620 {
01621   if (stg->wNoBoardFlag == NO_BOARD)
01622     {
01623       return;
01624     }
01625 
01626   StartTimer2TerminalCount(stg, counts);
01627 
01628   while (PollTimer2(stg));
01629 }
01630 
01631 void StartTimer2TerminalCount(STG_STRUCT *stg, unsigned short count)
01632 {
01633   char *pByte = (char *)&count;
01634 
01635   if (stg->wNoBoardFlag == NO_BOARD)
01636     {
01637       return;
01638     }
01639 
01640   fOutP(stg->wBaseAddress + TMRCMD, 0xb0);       /* timer 2, read/load LSB followed */
01641   /* by MSB, mode 0 (terminal count) */
01642   fOutP(stg->wBaseAddress + TIMER_2, *pByte++);  /* LSB (little endian) */
01643   fOutP(stg->wBaseAddress + TIMER_2, *pByte);    /* MSB */
01644 }
01645 
01646 void StartTimer2RTI(STG_STRUCT *stg, unsigned short count)
01647 {
01648   char *pByte = (char *)&count;
01649 
01650   if (stg->wNoBoardFlag == NO_BOARD)
01651     {
01652       return;
01653     }
01654 
01655   fOutP(stg->wBaseAddress + TMRCMD, 0xb4);       /* timer 2, read/load LSB followed */
01656   /* by MSB, mode 2 (real time int). */
01657   fOutP(stg->wBaseAddress + TIMER_2, *pByte++);  /* LSB (little endian) */
01658   fOutP(stg->wBaseAddress + TIMER_2, *pByte);    /* MSB */
01659 }
01660 
01661 unsigned short ReadTimer2TerminalCount(STG_STRUCT *stg)
01662 {
01663   unsigned short count;
01664   char *pByte = (char *)&count;
01665   fOutP(stg->wBaseAddress + TMRCMD, 0x80);       /* timer 2, latch */
01666   *pByte++ = fInP(stg->wBaseAddress + TIMER_2);  /* LSB */
01667   *pByte   = fInP(stg->wBaseAddress + TIMER_2);  /* MSB */
01668   return count;
01669 }
01670 
01671 short PollTimer2(STG_STRUCT *stg)
01672 {
01673   if (stg->wNoBoardFlag == NO_BOARD)
01674     {
01675       return 0;
01676     }
01677 
01678   if (stg->wModel == MODEL1)
01679     return !(CurrentIRR(stg) & TP2);  /* mask selects bit for TP2 */
01680   else   /* MODEL 2 */
01681     {
01682       unsigned char byRegCntrl1, byNewCntrl1;
01683 
01684       byRegCntrl1 = fInP(stg->wBaseAddress + CNTRL1);
01685       if (byRegCntrl1 & CNTRL1_INT_T2)
01686         {
01687           /* If it's set, we want to reset the latch, by writing a zero */
01688           /* to CNTRL1_INT_T2. */
01689 
01690           /* When we write to CNTRL1, we don't want to change SLAVE, IEN_G2 */
01691           /* IEN_T2, or IEN_T0 -- the lower 4 bits.  So, we start with */
01692 
01693           byNewCntrl1 = byRegCntrl1 & 0x0f;     /* (0000xxxx) */
01694 
01695           /* We don't want to reset WDTOUT, INT_G2, or INT_T0--we only want */
01696           /* to reset ...T2. (1101xxxx) */
01697 
01698           byNewCntrl1 |= CNTRL1_WDTOUT | CNTRL1_INT_G2 | CNTRL1_INT_T0;
01699 
01700           fOutP(stg->wBaseAddress + CNTRL1, byNewCntrl1);  /* reset bit */
01701 
01702           return 1;  /* true */
01703         }
01704     }
01705   return 0;      /* false */
01706 }
01707 
01708 void MaskTimer2Interrupt(STG_STRUCT *stg)
01709 {
01710   if (stg->wNoBoardFlag == NO_BOARD)
01711     {
01712       return;
01713     }
01714 
01715   if (stg->wModel == MODEL1)
01716     fOutP(stg->wBaseAddress + OCW1, CurrentIRR(stg) | TP2);
01717   else   /* Model 2 */
01718     /* we want to save the state of the slave mode, and set the */
01719     /* high nibble bits high, so you don't reset any latches. */
01720     /* bit pattern: 1111x000  where x is don't change */
01721     fOutP(stg->wBaseAddress + CNTRL1,
01722           (fInP(stg->wBaseAddress + CNTRL1) & CNTRL1_NOT_SLAVE) | 0xf0);
01723 }
01724 
01725 void UnMaskTimer2Interrupt(STG_STRUCT *stg)
01726 {
01727   if (stg->wModel == MODEL1)
01728     fOutP(stg->wBaseAddress + OCW1, CurrentIRR(stg) & ~TP2);
01729   else   /* Model 2 */
01730     {
01731       /* we want to save the state of the slave mode, and set the */
01732       /* high nibble bits high, so you don't reset any latches. */
01733       /* bit pattern: 1111x010 where x is don't change */
01734       fOutP(stg->wBaseAddress + CNTRL1,
01735             (fInP(stg->wBaseAddress + CNTRL1) & CNTRL1_NOT_SLAVE) | CNTRL1_IEN_T2);
01736     }
01737 }
01738 
01739 /***************************************************************************/
01740 /*                                                                         */
01741 /*                           StartInterrupts                               */
01742 /*                                                                         */
01743 /***************************************************************************/
01744 void StartInterrupts(STG_STRUCT *stg)
01745 {
01746   if (stg->wNoBoardFlag == NO_BOARD)
01747     {
01748       return;
01749     }
01750   if (stg->wModel == MODEL1)
01751     fOutP(stg->wBaseAddress + OCW1, ~0x04);   /* enable interrupt for timer 0 */
01752   else  /* MODEL2 */
01753     /* we want to save the state of the slave mode, and set the */
01754     /* high nibble bits high, so you don't reset any latches. */
01755     /* 1111x001, where x means don't change */
01756     {
01757       fOutP(stg->wBaseAddress + CNTRL1, CNTRL1_IEN_T0 | CNTRL1_NOT_SLAVE);
01758     }
01759 };
01760 
01761 /***************************************************************************/
01762 /*                                                                         */
01763 /*                           StopInterrupts                                */
01764 /*                                                                         */
01765 /***************************************************************************/
01766 void StopInterrupts(STG_STRUCT *stg)
01767 {
01768   if (stg->wNoBoardFlag == NO_BOARD)
01769     {
01770       return;
01771     }
01772   if (stg->wModel == MODEL1)
01773     fOutP(stg->wBaseAddress + OCW1, 0xff);     /* disable all interrupts */
01774   else  /* MODEL2 */
01775     /* we want to save the state of the slave mode, and set the */
01776     /* high nibble bits high, so you don't reset any latches. */
01777     /* disable all interrupts, since only one can be enabled at */
01778     /* a time (currently).  If more than one was enabled, it's */
01779     /* an error (currently). */
01780     /* 1111x000, where x means not changed. */
01781     fOutP(stg->wBaseAddress + CNTRL1,
01782           (fInP(stg->wBaseAddress + CNTRL1) & CNTRL1_NOT_SLAVE) | 0xf0);
01783 };
01784 
01785 /* Implementations of external functions */
01786 
01787 /* our global STG_STRUCT */
01788 /* FIXME-- need several, in general, for several boards */
01789 static STG_STRUCT theStg;
01790 
01791 /* saved outputs, for stgAioCheck(), since we can't really read outputs */
01792 static double checkedOutputs[STG_MAX_AXIS];
01793 
01794 #define STG_INIT_WAIT 1000      /* usecs to wait after stgMotInit() */
01795 int stgMotInit(const char * stuff)
01796 {
01797   int t;
01798 
01799   /* call STG init, which is the "constructor" */
01800   ServoToGoConstructor(&theStg, 5);
01801   StopInterrupts(&theStg);      /* we don't want interrupts */
01802 
01803   /* output 0's to amps */
01804   for (t = 0; t < STG_MAX_AXIS; t++)
01805     {
01806       /* write 0 to DACs */
01807       stgDacWrite(t, 0.0);
01808     }
01809 
01810   /* init digital IO directions */
01811   stgDioInit(0);
01812 
01813   /* need to force a wait here-- calls to stgEncoderReadAll()
01814      immediately after this return garbage. Is there status to
01815      poll on for 'ready'? */
01816   for (t = 0; t < STG_INIT_WAIT; t++)
01817     {
01818       /* do a dummy 1 usec write */
01819       _outp(0x80, 0);
01820     }
01821 
01822   return 0;
01823 }
01824 
01825 int stgMotQuit(void)
01826 {
01827   int t;
01828 
01829   for (t = 0; t < STG_MAX_AXIS; t++)
01830     {
01831       /* write 0's to DACs */
01832       stgDacWrite(t, 0.0);
01833     }
01834 
01835   ServoToGoDestructor(&theStg);
01836 
01837   return 0;
01838 }
01839 
01840 int stgAdcNum(void)
01841 {
01842   return STG_MAX_AXIS;
01843 }
01844 
01845 int stgAdcStart(int adc)
01846 {
01847   if (adc < 0 ||
01848       adc >= STG_MAX_AXIS) {
01849     return 0;                 /* don't care */
01850   }
01851 
01852   StartADC(&theStg, (unsigned short) adc);
01853 
01854   return 0;
01855 }
01856 
01857 #define STG_ADC_WAIT_USEC 20    /* microsecs to wait for conversion */
01858 
01859 void stgAdcWait(void)
01860 {
01861   int t;
01862 
01863   for (t = 0; t < STG_ADC_WAIT_USEC; t++) {
01864     _outp(0x80, 0);             /* 0x80 is historical dummy, 1 usec write */
01865   }
01866 }
01867 
01868 int stgAdcRead(int adc, double * volts)
01869 {
01870   if (adc < 0 ||
01871       adc >= STG_MAX_AXIS) {
01872     return 0;                 /* don't care */
01873   }
01874 
01875   *volts = (double) SpinReadADC(&theStg, (unsigned short) adc);
01876 
01877   return 0;
01878 }
01879 
01880 int stgDacNum(void)
01881 {
01882   return STG_MAX_AXIS;
01883 }
01884 
01885 int stgDacWrite(int dac, double volts)
01886 {
01887   long lCounts;
01888 
01889   if (dac < 0 ||
01890       dac >= STG_MAX_AXIS)
01891     {
01892       return 0;                 /* don't care */
01893     }
01894 
01895   checkedOutputs[dac] = volts;
01896 
01897   // WPS -- Hack: This made little sense to be but this hack seems necessary.
01898   lCounts = (long) (( volts * 0x1FFF) / 20.0);
01899 
01900   RawDAC(&theStg, dac, lCounts);
01901 
01902   return 0;
01903 }
01904 
01905 
01906 int stgDacWriteAll(int max, double * volts)
01907 {
01908   int t;
01909   int smax;
01910 
01911   /* clip smax to max supported-- if they want more, ignore */
01912   if (max > STG_MAX_AXIS) {
01913     smax = STG_MAX_AXIS;
01914   }
01915   else {
01916     smax = max;
01917   }
01918 
01919   for (t = 0; t < smax; t++) {
01920     if (0 != stgDacWrite(t, volts[t])){
01921       return -1;
01922     }
01923   }
01924 
01925   return 0;
01926 }
01927 
01928 unsigned int stgEncoderIndexModel(void)
01929 {
01930   return EXT_ENCODER_INDEX_MODEL_MANUAL;
01931 }
01932 
01933 int stgEncoderSetIndexModel(unsigned int model)
01934 {
01935   if (model != EXT_ENCODER_INDEX_MODEL_MANUAL)
01936     {
01937       return -1;
01938     }
01939 
01940   return 0;
01941 }
01942 
01943 int stgEncoderNum(void)
01944 {
01945   return STG_MAX_AXIS;
01946 }
01947 
01948 int stgEncoderRead(int encoder, double * counts)
01949 {
01950   double allCounts[STG_MAX_AXIS];
01951 
01952   if (encoder < 0 ||
01953       encoder >= STG_MAX_AXIS) {
01954     *counts = 0.0;
01955     return 0;
01956   }
01957 
01958   stgEncoderReadAll(encoder + 1, allCounts);
01959 
01960   *counts = allCounts[encoder];
01961 
01962   return 0;
01963 }
01964 
01965 int stgEncoderReadAll(int max, double * counts)
01966 {
01967   LONGBYTE lbEnc[MAX_AXIS];
01968   int t;
01969   int smax;                     /* how many we actually have */
01970 
01971   /* clip smax to max supported-- if they want more, give
01972      them zeros */
01973   if (max > STG_MAX_AXIS) {
01974     smax = STG_MAX_AXIS;
01975   }
01976   else {
01977     smax = max;
01978   }
01979 
01980   EncoderLatch(&theStg);
01981   EncReadAll(&theStg, lbEnc);
01982 
01983   /* fill ours with the actual values */
01984   for (t = 0; t < smax; t++) {
01985     counts[t] = (double) lbEnc[t].Long;
01986   }
01987   /* and fill the rest with zeros */
01988   for (t = smax; t < max; t++) {
01989     counts[t] = 0.0;
01990   }
01991 
01992   return 0;
01993 }
01994 
01995 int stgEncoderResetIndex(int encoder)
01996 {
01997   if (encoder < 0 ||
01998       encoder >= STG_MAX_AXIS)
01999     {
02000       return 0;
02001     }
02002 
02003   SelectIndexAxis(&theStg, encoder, 1);
02004 
02005   return 0;
02006 }
02007 
02008 int stgEncoderReadLatch(int encoder, int * flag)
02009 {
02010   *flag = IndexPulseLatch(&theStg);
02011 
02012   return 0;
02013 }
02014 
02015 /* basically same code as IndexPulseLatch() except mask is 2 bits higher */
02016 int stgEncoderReadLevel(int encoder, int * flag)
02017 {
02018   unsigned char byIRR, byAxisMask;
02019 
02020   byIRR = CurrentIRR(&theStg);
02021   byAxisMask = (theStg.byIndexPollAxis & 1) ? IXODD : IXEVN;
02022 
02023   if (byIRR & byAxisMask)
02024     {
02025       *flag = 1;
02026     }
02027   else
02028     {
02029       *flag = 0;
02030     }
02031 
02032   return 0;
02033 }
02034 
02035 /* Note: board only supports 6 axes max of digital in, out for limit and
02036    home switches, amp enables  */
02037 
02038 /* digital input bit masks, 32-bit word */
02039 /* maps to ports A and B */
02040 #define HOME_0    0x00000001
02041 #define MIN_LIM_0 0x00000002
02042 #define MAX_LIM_0 0x00000004
02043 #define FAULT_0   0x00000008
02044 #define HOME_1    0x00000010
02045 #define MIN_LIM_1 0x00000020
02046 #define MAX_LIM_1 0x00000040
02047 #define FAULT_1   0x00000080
02048 #define HOME_2    0x00000100
02049 #define MIN_LIM_2 0x00000200
02050 #define MAX_LIM_2 0x00000400
02051 #define FAULT_2   0x00000800
02052 #define HOME_3    0x00001000
02053 #define MIN_LIM_3 0x00002000
02054 #define MAX_LIM_3 0x00004000
02055 #define FAULT_3   0x00008000
02056 
02057 #ifdef STG_8_AXES
02058 /* skip C, maps to port D */
02059 #define HOME_4    0x01000000
02060 #define MIN_LIM_4 0x02000000
02061 #define MAX_LIM_4 0x04000000
02062 #define FAULT_4   0x08000000
02063 #define HOME_5    0x10000000
02064 #define MIN_LIM_5 0x20000000
02065 #define MAX_LIM_5 0x40000000
02066 #define FAULT_5   0x80000000
02067 
02068 #endif
02069 
02070 /* no space for axes 7 and 8, so these won't go through */
02071 int stgMaxLimitSwitchRead(int axis, int * flag)
02072 {
02073   long bits;
02074   int retval = 0;
02075 
02076   bits = RawDI(&theStg);
02077   *flag = 0;
02078 
02079   switch (axis)
02080     {
02081     case 0:
02082       if (bits & MAX_LIM_0)
02083         {
02084           *flag = 1;
02085         }
02086       break;
02087 
02088     case 1:
02089       if (bits & MAX_LIM_1)
02090         {
02091           *flag = 1;
02092         }
02093       break;
02094 
02095     case 2:
02096       if (bits & MAX_LIM_2)
02097         {
02098           *flag = 1;
02099         }
02100       break;
02101 
02102     case 3:
02103       if (bits & MAX_LIM_3)
02104         {
02105           *flag = 1;
02106         }
02107       break;
02108 
02109 #ifdef STG_8_AXES
02110     case 4:
02111       if (bits & MAX_LIM_4)
02112         {
02113           *flag = 1;
02114         }
02115       break;
02116 
02117     case 5:
02118       if (bits & MAX_LIM_5)
02119         {
02120           *flag = 1;
02121         }
02122       break;
02123 
02124 #endif
02125 
02126     default:
02127       retval = -1;
02128       break;
02129     }
02130 
02131   return retval;
02132 }
02133 
02134 /* no space for axes 7 and 8, so these won't go through */
02135 int stgMinLimitSwitchRead(int axis, int * flag)
02136 {
02137   long bits;
02138   int retval = 0;
02139 
02140   bits = RawDI(&theStg);
02141   *flag = 0;
02142 
02143   switch (axis)
02144     {
02145     case 0:
02146       if (bits & MIN_LIM_0)
02147         {
02148           *flag = 1;
02149         }
02150       break;
02151 
02152     case 1:
02153       if (bits & MIN_LIM_1)
02154         {
02155           *flag = 1;
02156         }
02157       break;
02158 
02159     case 2:
02160       if (bits & MIN_LIM_2)
02161         {
02162           *flag = 1;
02163         }
02164       break;
02165 
02166     case 3:
02167       if (bits & MIN_LIM_3)
02168         {
02169           *flag = 1;
02170         }
02171       break;
02172 
02173 #ifdef STG_8_AXES
02174     case 4:
02175       if (bits & MIN_LIM_4)
02176         {
02177           *flag = 1;
02178         }
02179       break;
02180 
02181     case 5:
02182       if (bits & MIN_LIM_5)
02183         {
02184           *flag = 1;
02185         }
02186       break;
02187 
02188 #endif
02189 
02190     default:
02191       retval = -1;
02192       break;
02193     }
02194 
02195   return retval;
02196 }
02197 
02198 /* no space for axes 7 and 8, so these won't go through */
02199 int stgHomeSwitchRead(int axis, int *flag)
02200 {
02201   long bits;
02202   int retval = 0;
02203 
02204   bits = RawDI(&theStg);
02205   *flag = 0;
02206 
02207   switch (axis)
02208     {
02209     case 0:
02210       if (bits & HOME_0)
02211         {
02212           *flag = 1;
02213         }
02214       break;
02215 
02216     case 1:
02217       if (bits & HOME_1)
02218         {
02219           *flag = 1;
02220         }
02221       break;
02222 
02223     case 2:
02224       if (bits & HOME_2)
02225         {
02226           *flag = 1;
02227         }
02228       break;
02229 
02230     case 3:
02231       if (bits & HOME_3)
02232         {
02233           *flag = 1;
02234         }
02235       break;
02236 
02237 #ifdef STG_8_AXES
02238     case 4:
02239       if (bits & HOME_4)
02240         {
02241           *flag = 1;
02242         }
02243       break;
02244 
02245     case 5:
02246       if (bits & HOME_5)
02247         {
02248           *flag = 1;
02249         }
02250       break;
02251 
02252 #endif
02253 
02254     default:
02255       retval = -1;
02256       break;
02257     }
02258 
02259   return retval;
02260 }
02261 
02262 /* there is space for axes 7 and 8, so these will go through */
02263 int stgAmpEnable(int axis, int enable)
02264 {
02265   if (axis < 0 || axis >= STG_MAX_AXIS)
02266     {
02267       return 0;
02268     }
02269 
02270   RawDOPort(&theStg, axis, (unsigned char) enable, 2); /* 2 is port C */
02271 
02272   return 0;
02273 }
02274 
02275 /* no space for axes 7 and 8, so these won't go through */
02276 int stgAmpFault(int axis, int * flag)
02277 {
02278   long bits;
02279   int retval = 0;
02280 
02281   bits = RawDI(&theStg);
02282   *flag = 0;
02283 
02284   switch (axis)
02285     {
02286     case 0:
02287       if (bits & FAULT_0)
02288         {
02289           *flag = 1;
02290         }
02291       break;
02292 
02293     case 1:
02294       if (bits & FAULT_1)
02295         {
02296           *flag = 1;
02297         }
02298       break;
02299 
02300     case 2:
02301       if (bits & FAULT_2)
02302         {
02303           *flag = 1;
02304         }
02305       break;
02306 
02307     case 3:
02308       if (bits & FAULT_3)
02309         {
02310           *flag = 1;
02311         }
02312       break;
02313 
02314 #ifdef STG_8_AXES
02315     case 4:
02316       if (bits & FAULT_4)
02317         {
02318           *flag = 1;
02319         }
02320       break;
02321 
02322     case 5:
02323       if (bits & FAULT_5)
02324         {
02325           *flag = 1;
02326         }
02327       break;
02328 
02329 #endif
02330 
02331     default:
02332       retval = -1;
02333       break;
02334     }
02335 
02336   return retval;
02337 }
02338 
02339 /*
02340   Analog and Digital IO
02341 
02342   Analog and digital IO are mapped directly onto the board with no care
02343   that they don't collide with axes functions above. This is good in the
02344   sense that you can effect motion, bad in the same sense. This means that
02345   you should leave indices 0..axis-1 alone and use the higher indices for
02346   general-purpose IO. For example, if you have a 4-axis board and only
02347   3 real axes, you can use digital input at indices 12..15 for anything.
02348 
02349   Analog input is an option for the board. There are 8 inputs, which
02350   don't collide with any motion IO so you can use indices 0..7
02351   regardless of how many motion axes you have.
02352 
02353   If you have an 8 axis board, only 6 axis can be fully used for motion
02354   since there is not enough digital input for all the home and limit switches.
02355 
02356   For a 4-axis board, there are:
02357   24 digital inputs (includes limit switches and amp faults at 0..15)
02358   8 digital outputs (includes amp enables at 0..3)
02359   8 analog inputs (with option)
02360   4 analog outputs (includes amp outputs at 0..3)
02361 
02362   For an 8-axis board (6 motion axes max), there are:
02363   24 digital inputs (includes limit switches and amp faults at 0..23)
02364   8 digital outputs (includes amp enables at 0..5)
02365   8 analog inputs (with option)
02366   8 analog outputs (includes amp outputs at 0..5)
02367 
02368   Here's the actual map. If you aren't using some axes then their
02369   slots are up for grabs.
02370 
02371   DIGITAL INPUT
02372   -------------
02373   Index   Function      Connector/Pin
02374   -----   --------      -------------
02375   00      X home sw     P1/47
02376   01      X +lim sw     P1/45
02377   02      X -lim sw     P1/43
02378   03      X amp fault   P1/41
02379   04      Y home sw     P1/39
02380   05      Y +lim sw     P1/37
02381   06      Y -lim sw     P1/35
02382   07      Y amp fault   P1/33
02383   08      Z home sw     P1/31
02384   09      Z +lim sw     P1/29
02385   10      Z -lim sw     P1/27
02386   11      Z amp fault   P1/25
02387   12      U home sw     P1/23
02388   13      U +lim sw     P1/21
02389   14      U -lim sw     P1/19
02390   15      U amp fault   P1/17
02391   16      V home sw     P2/31
02392   17      V +lim sw     P2/29
02393   18      V -lim sw     P2/27
02394   19      V amp fault   P2/25
02395   20      W home sw     P2/23
02396   21      W +lim sw     P2/21
02397   22      W -lim sw     P2/19
02398   23      W amp fault   P2/17
02399 
02400   DIGITAL OUTPUT
02401   -------------
02402   Index   Function      Connector/Pin
02403   -----   --------      -------------
02404   00      X amp enable  P1/15
02405   01      Y amp enable  P1/13
02406   02      Z amp enable  P1/11
02407   03      U amp enable  P1/9
02408   04      V amp enable  P1/7
02409   05      W amp enable  P1/5
02410   06      (none)        P1/3
02411   07      (none)        P1/1
02412 
02413   ANALOG INPUT
02414   -------------
02415   Index   Function      Connector/Pin
02416   -----   --------      -------------
02417   00*     (none)        P2/1
02418   01*     (none)        P2/3
02419   02*     (none)        P2/5
02420   03*     (none)        P2/7
02421   04*     (none)        P2/9
02422   05*     (none)        P2/11
02423   06*     (none)        P2/13
02424   07*     (none)        P2/15
02425 
02426   ANALOG OUTPUT
02427   -------------
02428   Index   Function      Connector/Pin
02429   -----   --------      -------------
02430   00      X amp ref     P3/2
02431   01      Y amp ref     P3/8
02432   02      Z amp ref     P3/5
02433   03      U amp ref     P3/11
02434   04**    V amp ref     P4/2
02435   05**    W amp ref     P4/8
02436   06**    (none)        P4/5
02437   07**    (none)        P4/11
02438 
02439   * = only available as option
02440   ** = only available for 8-axis boards, with STG_8_AXES defined
02441 
02442   */
02443 
02444 int stgDioInit(const char * stuff)
02445 {
02446   /* set digital IO bits for A,B input, C output */
02447   DioDirection(&theStg, 0x0C);
02448 
02449   return 0;
02450 }
02451 
02452 int stgDioQuit(void)
02453 {
02454   return 0;
02455 }
02456 
02457 int stgDioMaxInputs(void)
02458 {
02459   return 24;
02460 }
02461 
02462 int stgDioMaxOutputs(void)
02463 {
02464   return 24;
02465 }
02466 
02467 int stgDioRead(int index, int *value)
02468 {
02469   unsigned char byte;
02470   unsigned char mask;
02471 
02472   mask = 1 << (index % 8);
02473 
02474   /* try A */
02475   if (index >= 0 && index < 8)
02476     {
02477       byte = _inp(STG_BASE_ADDRESS + DIO_A);
02478       *value = (byte & mask ? 1 : 0);
02479       return 0;
02480     }
02481 
02482   /* try B */
02483   if (index >= 8 && index < 16)
02484     {
02485       byte = _inp(STG_BASE_ADDRESS + DIO_B);
02486       *value = (byte & mask ? 1 : 0);
02487       return 0;
02488     }
02489 
02490   /* try D */
02491   if (index >= 16 && index < 24)
02492     {
02493       byte = _inp(STG_BASE_ADDRESS + DIO_D);
02494       *value = (byte & mask ? 1 : 0);
02495       return 0;
02496     }
02497 
02498   /* bad index */
02499   *value = 0;
02500   return -1;
02501 }
02502 
02503 /* writes go to the C register */
02504 int stgDioWrite(int index, int value)
02505 {
02506   if (index < 0 || index >= 8)
02507     {
02508       return -1;
02509     }
02510 
02511   RawDOPort(&theStg, index, (unsigned char) value, 2); /* 2 is port C */
02512 
02513   return 0;
02514 }
02515 
02516 /* we can really read the outputs in the C register */
02517 int stgDioCheck(int index, int *value)
02518 {
02519   unsigned char byte;
02520   unsigned char mask;
02521 
02522   if (index < 0 || index >= 8)
02523     {
02524       *value = 0;
02525       return -1;
02526     }
02527 
02528   byte = _inp(STG_BASE_ADDRESS + DIO_C);
02529   mask = 1 << index;
02530 
02531   *value = (byte & mask ? 1 : 0);
02532 
02533   return 0;
02534 }
02535 
02536 /* can read bytes 0..2 */
02537 int stgDioByteRead(int index, unsigned char *byte)
02538 {
02539   if (index == 0)
02540     {
02541       *byte = _inp(STG_BASE_ADDRESS + DIO_A);
02542       return 0;
02543     }
02544 
02545   if (index == 1)
02546     {
02547       *byte = _inp(STG_BASE_ADDRESS + DIO_B);
02548       return 0;
02549     }
02550 
02551   if (index == 2)
02552     {
02553       *byte = _inp(STG_BASE_ADDRESS + DIO_D);
02554       return 0;
02555     }
02556 
02557   *byte = 0;
02558   return -1;
02559 }
02560 
02561 /* can read shorts 0..1, 0 is BA, 1 is 0D */
02562 int stgDioShortRead(int index, unsigned short *sh)
02563 {
02564   unsigned char lo, hi;
02565 
02566   if (index == 0)
02567     {
02568       lo = _inp(STG_BASE_ADDRESS + DIO_A);
02569       hi = _inp(STG_BASE_ADDRESS + DIO_B);
02570 
02571       *sh = (hi << 8) + lo;
02572       return 0;
02573     }
02574 
02575   if (index == 1)
02576     {
02577       lo = _inp(STG_BASE_ADDRESS + DIO_D);
02578 
02579       *sh = lo;
02580       return 0;
02581     }
02582 
02583   *sh = 0;
02584   return -1;
02585 }
02586 
02587 /* can only read 1 word, and this is 0DBA */
02588 int stgDioWordRead(int index, unsigned int *word)
02589 {
02590   if (index != 0)
02591     {
02592       *word = 0;
02593       return -1;
02594     }
02595 
02596   *word = _inp(STG_BASE_ADDRESS + DIO_D) << 16;
02597   *word += _inp(STG_BASE_ADDRESS + DIO_B) << 8;
02598   *word += _inp(STG_BASE_ADDRESS + DIO_A);
02599 
02600   return 0;
02601 }
02602 
02603 /* index is 0 only for port C */
02604 int stgDioByteWrite(int index, unsigned char byte)
02605 {
02606   if (index == 0)
02607     {
02608       _outp(STG_BASE_ADDRESS + DIO_C, byte);
02609       return 0;
02610     }
02611 
02612   return -1;
02613 }
02614 
02615 /* can't do a short write-- only 1 byte of digital out */
02616 int stgDioShortWrite(int index, unsigned short sh)
02617 {
02618   return -1;
02619 }
02620 
02621 /* can't do a word write-- only 1 byte of digital out */
02622 int stgDioWordWrite(int index, unsigned int word)
02623 {
02624   return -1;
02625 }
02626 
02627 /* can do a byte check on output C register */
02628 int stgDioByteCheck(int index, unsigned char *byte)
02629 {
02630   if (index == 0)
02631     {
02632       *byte = _inp(STG_BASE_ADDRESS + DIO_C);
02633       return 0;
02634     }
02635 
02636   *byte = 0;
02637   return -1;
02638 }
02639 
02640 /* can't read a short-- only 8 bits of digital out */
02641 int stgDioShortCheck(int index, unsigned short *sh)
02642 {
02643   *sh = 0;
02644   return -1;
02645 }
02646 
02647 /* cant' read a word-- only 8 bits of digital out */
02648 int stgDioWordCheck(int index, unsigned int *word)
02649 {
02650   *word = 0;
02651   return -1;
02652 }
02653 
02654 int stgAioInit(const char * stuff)
02655 {
02656   /* nothing need be done */
02657   return 0;
02658 }
02659 
02660 int stgAioQuit(void)
02661 {
02662   return 0;
02663 }
02664 
02665 int stgAioMaxInputs(void)
02666 {
02667   return 8;                     /* make sure you have this option */
02668 }
02669 
02670 int stgAioMaxOutputs(void)
02671 {
02672   return STG_MAX_AXIS;
02673 }
02674 
02675 int stgAioStart(int index)
02676 {
02677   return stgAdcStart(index);
02678 }
02679 
02680 void stgAioWait(void)
02681 {
02682   stgAdcWait();
02683 }
02684 
02685 int stgAioRead(int index, double *volts)
02686 {
02687   return stgAdcRead(index, volts);
02688 }
02689 
02690 int stgAioWrite(int index, double volts)
02691 {
02692   return stgDacWrite(index, volts);
02693 }
02694 
02695 int stgAioCheck(int index, double *volts)
02696 {
02697   if (index < 0 || index >= STG_MAX_AXIS) {
02698     *volts = 0.0;
02699     return 0;
02700   }
02701 
02702   /* can't read outputs directly, so return stored value */
02703   *volts = checkedOutputs[index];
02704 
02705   return 0;
02706 }
02707 
02708 int stgModel()
02709 {
02710   return theStg.wModel;
02711 }