/*
 * ServoToGo.cpp  ( called "stgmembs.cpp" by ServoToGo )
 *
 * A part of the Windows 95 driver for Servo To Go, Inc.'s ISA Bus
 * servo I/O card.
 *
 * This file contains member functions for the ServoToGo class.
 *
 * Copyright (c) 1996 Servo To Go Inc.  All rights reserved.
 *
 * Modified for QNX by  - RAH 8-99
 *
 * 02/26/03 : Weyn Ong : Version for QNX 6.  I/O permissions enabled.
 */

#include <assert.h>
#include <stdio.h>
#include <sys/mman.h>
#include <sys/neutrino.h>

#include "driver/stgdefs.h"
#include "driver/ServoToGo.h"
#include "driver/PrIOWrap.h"
#include "driver/PrSTGDefn.h"

// These 4 port access functions used to wrap around the implementation
// provided by PrIOWrap.

PrIOWrap hwIO;

inline unsigned char fInP(int port)
{
   return (unsigned char) hwIO.input1B(port);
}

inline void fOutP(int port, unsigned int data)
{
   hwIO.output1B(port, data);
}

inline unsigned short fInPW(int port)
{
   return (unsigned short) hwIO.input2B(port);
}

inline void fOutPW(int port, unsigned int data)
{
   hwIO.output2B(port, data);
}

// End PrIOWrap functions


const int aPortOffset_1[] = {STG_DIO_A,  STG_DIO_B,  STG_DIO_C,  STG_DIO_D};
const int aPortOffset_2[] = {STG_PORT_A, STG_PORT_B, STG_PORT_C, STG_PORT_D};


/***************************************************************************/
/*                                                                         */
/*                           Constructor                                   */
/*                                                                         */
/***************************************************************************/

ServoToGo::ServoToGo(unsigned short RequestedIrq, unsigned short BaseAddress)
{
    ThreadCtl( _NTO_TCTL_IO, 0 );
    mmap_device_io( STG_ADDR_SPACE_SIZE_1, BaseAddress );
    mmap_device_io( STG_ADDR_SPACE_SIZE_2, 
        BaseAddress + STG_ARM_ADDRESS_2_OFFSET );

    Init(RequestedIrq,BaseAddress);   // init irq controller, board address
//  THESE LINES MOVED TO ::Init(.,.) SO CALLED BY BOTH CONSTRUCTION METHODS
//    EncoderInit();  // 
//    SelectInterruptPeriod(STG_1_MILLISECOND);  // initialize timer
//    byIndexPulsePolarity = 1;               // default active level high
};

/***************************************************************************/
/*                                                                         */
/*                            Destructor                                   */
/*                                                                         */
/***************************************************************************/
ServoToGo::~ServoToGo(void)
{
    StopInterrupts();

    // set all the DAC outputs to 0.
    for (unsigned short nAxis = 0; nAxis < STG_MAX_AXIS; nAxis++)
        RawDAC(nAxis, 0);

    // set all the digital I/O bits to input
    DioDirection(0);
}

/***************************************************************************/
/*                                                                         */
/*                            DioDirection                                 */
/*                                                                         */
/***************************************************************************/
//
// Two overloaded functions, the first takes a port and a direction, the
// second takes a word with bits set for the direction of each port
//
void ServoToGo::DioDirection(unsigned short nPortBits, unsigned short nDirection)
{
    // sometimes you want to set the direction of a port, but may not know
    // what direction other ports are.  Here we select the port (or ports)
    // that we want to set the direction of, in nPortBits.  Then the direction,
    // either input or output, with nDirection.
    //

    assert(nDirection <= 1);      // direction is either 0 or 1
    if (nDirection > 1)
        return;

    unsigned short nSwDir = wSaveDirs;

    if (nDirection == STG_PORT_OUTPUT)
        nSwDir |= nPortBits;
    else
        nSwDir &= ~nPortBits;

    DioDirection(nSwDir);
}


//                                        Don't care.
//                                        |<------>|
// bits in nSwDir set direction of ports: xxxxxxxxxxbbbbbb
//                                                  ||||||
//                                                  |||||Port A
//       1 = Output                                 ||||PortB
//       0 = Input                                  |||Port C, high nibble
//                                                  ||Port C, low nibble
//                                                  |Port D (port D, low nibble)
//                                                  |(Port D, high nibble)

void ServoToGo::DioDirection(const unsigned short nSwDir)
{
    unsigned char byHwDir;                   // direction bits for hardware

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }
    byHwDir = 0x9b;                          // initially all ports input

    if (nSwDir & STG_PORT_A_OUT)                 // check the bit for A out
         byHwDir &= ~STG_A_DIR;                  // if output, set bit to 0
    if (nSwDir & STG_PORT_B_OUT)
         byHwDir &= ~STG_B_DIR;
    if (nSwDir & STG_PORT_C_LO_OUT)
         byHwDir &= ~STG_C_LOW_DIR;
    if (nSwDir & STG_PORT_C_HI_OUT)
         byHwDir &= ~STG_C_HI_DIR;

    fOutP(wBaseAddress + STG_MIO_1, byHwDir);    // set direction for A, B and C

    SetDDir(nSwDir);
    wSaveDirs = nSwDir;                     // save, mostly for other function
}

