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

stg_v2_axis8.c

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