/********************************* servo2go.cpp Author: Ryan Findley Modified by Weston Griffin Description: An object-oriented approach to the servo2go card. The software which we got with the card failed to compile, and was hard to understand. This attempts to make it easier. History: 8/4/99 ryan started 02/25/00 wbg removed far handler and added some functions ***********************************/ //#define TEST_OLD //#define TEST_ADC_POLL //#define TEST_ADC_EOC //------------------ include files -------------------- #include #include #include #include #include #include #include "DML_servo2go.h" //------------------ definitions ---------------------- #define MIN_BASE 0x200 #define MAX_BASE 0x3E0 //encoder preset const long ENC_PRESET = 0x7fffff; const BYTE ENC_PRESET1 = ((BYTE)( ENC_PRESET )); //low byte const BYTE ENC_PRESET2 = ((BYTE)( ENC_PRESET >> 8 )); //middle byte const BYTE ENC_PRESET3 = ((BYTE)( ENC_PRESET >> 16 )); //high byte int autoZeroFlag = 1; //1 if autozeroing assume you are just in case it is not set typedef union { short word; BYTE byte[2]; } wordbyte; //------------------ module code ---------------------- /*************************************** constructor and destructor performs necessary initialization and safety shutdown. ****************************************/ servo2go::servo2go( off_t baseAddress ) { //check for baseAddress to be in the proper range if ( baseAddress < MIN_BASE || baseAddress > MAX_BASE ) { printf("Servo2Go error: base address out of bounds! (%#x)\n",baseAddress); abort(); } wbase = baseAddress; } /******************************** init/shutdown functions *********************************/ servo2go::init() { // initialize the INTC register // set all digital ports to inputs outp( wbase + MIO_1, 0x9b ); outp( wbase + MIO_2, 0x92 ); // set up the INTC register outp( wbase + INTC, 0x00 ); // select an IRQ for the interrupt: if a stray interrupt is issued // hopefully no one is listening to this IRQ. setIRQ( 5 ); // initialize 82C59 as single chip, // ICW1. level triggered, single mode, // 8-interval call address, no ICW4 needed. outp( wbase + ICW1, 0x1a ); // icw2 - not used, must write // could issue stray interrupt here - danger! outp( wbase + ICW2, 0x00 ); // mask off all interrupt sources (the interrupt on the motherboard isn't // enabled yet, you do that when you install // your interrupt handler.). outp( wbase + OCW1, 0xff ); // initialize the encoders initEncoders(); } servo2go::zeroAll() { //set all the digital ports to inputs setDigitalDirections( dInput, dInput, dInput, dInput, dInput); //set all DAC outputs to zero volts. for (int i = 0; i < 8; i++ ) DAC( DACport[i], 0.0 ); //mask all interrupts outp(wbase + IMR, 0xff); } /**************************************** interrupt controls *****************************************/ servo2go::enableInterrupt( intSrc which ) { outp( wbase + OCW1, inp(wbase + IMR) & ~which ); } servo2go::disableInterrupt( intSrc which ) { outp( wbase + OCW1, inp(wbase + IMR) | which ); } int servo2go::setIRQ( int which ) { BYTE s2gint; switch( which ) { case 3: s2gint = 0x00; break; case 5: s2gint = 0x04; break; case 7: s2gint = 0x02; break; case 9: s2gint = 0x06; break; case 10: s2gint = 0x05; break; case 11: s2gint = 0x07; break; case 12: s2gint = 0x03; break; case 15: s2gint = 0x01; break; default: // Error! Tried to select an interrupt which the servo2go can't generate!!! return(-1); break; } outp( wbase + INTC, (inp( wbase + INTC ) & 0xF8) | s2gint ); return(which); } /***************************************** timer controls ******************************************/ int servo2go::setTimerPeriod( off_t whichTimer, unsigned long timerPeriodMicroSeconds ) { static wordbyte period; switch( whichTimer ) { case TIMER_0: // timer 1, read/load LSB (MSB is 0) // mode 3 (square wave) outp( wbase + TMRCMD, 0x56 ); outp( wbase + TIMER_1, 0xb4 ); // 0xb4 = 180 -> 25 uSec period outp( wbase + TMRCMD, 0x34 ); // read in LSB, then MSB timer0, mode 2 (real-time interrupt) period.word = (timerPeriodMicroSeconds / 25); break; case TIMER_1: outp( wbase + TMRCMD, 0x74 ); // same, timer1 period.word = ( timerPeriodMicroSeconds ); break; case TIMER_2: outp( wbase + TMRCMD, 0xb0 ); // timer2 set to trigger TC int period.word = ( timerPeriodMicroSeconds*7 ); // counter runs at .13 usec thus 7 counts per usec break; default: return(-1); break; } //load up the requested timer period outp( wbase + whichTimer, period.byte[0] ); outp( wbase + whichTimer, period.byte[1] ); return (0); } /**************************************** digital I/O stuff *****************************************/ servo2go::setDigitalDirections( int ddra, int ddrb, int ddrcl, int ddrch, int ddrd ) { BYTE directionByte; directionByte = 0x80; if(ddra) directionByte |= 0x10; if(ddrb) directionByte |= 0x02; if(ddrch) directionByte |= 0x08; if(ddrcl) directionByte |= 0x01; outp( wbase + MIO_1, directionByte ); BYTE intC; BYTE intMask; //save and restore INTC and intMask intC = inp( wbase + INTC ); intMask = inp( wbase + IMR ); //mask interrupts outp(wbase + OCW1, 0xff); //set the port D direction ddrd ? directionByte = 0x92 : directionByte = 0x82 ; outp(wbase + MIO_2, directionByte ); //restore intC and IMR outp(wbase + INTC, intC ); outp(wbase + OCW1, intMask ); } servo2go::digitalOut( off_t whichPort, BYTE value ) { outp(wbase + whichPort, value); } servo2go::digitalOutCBit( int whichBit, int value ) { switch( whichBit ) { case 0: value ? outp(wbase + MIO_1, 0x01) : outp(wbase + MIO_1, 0x00); break; case 1: value ? outp(wbase + MIO_1, 0x03) : outp(wbase + MIO_1, 0x02); break; case 2: value ? outp(wbase + MIO_1, 0x05) : outp(wbase + MIO_1, 0x04); break; case 3: value ? outp(wbase + MIO_1, 0x07) : outp(wbase + MIO_1, 0x06); break; case 4: value ? outp(wbase + MIO_1, 0x09) : outp(wbase + MIO_1, 0x08); break; case 5: value ? outp(wbase + MIO_1, 0x0b) : outp(wbase + MIO_1, 0x0a); break; case 6: value ? outp(wbase + MIO_1, 0x0d) : outp(wbase + MIO_1, 0x0c); break; case 7: value ? outp(wbase + MIO_1, 0x0f) : outp(wbase + MIO_1, 0x0e); break; } } BYTE servo2go::digitalIn( off_t whichPort ) { return( inp(wbase + whichPort) ); } /****************************** Digital-to-Analog (and vice-versa) converter functions (DAC) and (ADC) *******************************/ servo2go::DAC( off_t whichPort, float volts ) { static long counts; /* convert voltage to counts */ counts = (long) ( 409.6*volts + .5); // input /output // // counts (decimal) -counts +0x1000 volts // // 0x1000 (4096) 0xfffff000 0 +10 // 0 0 0x1000 0 // 0xfffff001 (-4096) 0xfff 0x1fff -10 // thus domain is 0xf000 (-4096) to 0xfff (4095) //reserve slope so pos counts gices pos voltage counts = - counts; // shift for DAC counts += 0x1000; // clamp voltage if ( counts > 0x1fff ) counts = 0x1fff; if ( counts < 0 ) counts = 0; outpw (wbase + whichPort, (unsigned short) counts); } servo2go::toggleDACport (off_t whichPort, float highVoltage) { static int toggleFlag = LOW; if (toggleFlag == HIGH) { DAC(whichPort, 0.0); toggleFlag = LOW; } else { DAC(whichPort, highVoltage); toggleFlag = HIGH; } } servo2go::startSingleADConversion (off_t whichPort) { // DAC(DAC_0, 5.0); // printf("single conversion started\n"); inpw(wbase + whichPort ); //dummy read. sets the multiplexer. setTimerPeriod(TIMER_2, 5); //WAIT ~5.0 us for the multiplexer to settle outp( wbase + OCW3, 0x0a ); //select IRR while( (inp( wbase + IRR ) & S2G_TP2_INT) != S2G_TP2_INT ) ; //poll until the timer goes off outpw(wbase + whichPort, 0x0000 ); //dummy write: starts conversion. // DAC(DAC_0, 2.5); } int servo2go::checkEOCflag () { outp( wbase + OCW3, 0x0a ); //select IRR return ( (inp( wbase + IRR ) & S2G_EOC_INT) != S2G_EOC_INT ); } float servo2go::readADC( off_t whichPort ) { static short counts; static float voltage; counts = inpw( wbase + whichPort ); //process the counts if ( counts & 0x1000 ) // the number is negative take the complement { counts = ~counts; counts = counts & 0x0fff; voltage = -( counts * BITS2VOLTS ); } else { counts = counts & 0x0fff; voltage = counts * BITS2VOLTS; } return (voltage); } float servo2go::readADCpoll( off_t whichPort ) { static short counts; static float voltage; static int pollingDelay; startSingleADConversion( whichPort ); //we must wait before we start polling the EOC flag if (autoZeroFlag) { pollingDelay = 20; //microseconds see S2G manual page 30 } else { pollingDelay = 5; } setTimerPeriod(TIMER_2, pollingDelay); //wait appropriate amount of time before polling outp( wbase + OCW3, 0x0a ); //select IRR while( (inp( wbase + IRR ) & S2G_TP2_INT) != S2G_TP2_INT ) ; //wait for it to go high //now poll on the EOC bit while( (inp( wbase + IRR ) & S2G_EOC_INT) != S2G_EOC_INT ) ; counts = inpw( wbase + whichPort ); //process the counts if ( counts & 0x1000 ) // the number is negative take the complement { counts = ~counts; counts = counts & 0x0fff; voltage = -( counts * BITS2VOLTS ); } else { counts = counts & 0x0fff; voltage = counts * BITS2VOLTS; } return (voltage); } servo2go::setADCautoZero( int onOff ) { autoZeroFlag = onOff; outp( wbase + INTC, onOff ? (inp(wbase + INTC) & 0x7f) : (inp(wbase + INTC) | 0x80) ); } /************************************ Encoder section *************************************/ servo2go::initEncoders( int quadrature ) { off_t address; //program the encoder inits for( address = wbase + CNT0_C; address <= wbase + CNT6_C; address += 4 ) { // Set Counter Command Register - Master Control, Master Reset (MRST), // and Reset address pointer (RADR). outpw( address, 0x2323 ); // Set Counter Command Register - Input Control, OL Load (P3), // and Enable Inputs A and B (INA/B). outpw( address, 0x6868 ); // Set Counter Command Register - Output Control outpw( address, 0x8080 ); // Set Counter Command Register - Quadrature switch( quadrature ) { case QUADRATURE_NONE: outpw( address, 0xc0c0 ); break; case QUADRATURE_1: outpw( address, 0xc1c1 ); break; case QUADRATURE_2: outpw( address, 0xc2c2 ); break; case QUADRATURE_4: outpw( address, 0xc3c3 ); break; } } zeroEncoders(); } servo2go::zeroEncoders( byteMask which ) { off_t address; int i; for( i = 0; i < 8; i++ ) { if( which >> i & 0x01 ) //step through the mask { address = wbase + CNT0_C + i*2 - (i & 1); //command registers (subtract 1 if axis is odd) outp(address, 0x01); //reset the address pointer address -= 2; //move to data register outp(address, ENC_PRESET1); //load the preset value outp(address, ENC_PRESET2); outp(address, ENC_PRESET3); outp(address + 2, 0x09); //transfer the preset value, reset address pointer. } } } servo2go::latchEncoders() { static int i; //latches in pairs for( i = 0; i < 4; i++ ) outpw( wbase + CNT0_C + i*4, 0x0202 ); } long servo2go::readEnc( int which ) { static BYTE bytes[4]; static off_t address; static long *longbyte = (long *)bytes; address = wbase + CNT0_C + which*2 - (which & 1); //subtract 1 if odd outp( address, 0x01 ); bytes[0] = inp( address - 2 ); //data register bytes[1] = inp( address - 2 ); bytes[2] = inp( address - 2 ); bytes[3] = 0; return( *longbyte - ENC_PRESET ); } servo2go::readEnc2( int which, long values[2] ) { static short theWord[3]; static BYTE *theBytes = (BYTE *)&theWord; static BYTE *theLongs; static off_t address; theLongs = (BYTE *)values; address = wbase + CNT0_C + which*2; outpw( address, 0x0101 ); theWord[0] = inpw( address - 2 ); theWord[1] = inpw( address - 2 ); theWord[2] = inpw( address - 2 ); theLongs[0] = theBytes[0]; theLongs[1] = theBytes[2]; theLongs[2] = theBytes[4]; theLongs[3] = 0; theLongs[4] = theBytes[1]; theLongs[5] = theBytes[3]; theLongs[6] = theBytes[5]; theLongs[7] = 0; values[0] -= ENC_PRESET; values[1] -= ENC_PRESET; } #ifdef TEST_OLD //---- test routines ------ #include #include #include //#define TEST_INPUTS #define TEST_OUTPUTS void main( void ) { servo2go s2go(0x300); s2go.init(); #ifdef TEST_OUTPUTS printf("Testing all port outputs\n"); s2go.setDigitalDirections(dOutput, dOutput, dOutput, dOutput, dOutput); DML_timer timer(FREQ100Hz); while(!kbhit()) { Receive(timer, 0, 0); s2go.digitalOut( PORT_A, 0 ); s2go.digitalOut( PORT_B, 0 ); s2go.digitalOut( PORT_C, 0 ); Receive(timer, 0, 0); s2go.digitalOut( PORT_A, 0xff ); s2go.digitalOut( PORT_B, 0xff ); s2go.digitalOut( PORT_C, 0xff ); } #endif #ifdef TEST_INPUTS s2go.setDigitalDirections( dInput, dInput, dInput, dInput, dInput ); unsigned char pA, pB, pC; printf(" Port A | Port B | Port C \n"); printf(" 01234567 01234567 01234567 \n"); while(!kbhit()) { pA = s2go.digitalIn( PORT_A ); pB = s2go.digitalIn( PORT_B ); pC = s2go.digitalIn( PORT_C ); printf(" "); printf("%1d%1d%1d%1d%1d%1d%1d%1d ", pA&0x01?1:0, pA&0x02?1:0, pA&0x04?1:0, pA&0x08?1:0, pA&0x10?1:0, pA&0x20?1:0, pA&0x40?1:0, pA&0x80?1:0 ); printf("%1d%1d%1d%1d%1d%1d%1d%1d ", pB&0x01?1:0, pB&0x02?1:0, pB&0x04?1:0, pB&0x08?1:0, pB&0x10?1:0, pB&0x20?1:0, pB&0x40?1:0, pB&0x80?1:0 ); printf("%1d%1d%1d%1d%1d%1d%1d%1d ", pC&0x01?1:0, pC&0x02?1:0, pC&0x04?1:0, pC&0x08?1:0, pC&0x10?1:0, pC&0x20?1:0, pC&0x40?1:0, pC&0x80?1:0 ); printf("\r"); fflush(stdout); } #endif s2go.zeroAll(); } #endif