void ServoToGo::SetDDir(unsigned short nSwDir)
{
    unsigned char byHwDir;                   // direction bits for hardware
    unsigned char bySaveReg, bySaveIMR, bySaveCntrl1;

    if (wModel == STG_MODEL1)
    {
        bySaveReg = fInP(wBaseAddress + STG_INTC);   // INTC needs to be saved, because
                                                 // MIO_2 reinitializes the 8255 which
                                                 // implements the INTC register.
        byHwDir = 0x92;                          // initialize port D input
        if (nSwDir & STG_PORT_D)                 // is port D output?
             byHwDir &= ~STG_D_DIR;                  // if yes, set bit to 0,
        bySaveIMR = fInP(wBaseAddress + STG_IMR);    // get the current interrupt mask
        fOutP(wBaseAddress + STG_OCW1, 0xff);        // mask off all interrupts
        fOutP(wBaseAddress + STG_MIO_2, byHwDir);    // set direction for port D
        fOutP(wBaseAddress + STG_INTC, bySaveReg);   // restore interrupt control reg.
        fOutP(wBaseAddress + STG_OCW1, bySaveIMR);   // restore interrupt mask
    }
    else  // Model 2
    {
        bySaveReg = fInP(wBaseAddress + STG_CNTRL0); // CNTRL0 needs to be saved, because
                                                 // MIO_2 reinitializes the 8255 which
                                                 // implements the CNTRL0 register.
        byHwDir = 0x8b;                          // initialize CNTRL0 as output reg.
                                                 // BRDTST to input.
                                                 // sets port D, high and low, to input
        if (nSwDir & STG_PORT_D_LO_OUT)          // low nibble
            byHwDir &= ~STG_D_LOW_DIR;
        if (nSwDir & STG_PORT_D_HI_OUT)          // high nibble
            byHwDir &= ~STG_D_HI_DIR;

        bySaveCntrl1 = fInP(wBaseAddress+STG_CNTRL1);// save for interrupt enables

        fOutP(wBaseAddress + STG_CNTRL1, (bySaveCntrl1 & STG_CNTRL1_SLAVE) | 0xf0);
                                                 // don't reset any latches;
                                                 // put back slave state;
                                                 // disable interrupts, so the
                                                 // glitch in CTRL0 doesn't
                                                 // cause an interrupt on wrong irq
        fOutP(wBaseAddress + STG_MIO_2, byHwDir);
        fOutP(wBaseAddress + STG_CNTRL0, bySaveReg); // restore CNTRL0, because it was
                                                 // re-initialized, which lost
                                                 // any previous contents.
        fOutP(wBaseAddress+STG_CNTRL1, bySaveCntrl1 | 0xf0);// re-enable interrupts
    }
};

/***************************************************************************/
/*                                                                         */
/*                                RawDO                                    */
/*                                                                         */
/***************************************************************************/
//
// Overloaded functions:
//    1) takes a 32 bit value and set all 32 iobits
//    2) takes which bit, which port, and what value, and sets only that bit
//

void ServoToGo::RawDO(unsigned long lOutBits)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }
    if (wModel == STG_MODEL1)
    {
        fOutP(wBaseAddress + STG_DIO_A, ((STG_IO32 *)&lOutBits)->port.A);
        fOutP(wBaseAddress + STG_DIO_B, ((STG_IO32 *)&lOutBits)->port.B);
        fOutP(wBaseAddress + STG_DIO_C, ((STG_IO32 *)&lOutBits)->port.C);
        fOutP(wBaseAddress + STG_DIO_D, ((STG_IO32 *)&lOutBits)->port.D);
    }
    else  // Model 2
    {
        fOutP(wBaseAddress + STG_PORT_A, ((STG_IO32 *)&lOutBits)->port.A);
        fOutP(wBaseAddress + STG_PORT_B, ((STG_IO32 *)&lOutBits)->port.B);
        fOutP(wBaseAddress + STG_PORT_C, ((STG_IO32 *)&lOutBits)->port.C);
        fOutP(wBaseAddress + STG_PORT_D, ((STG_IO32 *)&lOutBits)->port.D);
    }
};

void ServoToGo::RawDO(unsigned char byBitNumber, unsigned char bySet0or1,
                      unsigned short nPort)
{
    unsigned nOffset;
    unsigned char byData;

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }

    if (nPort > 3)
        return;

    if (wModel == STG_MODEL1)
        nOffset = aPortOffset_1[nPort];
    else  // Model 2
        nOffset = aPortOffset_2[nPort];
    //read it
    byData = fInP(wBaseAddress + nOffset);
    // change the bit
    if (bySet0or1 == 1)
        byData |= 1 << byBitNumber;
    else
        byData &= ~(1u << byBitNumber);
    // write the port
    fOutP(wBaseAddress + nOffset, byData);
};

void ServoToGo::RawDO(unsigned char byData, short nPort)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }

    if (nPort > 3)
        return;

    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + aPortOffset_1[nPort], byData);
    else  // Model 2
        fOutP(wBaseAddress + aPortOffset_2[nPort], byData);
};

/***************************************************************************/
/*                                                                         */
/*                                RawDI                                    */
/*                                                                         */
/***************************************************************************/

// some overloaded functions

unsigned long ServoToGo::RawDI()
{
    STG_IO32 xInBits;

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        xInBits.all = 0;
        return(xInBits.all);
    }

    if (wModel == STG_MODEL1)
    {
        xInBits.port.A = fInP(wBaseAddress + STG_DIO_A);
        xInBits.port.B = fInP(wBaseAddress + STG_DIO_B);
        xInBits.port.C = fInP(wBaseAddress + STG_DIO_C);
        xInBits.port.D = fInP(wBaseAddress + STG_DIO_D);
    }
    else  // Model 2
    {
        xInBits.port.A = fInP(wBaseAddress + STG_PORT_A);
        xInBits.port.B = fInP(wBaseAddress + STG_PORT_B);
        xInBits.port.C = fInP(wBaseAddress + STG_PORT_C);
        xInBits.port.D = fInP(wBaseAddress + STG_PORT_D);
    }
    return (xInBits.all);
};

unsigned char ServoToGo::RawDI(unsigned char byBitNumber, short nPort)
{
    unsigned char nData;

    if (nPort > 3)
        return 0;
    if (wModel == STG_MODEL1)
        nData = fInP(wBaseAddress + aPortOffset_1[nPort]);
    else // Model 2
        nData = fInP(wBaseAddress + aPortOffset_2[nPort]);
    return nData & (1 << byBitNumber);
}

unsigned char ServoToGo::RawDI(short nPort)
{
    if (nPort > 3)
        return 0;

    if (wModel == STG_MODEL1)
        return fInP(wBaseAddress + aPortOffset_1[nPort]);
    else
        return fInP(wBaseAddress + aPortOffset_2[nPort]);
}

unsigned long ServoToGo::RawDI(long lPort, long lBitNumber)
{
    unsigned short wAddOff = 0;
    unsigned char bPortBits;

    switch (lPort)
    {
    case 0: wAddOff = STG_DIO_A;
            break;
    case 1: wAddOff = STG_DIO_B;
            break;
    case 2: wAddOff = STG_DIO_C;
            break; 
    case 3: if (wModel == STG_MODEL1)
                wAddOff = STG_DIO_D;
            else
                wAddOff = STG_PORT_D;
            break;
    }
    bPortBits = fInP(wBaseAddress + wAddOff);
    if ( bPortBits & (1 << lBitNumber) )
        return 1;   // true
    return 0;       // false
}

/***************************************************************************/
/*                                                                         */
/*                               RawDAC                                    */
/*                                                                         */
/***************************************************************************/
void ServoToGo::RawDAC(unsigned short nAxis, long lCounts)
{
    if ( nAxis > 7 )        // is axis within range?
       return;

    // input / output:
    //
    //    lCounts (decimal) ... -lCounts ... +0x1000 ... volts
    //
    //     0x1000  (4096)     0xfffff000           0       +10
    //          0                      0      0x1000         0
    // 0xfffff001 (-4095)          0xfff      0x1fff       -10

    // So, the domain might be different than you expected. I expected:
    //     0xf000 (-4096)  to  0xfff (4095), rather than
    //     0xf001 (-4095)  to 0x1000 (4096)

    // reverse slope so positive counts give positive voltage
    lCounts = - lCounts;
    
    // shift for DAC
    lCounts += 0x1000;
    
    if (lCounts > 0x1FFF)    // clamp + output
    {
        lCounts = 0x1FFF;
    }
    if (lCounts < 0)         // clamp - output
    {
        lCounts = 0;
    }

    if (wNoBoardFlag == STG_NO_BOARD)      // are we simulating?
    {
        lSimDac[nAxis] = lCounts;
        return;
    }

//    nCounts = (USHORT) (lCounts + 0x1000); // correct range for DAC

    fOutPW(wBaseAddress + STG_DAC_0 + (nAxis << 1), (unsigned short)lCounts);
};

/***************************************************************************/
/*                                                                         */
/*                               MotSim                                    */
/*                                                                         */
/***************************************************************************/
// a motor simulator
//
// takes it's input from lSimDac[]
// writes it's output to lSimEnc[]
//
// This isn't used in the DOS program, it's only used in the Windows 95 driver

void ServoToGo::MotSim(void)
{
    static long lState_1[STG_MAX_AXIS] = {0};                    // state variables
    long lScaledUp;
    const short nScale = 10;

    for (int nAxis = 0; nAxis < STG_MAX_AXIS; nAxis++)
    {
        // The input is guaranteed to be +/- 12 bits
        // Scale up state so we don't loose resolution
        lScaledUp = lSimDac[nAxis] << nScale;

        // note: I assume right shift is sign preserving (for signed value)

        // lag
        lState_1[nAxis] += (lScaledUp - lState_1[nAxis]) >> (4 + nAxis); //lint !e704
                                                          //   ^^^^^^^ time constant
                                                          // is different for each axis 

        // integrator (shift out the scale factor and then some)
        lSimEnc[nAxis] += lState_1[nAxis] >> (nScale + 1);               //lint !e704
    }
}

/***************************************************************************/
/*                                                                         */
/*                              StartADC                                   */
/*                                                                         */
/***************************************************************************/
void ServoToGo::StartADC(unsigned short wAxis)
{

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }

    if (wAxis > 7)
        return;

    if (wModel == STG_MODEL1)
    {
        // do a dummy read from the ADC, just to set the input multiplexer to
        // the right channel
        fInPW(wBaseAddress + STG_ADC_0 + (wAxis << 1));        //lint !e534

        // wait 4 uS for settling time on the multiplexer and ADC
        // you probably shouldn't really have a delay in
        // a driver.
        Timer2Delay(28);

        // now start conversion.
        fOutPW(wBaseAddress + STG_ADC_0 + (wAxis << 1), 0);
    }
    else  // Model 2
    {
        unsigned char Cntrl0;

        Cntrl0 = fInP(wBaseAddress + STG_CNTRL0) & 0x07;  // save irq

        Cntrl0 |= (wAxis << 4) | 0x88;  // shift bits to AD2, AD1, AD0
                                        // set bit 0x80 high for autozero
                                        // set bit 0x08 high, not calibrating
        fOutP(wBaseAddress + STG_CNTRL0, Cntrl0);  // select ADC channel

        // don't have to do a dummy read for a model 2

        // wait 4 uS for settling time on the multiplexer and ADC
        // you probably shouldn't really have a delay in
        // a driver.
        Timer2Delay(28);

        // now start conversion.
        fOutPW(wBaseAddress + STG_ADC, 0);

    }
};

void ServoToGo::AutoZeroAdc(void)
{
    // set the Analog to Digital converter to autozero on each conversion

    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_INTC, fInP(wBaseAddress + STG_INTC) & ~STG_AUTOZERO);
    else   // MODEL2
        fOutP(wBaseAddress + STG_CNTRL0, fInP(wBaseAddress + STG_CNTRL0) | STG_CNTRL0_AZ);
}

void ServoToGo::DontAutoZeroAdc(void)
{
    // set the Analog to Digital converter to NOT autozero

    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_INTC, fInP(wBaseAddress + STG_INTC) | STG_AUTOZERO);
    else   // MODEL2
        fOutP(wBaseAddress +STG_CNTRL0, fInP(wBaseAddress + STG_CNTRL0) & ~STG_CNTRL0_AZ);
}

void ServoToGo::CalADC(void)
{
    // Start calibration cycle on ADC chip

    unsigned char Cntrl0;

    if (wModel == STG_MODEL2)   // this function only in Model 2 board.
    {
        Cntrl0 = fInP(wBaseAddress + STG_CNTRL0) & 0x07;       // save irq
        fOutP(wBaseAddress + STG_CNTRL0, Cntrl0);              // cal is low
        // cal pulse should be 60 ns.  The ISA bus is 10 MHz. or 100 ns.
        // so we can just set it back high.
        fOutP(wBaseAddress + STG_CNTRL0, Cntrl0 | 0x08);       // cal is high
    }
}

/***************************************************************************/
/*                                                                         */
/*                               RawADC                                    */
/*                                                                         */
/***************************************************************************/
short ServoToGo::SpinReadADC(unsigned short wAxis)
{
    short counts;

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return 0;
    }

    if (wAxis > 7)
         return -1;

    if (wModel == STG_MODEL1)
    {
        // make sure conversion is done, assume polling delay is done.
        // EOC (End Of Conversion) is bit 0x08 in IIR (Interrupt Request
        // Register) of Interrupt Controller.  Don't wait forever though
        // bail out eventually.

        for ( short j = 0; (!(CurrentIRR() & 0x08)) && (j < 10000); j++);

        counts = ( fInPW(wBaseAddress + STG_ADC_0 + (wAxis << 1)) );
    }
    else
    {
        // is the conversion done?
        for ( short j = 0; (fInP(wBaseAddress + STG_BRDTST) & STG_BRDTST_EOC) && (j < 10000); j++);

        // conversion is done, get counts.
        counts = fInPW(wBaseAddress + STG_ADC);
    }

    if (counts & 0x1000)       // is sign bit negative?
        counts |= 0xf000;      // sign extend
    else
        counts &= 0xfff;       // make sure high order bits are zero.

    return counts;
};

long ServoToGo::ReadADC(unsigned short wAxis, short& counts)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return STG_SUCCESS;
    }

    if (wAxis > 7)
         return STG_FAILURE;

    if (wModel == STG_MODEL1)
    {
        if ( !(CurrentIRR() & 0x08) ) // is the conversion done?
            return STG_FAILURE;                  // no, return failure

        // conversion is done, get counts.
//        counts = fInPW(wBaseAddress + STG_ADC_0 + (wAxis << 1));
        counts = fInPW(wBaseAddress + STG_ADC_1);
    }
    else  // Model 2
    {
        // is the conversion done?
        if ( fInP(wBaseAddress + STG_BRDTST) & STG_BRDTST_EOC )
            return STG_FAILURE;                 // no, return failure

        // conversion is done, get counts.
        counts = fInPW(wBaseAddress + STG_ADC_1);
    }

    if (counts & 0x1000)       // is sign bit negative?
        counts |= 0xf000;      // sign extend
    else
        counts &= 0xfff;       // make sure high order bits are zero.

    return STG_SUCCESS;

};

/***************************************************************************/
/*                                                                         */
/*                             EncoderInit                                 */
/*                                                                         */
/***************************************************************************/
// BEWARE: THIS FUNCTION DISTURBS THE CURRENT ENCODER VALUES
void ServoToGo::EncoderInit(void)
{

    if (wNoBoardFlag == STG_NO_BOARD)
    {
        wAxesInSys = STG_MAX_AXIS;
        return;
    }
   
    for (unsigned wAdd = wBaseAddress + STG_CNT0_C; wAdd <= wBaseAddress + STG_CNT6_C; wAdd +=4)
    {
      // Set Counter Command Register - Master Control, Master Reset (MRST),
      // and Reset address pointer (RADR).
      // x03=00000011
      fOutPW(wAdd, 0x0303); // Don't Master Reset - RAH

      // Set Counter Command Register - Input Control, OL Load (P3),
      // and Enable Inputs A and B (INA/B).
      // x68=01101000
      fOutPW(wAdd, 0x6868);

      // Set Counter Command Register - Output Control
      // x80=1000000
      fOutPW(wAdd, 0x8080);

      // Set Counter Command Register - Quadrature
      // xc3=11000011
      fOutPW(wAdd, 0xc3c3);
    }

#if 1  
    //  - RAH - Set Axes=8 
    wAxesInSys = 8;
    //  - RAH - Sign EXTEND THE 24bit HW counter on startup
    {
      int k; 
      STG_LONGBYTE e[8]; 
      EncoderLatch();
      EncReadAll(e);
      for ( k=0 ; k<wAxesInSys ; k++)
      { byEncHighByte[k] = e[k].Byte[2] & 0x80 ? 0xff : 0;
        byOldByte2[k] = e[k].Byte[2];
      }
    }
#else
    //  Figure out how many axes are on the card
    for (unsigned wAdd_ = wBaseAddress + STG_CNT0_D; wAdd_ <= wBaseAddress + STG_CNT6_D; wAdd_ +=4)
    {
        unsigned short const wTestPat = 0x5aa5;

        // write a pattern to the preset register

        fOutPW(wAdd_, wTestPat);
        fOutPW(wAdd_, wTestPat);
        fOutPW(wAdd_, wTestPat);

        // transfer the preset register to the count register

        fOutPW(wAdd_ + 2, 0x0909);

        // transfer counter to output latch

        fOutPW(wAdd_ + 2, 0x0202);

        // read the output latch and see if it matches

        if (fInPW(wAdd_) != wTestPat)
            break;
        if (fInPW(wAdd_) != wTestPat)
            break;
        if (fInPW(wAdd_) != wTestPat)
            break;

        wAxesInSys += 2;

        // write zeros to preset register, we don't want to do a master reset
        // (MRST), because then we would need to re-initialize the counter

        fOutPW(wAdd_, 0);
        fOutPW(wAdd_, 0);
        fOutPW(wAdd_, 0);

        // reset counter, BRW and CRY and address pointer (RADR)

        fOutPW(wAdd_ + 2, 0x0505);

    }
#endif

};

/***************************************************************************/
/*                                                                         */
/*                            SetEncoderCounts                             */
/*                                                                         */
/***************************************************************************/
void ServoToGo::SetEncoderCounts(unsigned short nAxis, long lCounts)
{
    unsigned short wAddress;
    char *ByteUnion = (char *)&lCounts;  // get pointer to lCounts, so you
                                         // can take it apart byte by byte

    wAddress = wBaseAddress + STG_CNT0_D;
    wAddress += (nAxis & 0x6) << 1; // shift to multiply by 2
                                    // pairs of data regs seperated by pairs
                                    // of control regs, skip over control.
    wAddress += nAxis & 1;
    fOutP(wAddress, ByteUnion[0]);
    fOutP(wAddress, ByteUnion[1]);
    fOutP(wAddress, ByteUnion[2]);

    // transfer the preset register to the count register
    fOutP(wAddress + 2, 0x09);

    // set things for the part that extends the 24 bit counter
    // to 32 bits.
    byEncHighByte[nAxis] = ByteUnion[3];
    byOldByte2[nAxis] = ByteUnion[2];
}

/***************************************************************************/
/*                                                                         */
/*                            EncoderLatch                                 */
/*                                                                         */
/***************************************************************************/
void ServoToGo::EncoderLatch(void)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }

   // normally you'll have the timer latch the data in hardware, but
   // if the timer isn't running, we need to latch it ourselves.

   for (unsigned wAdd = wBaseAddress + STG_CNT0_C; wAdd <= wBaseAddress + STG_CNT6_C; wAdd +=4)
       fOutPW(wAdd, 0x0303);
};

/***************************************************************************/
/*                                                                         */
/*                          EncoderResetAddr                               */
/*                                                                         */
/***************************************************************************/
void ServoToGo::EncoderResetAddr(void)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }

   // This function resets all the counter's internal address pointers to point
   // to the first byte in the 3 byte sequence

   for (unsigned wAdd = wBaseAddress + STG_CNT0_C; wAdd <= wBaseAddress + STG_CNT6_C; wAdd +=4)
       fOutPW(wAdd, 0x0101);
};

/***************************************************************************/
/*                                                                         */
/*                          SelectIndexAxis                                */
/*                       SelectIndexOrExtLatch                             */
/*                                                                         */
/***************************************************************************/
void ServoToGo::SelectIndexAxis(unsigned char byAxis)
{
    // overloaded function.  Use this if you don't need to set polarity

     // routine for Model 1
     SelectIndexAxis(byAxis, byIndexPulsePolarity);

// Attempt to unify Model1 & Model2 controllers by Bob
//     // routine for Model 1
//     if( wModel == STG_MODEL1 )
//       SelectIndexAxis(byAxis, byIndexPulsePolarity);

//     // routine for Model 2     // Bob Added MODEL2 function here.
//     if( wModel == STG_MODEL2 )
//     { //unsigned char bySelectBits = ~byAxis;	// Select channels as indicated
//       unsigned char bySelectBits = 0x00;	// Select all 8 channels as Index
//       fOutP(wBaseAddress + STG_SELDI, bySelectBits);
//     }
}

void ServoToGo::SelectIndexAxis(unsigned char byAxis, unsigned char byPol)
{
    // routine for Model 1

    //
    // initialize stuff to poll index pulse
    //
    unsigned char byIntc;

//     if( wModel == STG_MODEL1 )
//     { 
      byIndexPollAxis = byAxis;           // save axis to check later
      byIndexPulsePolarity = byPol;       // save polarity as new default
      byAxis &= 0x6;                      // ignore low bit, we check 2 axes at a time
      byAxis <<= 3;                       // shift into position for IXS1, IXS0
      byIntc = fInP(wBaseAddress + STG_INTC); // get a copy of INTC, we'll change
                                          // some bits in it, not all
      byIntc &= ~(STG_IXLVL | STG_IXS1 | STG_IXS0);   // zero bits for axis and polarity
      byIntc |= byAxis;                   // put axes address in INTC
      if (byPol != 0)                     // is index pulse active high?
        byIntc |= STG_IXLVL;
      fOutP(wBaseAddress + STG_INTC, byIntc);
      ResetIndexLatch();

      // The latched index pulse should be low now.  If it's not, either something's
      // wrong, or we happened to initialize it while the index pulse was active.
//     }

// Attempt to unify Model1 & Model2 controllers by Bob
//     // routine for Model 2     // Bob Added MODEL2 function here.
//     if( wModel == STG_MODEL2 )
//     { unsigned char bySelectBits = 0x00;	// Select all 8 channels as Index
//       SelectIndexOrExtLatch(bySelectBits);
//       EnableCounterLatchOnIndexOrExt(bySelectBits); // Cannot choose Polarity
//     }
}

void ServoToGo::SelectIndexOrExtLatch(unsigned char bySelectBits)
{
    // routine for Model 2

    fOutP(wBaseAddress + STG_SELDI, bySelectBits);
}

void ServoToGo::EnableCounterLatchOnIndexOrExt(unsigned char bySelectBits)
{
    // routine for Model 2
    fOutP(wBaseAddress + STG_IDLEN, bySelectBits);
}

/***************************************************************************/
/*                                                                         */
/*                          ResetIndexLatch(es)                            */
/*                                                                         */
/***************************************************************************/
void ServoToGo::ResetIndexLatch()
{

  // routine for Model 1
  fInP(wBaseAddress + STG_ODDRST);        //lint !e534 reset index pulse latch for ODD axis
  fInP(wBaseAddress + STG_BRDTST);        //lint !e534 reset index pulse latch for EVEN axis

// Attempt to unify Model1 & Model2 controllers by Bob
//     // routine for Model 1
//     if( wModel == STG_MODEL1 )
//     { fInP(wBaseAddress + STG_ODDRST);        //lint !e534 reset index pulse latch for ODD axis
//       fInP(wBaseAddress + STG_BRDTST);        //lint !e534 reset index pulse latch for EVEN axis
//     }
//     // routine for Model 2     // Bob Added MODEL2 function here.
//     if( wModel == STG_MODEL2 )
//     { unsigned char byLatchBits = 0x00;
//     ResetIndexLatches(byLatchBits);
//     }
}

void ServoToGo::ResetIndexLatches(unsigned char byLatchBits)
{
    // routine for Model 2
    fOutP(wBaseAddress + STG_IDL, byLatchBits);
}

/***************************************************************************/
/*                                                                         */
/*                        (Get)IndexPulseLatch(es)                         */
/*                                                                         */
/***************************************************************************/
unsigned short ServoToGo::IndexPulseLatch(void)
{
    // routine for Model 1 board

    // poll the latched index pulse of the axis that was previously set up

    unsigned char byIRR, byAxisMask;

//     if( wModel == STG_MODEL1 )
//     { 
      byIRR = CurrentIRR();
      byAxisMask = (byIndexPollAxis & 1) ? STG_LIXODD : STG_LIXEVN;  // even or odd axis?
      if (byIRR & byAxisMask)                          // check latched index pulse
        return 1;
      return 0;

    //
    // a faster, but messier way
    //
    //fOutP(wBaseAddress + OCW3, 0x0a);           // IRR on next read
    //
    //return (   fInP(wBaseAddress + IRR)
    //         & ( (byIndexPollAxis & 1) ? LIXODD : LIXEVN ) // mask for even or odd
    //       );
//     }

// Attempt to unify Model1 & Model2 controllers by Bob
//     // routine for Model 2     // Bob Added MODEL2 function here.
//     if( wModel == STG_MODEL2 )
//     { return ( GetIndexLatches() != 0 );
//     }
//     return 0;

}

char ServoToGo::GetIndexLatches()
{
    // routine for Model 2 board

    return fInP(wBaseAddress + STG_IDL);
}

char ServoToGo::GetSELDI()
{
    return fInP(wBaseAddress + STG_SELDI);
}

char ServoToGo::GetIDLEN()
{
    return fInP(wBaseAddress + STG_IDLEN);
}

/***************************************************************************/
/*                                                                         */
/*                            IndexPulse                                   */
/*                                                                         */
/***************************************************************************/
short ServoToGo::IndexPulse(void)
{
    // poll for the index pulse of the axis that was previously set up.
    // Normally you would look at the latched pulse.  This function will
    // probably only get used during testing.
    
    unsigned char byIRR, byAxisMask;
    
//     if( wModel == STG_MODEL1 )
//     { 
      byIRR = CurrentIRR();
      byAxisMask = (byIndexPollAxis & 1) ? STG_IXODD : STG_IXEVN;  // even or odd axis?

      // The raw index pulse isn't inverted by the hardware if the index pulse is
      // low active (only the latched pulse is).  For consistancy, we'll invert
      // the pulse in software.

      if (byIndexPulsePolarity == 0)                       // if pulse is low true
        byIRR ^= byAxisMask;                             // flip bit

      if (byIRR & byAxisMask)                              // check index pulse
        return 1;
      return 0;
//     }

// Attempt to unify Model1 & Model2 controllers by Bob
//     // routine for Model 2     // Bob Added MODEL2 function here.
//     if( wModel == STG_MODEL2 )
//     { return ( GetIndexLatches() != 0 );
//     }
//     return 0;

}

/***************************************************************************/
/*                                                                         */
/*                             CurrentIRR                                  */
/*                                                                         */
/***************************************************************************/
unsigned char ServoToGo::CurrentIRR(void)
{
    fOutP(wBaseAddress + STG_OCW3, 0x0a);           // IRR on next read
    return fInP(wBaseAddress + STG_IRR);
}    

/***************************************************************************/
/*                                                                         */
/*                             EncReadAll                                  */
/*                                                                         */
/***************************************************************************/
void ServoToGo::EncReadAll(STG_LONGBYTE *lbEnc)
{
    STG_WORDBYTE wbTransfer;
//    static unsigned char byOldByte2[STG_MAX_AXIS];
//    static unsigned char byEncHighByte[STG_MAX_AXIS];
    short i;
    
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        for (i = 0; i < 8; i++)
        { 
            lbEnc[i].Long = lSimEnc[i];
        }
        return;
    }

    // Disable interrupts here?  No, the timer will latch new data in the
    // hardware anyway.  Maybe we should stop the timer?  In an interrupt
    // service routine, you're synchronized with the timer; so the readings
    // will never change while you're reading them.  If you're polling, you
    // would first latch the encoder counts with the EncoderLatch() function.
    // But, the timer could latch the counts again, in the middle of the read.

    // reset counter internal addr ptr to point to first byte
    for (unsigned add = wBaseAddress + STG_CNT0_C; add <= wBaseAddress + STG_CNT6_C; add +=4)
        fOutPW(add, 0x0101);

    for (i = 0; i < 3; i++)            // 24 bits means get 3 bytes each
    {
        wbTransfer.Word = fInPW(wBaseAddress + STG_CNT0_D);

        lbEnc[0].Byte[i] = wbTransfer.Byte.high;
        lbEnc[1].Byte[i] = wbTransfer.Byte.low;

        wbTransfer.Word = fInPW(wBaseAddress + STG_CNT2_D);

        lbEnc[2].Byte[i] = wbTransfer.Byte.high;
        lbEnc[3].Byte[i] = wbTransfer.Byte.low;

        wbTransfer.Word = fInPW(wBaseAddress + STG_CNT4_D);

        lbEnc[4].Byte[i] = wbTransfer.Byte.high;
        lbEnc[5].Byte[i] = wbTransfer.Byte.low;

        wbTransfer.Word = fInPW(wBaseAddress + STG_CNT6_D);

        lbEnc[6].Byte[i] = wbTransfer.Byte.high;
        lbEnc[7].Byte[i] = wbTransfer.Byte.low;
    }

    // code to sign extend the 24 bit value
    // but we won't do this.
    //for ( i = 0; i < 8; i++)
    //  lbEnc[i].Byte[3] = lbEnc[i].Byte[2] & 0x80 ? 0xff : 0;

    // maintain the high byte, to extend the counter to 32 bits
    //
    // base decisions to increment or decrement the high byte
    // on the highest 2 bits of the 24 bit value.  To get the
    // highest 2 bits, use 0xc0 as a mask on byte [2] (the third
    // byte).

    for (i = 0; i < STG_MAX_AXIS; i++)
    {
        // check for -1 to 0 transition

        if (    ( (byOldByte2[i]    & 0xc0) == 0xc0 ) // 11xxxxxx
             && ( (lbEnc[i].Byte[2] & 0xc0) == 0 )    // 00xxxxxx
           )
           byEncHighByte[i]++;

        // check for 0 to -1 transition

        if (    ( (byOldByte2[i]    & 0xc0) == 0 )    // 00xxxxxx
             && ( (lbEnc[i].Byte[2] & 0xc0) == 0xc0 ) // 11xxxxxx
           )
           byEncHighByte[i]--;

        lbEnc[i].Byte[3] = byEncHighByte[i];
        byOldByte2[i] = lbEnc[i].Byte[2];    // current byte 2 becomes old one
    }
};

/***************************************************************************/
/*                                                                         */
/*                        SelectInterruptPeriod                            */
/*                                                                         */
/***************************************************************************/
void ServoToGo::SelectInterruptPeriod(long lPeriodSelect)
{
   if (wNoBoardFlag == STG_NO_BOARD)
   {
       return;
   }

   if (lPeriodSelect != STG_MAX_PERIOD)
   {
       fOutP(wBaseAddress + STG_TMRCMD, 0x56);   // timer 1, read/load LSB (MSB is 0)
                                             // mode 3 (square wave)
       fOutP(wBaseAddress + STG_TIMER_1, 0xb4);  // 0xb4 = 180 -> 25 uSec period
   }
   else
   {
       fOutP(wBaseAddress + STG_TMRCMD, 0x76);   // timer 1, read/load LSB then MSB
                                             // mode 3 (square wave)
       fOutP(wBaseAddress + STG_TIMER_1, 0xff);  // LSB
       fOutP(wBaseAddress + STG_TIMER_1, 0xff);  // MSB
   }

   switch (lPeriodSelect)
   {
        case STG_500_MICROSECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x14);   // 0x14 = 20 = .5 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);  
            break;
        case STG_1_MILLISECOND:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x28);   // 0x28 = 40 = 1 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);
            break;
        case STG_2_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x50);   // 0x50 = 80 = 2 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);
            break;
        case STG_3_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x78);   // 0x78 = 120 = 3 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);  
            break;
        case STG_4_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0xA0);   // 0xA0 = 160 = 4 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);
            break;
        case STG_5_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0xC8);   // 0xC8 = 200 = 5 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x00);  
            break;
        case STG_10_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x90);   // 0x0190 = 400 = 10 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x01);  
            break;
        case STG_100_MILLISECONDS:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0xA0);   // 0x0FA0 = 4000 = 100 mS
            fOutP(wBaseAddress + STG_TIMER_0, 0x0F);
            break;
        case STG_1_SECOND:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0x40);   // 0x9C40 = 40000 = 1 S
            fOutP(wBaseAddress + STG_TIMER_0, 0x9c);  
            break;
        case STG_MAX_PERIOD:
            fOutP(wBaseAddress + STG_TMRCMD, 0x34);    // timer 0, read/load LSB followed by
                                                   // MSB, mode 2 (real-time interrupt)
            fOutP(wBaseAddress + STG_TIMER_0, 0xff);   // LSB
            fOutP(wBaseAddress + STG_TIMER_0, 0xff);   // MSB
            break;
        default:
            // wrong input? then don't change it
            break;
   }
};

/***************************************************************************/
/*                                                                         */
/*                             StopTimer                                   */
/*                                                                         */
/***************************************************************************/
void ServoToGo::StopTimer()
{
    // stop the timer by putting it into one shot mode, it will never get
    // a trigger

    fOutP(wBaseAddress + STG_TMRCMD, 0x02);    // timer 0, mode 1
}

/***************************************************************************/
/*                                                                         */
/*                           StartInterrupts                               */
/*                                                                         */
/***************************************************************************/
void ServoToGo::StartInterrupts(void)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }
    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_OCW1, ~0x04);   // enable interrupt for timer 0
    else  // MODEL2
        fOutP(wBaseAddress + STG_CNTRL1, STG_CNTRL1_IEN_T0 | STG_CNTRL1_SLAVE);
};

/***************************************************************************/
/*                                                                         */
/*                           Timer2 functions                              */
/*                                                                         */
/***************************************************************************/
void ServoToGo::Timer2Delay(unsigned short counts)
{
    StartTimer2TerminalCount(counts);

    while (PollTimer2());
}

void ServoToGo::StartTimer2TerminalCount(unsigned short count)
{
   if (wNoBoardFlag == STG_NO_BOARD)
   {
       return;
   }

   char *pByte = (char *)&count;
   fOutP(wBaseAddress + STG_TMRCMD, 0xb0);       // timer 2, read/load LSB followed
                                             // by MSB, mode 0 (terminal count)
   fOutP(wBaseAddress + STG_TIMER_2, *pByte++);  // LSB (little endian)
   fOutP(wBaseAddress + STG_TIMER_2, *pByte);    // MSB
}

void ServoToGo::StartTimer2RTI(unsigned short count)
{
   if (wNoBoardFlag == STG_NO_BOARD)
   {
       return;
   }

   char *pByte = (char *)&count;
   fOutP(wBaseAddress + STG_TMRCMD, 0xb4);       // timer 2, read/load LSB followed
                                             // by MSB, mode 2 (real time int).
   fOutP(wBaseAddress + STG_TIMER_2, *pByte++);  // LSB (little endian)
   fOutP(wBaseAddress + STG_TIMER_2, *pByte);    // MSB
}

unsigned short ServoToGo::ReadTimer2TerminalCount(void)
{
    unsigned short count;
    char *pByte = (char *)&count;
    fOutP(wBaseAddress + STG_TMRCMD, 0x80);       // timer 2, latch
    *pByte++ = fInP(wBaseAddress + STG_TIMER_2);  // LSB
    *pByte   = fInP(wBaseAddress + STG_TIMER_2);  // MSB
    return count;
}

short ServoToGo::PollTimer2(void)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return 0;
    }

    if (wModel == STG_MODEL1)
        return !(CurrentIRR() & STG_TP2);  // mask selects bit for TP2
    else   // MODEL 2
    {
        unsigned char byRegCntrl1, byNewCntrl1;

        byRegCntrl1 = fInP(wBaseAddress + STG_CNTRL1);
        if (byRegCntrl1 & STG_CNTRL1_INT_T2)
        {
            // If it's set, we want to reset the latch, by writing a zero
            // to CNTRL1_INT_T2.

            // When we write to CNTRL1, we don't want to change SLAVE, IEN_G2
            // IEN_T2, or IEN_T0 -- the lower 4 bits.  So, we start with

            byNewCntrl1 = byRegCntrl1 & 0x0f;

            // We don't want to reset WDTOUT, INT_G2, or INT_T0--we only want
            // to reset ...T2.

            byNewCntrl1 |= STG_CNTRL1_WDTOUT | STG_CNTRL1_INT_G2 | STG_CNTRL1_INT_T0;

            fOutP(wBaseAddress + STG_CNTRL1, byNewCntrl1);  // reset bit

            return 1;  // true
        }
    }
    return 0;      // false
}

void ServoToGo::MaskTimer2Interrupt()
{
    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_OCW1, CurrentIRR() | STG_TP2);
    else   // Model 2
        fOutP(wBaseAddress + STG_CNTRL1,
            fInP(wBaseAddress + STG_CNTRL1) & ~STG_CNTRL1_IEN_T2);
}

void ServoToGo::UnMaskTimer2Interrupt()
{
    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_OCW1, CurrentIRR() & ~STG_TP2);
    else   // Model 2
        fOutP(wBaseAddress + STG_CNTRL1,
            fInP(wBaseAddress + STG_CNTRL1) | STG_CNTRL1_IEN_T2);
}

/***************************************************************************/
/*                                                                         */
/*                           StopInterrupts                                */
/*                                                                         */
/***************************************************************************/
void ServoToGo::StopInterrupts(void)
{
    if (wNoBoardFlag == STG_NO_BOARD)
    {
        return;
    }
    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_OCW1, 0xff);     // disable all interrupts
    else  // MODEL2
        fOutP(wBaseAddress + STG_CNTRL1, STG_CNTRL1_SLAVE);
};

/***************************************************************************/
/*                                                                         */
/*                             BaseFind                                    */
/*                                                                         */
/***************************************************************************/

// find the base address of the Servo To Go board

unsigned short ServoToGo::BaseFind(void)
{
   short i;
   unsigned short io_add;

   for (i = 15; i >= 0; i--)                    // search all possible addresses
   {
       io_add = i * 0x20 + 0x200;
       if ( BrdtstOK(io_add) )
        return io_add;
   }
   return(0);
}

/***************************************************************************/
/*                                                                         */
/*                             BrdtstOK                                    */
/*                                                                         */
/***************************************************************************/

// check the board tests register, figure out board model

unsigned short ServoToGo::BrdtstOK(unsigned short BaseAddress)
{
    unsigned short BrdtstAddress;
    unsigned short SerSeq, HighNibble;

    BrdtstAddress = BaseAddress + STG_BRDTST;

    SerSeq = 0;
    for (int j = 7; j >= 0; j--)
    {
        HighNibble = fInP(BrdtstAddress) >> 4;
        if (HighNibble & 8)     // is SER set
        {
           // shift bit to position specifed by Q2, Q1, Q0
           // which are the lower three bits.  Put bit in SerSeq.
           SerSeq |= 1 << (HighNibble & 7);
        }
    }
    if (SerSeq == 0x75)        // SER sequence is 01110101
    {
        wModel = STG_MODEL1;
        return 1;              //true
    }
    if (SerSeq == 0x74)        // SER sequence is 01110100
    {
        wModel = STG_MODEL2;
        return 1;              // true
    }
    return 0;  // false
}

/***************************************************************************/
/*                                                                         */
/*                             Initialize                                  */
/*                                                                         */
/***************************************************************************/
int ServoToGo::Init(unsigned short RequestedIrq, unsigned short BaseAddress)
{
  /*
   * First find the the base I/O address of the board.
   *
   * only do this in the DOS example.  It's dangerous under 95, and can't be
   * done under NT
   * AND WON't BE done in QNX -- RAH
   */
//   wBaseAddress = BaseFind();

    // Enable I/O permission for QNX 6

    ThreadCtl( _NTO_TCTL_IO, 0 );
    mmap_device_io( STG_ADDR_SPACE_SIZE_1, BaseAddress );
    mmap_device_io( STG_ADDR_SPACE_SIZE_2, 
        BaseAddress + STG_ARM_ADDRESS_2_OFFSET );

   assert( BrdtstOK(BaseAddress) );  // FAIL IF NO BOARD - RAH

   printf("wModel= %d\n",wModel);
   wBaseAddress = BaseAddress;

   if (wBaseAddress == 0)
   {
       wNoBoardFlag = STG_NO_BOARD;
   }
   wNoBoardFlag = STG_BOARD_PRESENT;

  /*
   * Initialize the interrupt controller
   */
   if (wModel == STG_MODEL1)
   {
   fOutP(wBaseAddress + STG_MIO_2, 0x92);  // initialize INTC as output reg.
                                       // sets port D to input since we have
                                       // to set it to something.
   SetIrq(RequestedIrq);               // selects the IRQ in INTC. Now, if a stray
                                       // interrupt is issued (see below) it will
                                       // go to an interrupt that isn't enabled on
                                       // the motherboard yet (if your system is
                                       // set up correctly).
   fOutP( wBaseAddress + STG_ICW1, 0x1a ); // initialize 82C59 as single chip,
                                       // level triggered
   fOutP( wBaseAddress + STG_ICW2, 0x00 ); // icw2 - not used, must write
                                       // could issue stray interrupt here - danger!
   fOutP( wBaseAddress + STG_OCW1, 0xff);  // mask off all interrupt sources (the
                                       // interrupt on the motherboard isn't
                                       // enabled yet, you do that when you install
                                       // your interrupt handler.).
   }
   else   // must be a Model 2
   {
   fOutP(wBaseAddress + STG_MIO_2, 0x8b);  // initialize CNTRL0 as output reg.
                                       // BRDTST to input.
                                       // sets port D, high and low, to input
                                       // since we have to set it to something.
   SetIrq(RequestedIrq);               // selects the IRQ in CNTRL0. Now, if a stray
                                       // interrupt is issued (see below) it will
                                       // go to an interrupt that isn't enabled on
                                       // the motherboard yet (if your system is
                                       // set up correctly).
   }


   // THIS STUFF WAS IN 2-ARG CONSTRUCTOR, BUT IT WASN'T CALLED
   // WHEN 0-ARG CONSTRUCTOR CALLED FOLLOWED BY .Init(.,.)
   // SO MOVE IT HERE AND IT WILL ALWAYS GET CALLED. - RAH
    EncoderInit();  // Do this, It sign extends the 24bit HW counter
//   SelectInterruptPeriod(_1_MILLISECOND);  // initialize timer
//    byIndexPulsePolarity = 1;               // default active level high

  return 1;
};

/***************************************************************************/
/*                                                                         */
/*                              SetIrq                                     */
/*                                                                         */
/***************************************************************************/
void ServoToGo::SetIrq(unsigned short RequestedIrq)
{
    unsigned char byIntReg;

    if (wNoBoardFlag == STG_NO_BOARD)
        return;

    wIrq = RequestedIrq;  // assume it's OK for now, check later

    if (wModel == STG_MODEL1)
        byIntReg = 0x80;       // initial value for the high bits in the register
                               // sets auto zero off
    else   // MODEL2
        byIntReg = 0x88;       // cal high too, not calibrating ADC

    // now put low bits into byIntReg to select irq

    switch (RequestedIrq)
    {
        case 3: break;      // add zero

        case 5: byIntReg |= 4;
                break;

        case 7: byIntReg |= 2;
                break;

        case 9: byIntReg |= 6;
                break;

        case 10: byIntReg |= 5;
                 break;

        case 11: byIntReg |= 7;
                 break;

        case 12: byIntReg |= 3;
                 break;

        case 15: byIntReg |= 1;
                 break;

        default: wIrq = 5;      // ERROR, requested irq not valid, use 5
                 byIntReg |= 4; // There is no safe value, leaving zero
                                // here would select IRQ 3 which is worse
                                // than 5 because IRQ 3 is usually for COM 2
                 break;
    }

    if (wModel == STG_MODEL1)
        fOutP(wBaseAddress + STG_INTC, byIntReg);  // set irq
    else   // MODEL2
        fOutP(wBaseAddress + STG_CNTRL0, byIntReg);
}


void ServoToGo::HandOpen()
{
    unsigned char xInBitsPortC, temp, openGripper;
  
    //read and save the information in PORT C
    xInBitsPortC = fInP(wBaseAddress+STG_PORT_C);  

    //C6 controls the hand Open solenoid
    //C5 controls the hand Close solenoid

    // 0x9f => 1001 1111
    temp = xInBitsPortC & 0x9f;

    // 0x40 => 0100 0000
    openGripper = temp | 0x40;
    
    fOutP(wBaseAddress+STG_PORT_C,openGripper);

}

void ServoToGo::HandClose()
{
    unsigned char xInBitsPortC, temp, closeGripper;
  
    //read and save the information in PORT C
    xInBitsPortC = fInP(wBaseAddress+STG_PORT_C);  

    //C6 controls the hand Open solenoid
    //C5 controls the hand Close solenoid

    // 0x9f => 1001 1111
    temp = xInBitsPortC & 0x9f;

    // 0x20 => 0010 0000
    closeGripper = temp | 0x20;
    
    fOutP(wBaseAddress+STG_PORT_C,closeGripper);

}