DECLARE SUB sxmoving ()
DECLARE SUB sxformatv (num!, num$)
DECLARE SUB sxformata (num!, num$)
DECLARE SUB sxformat (num!, num$)
DECLARE SUB sxerr ()
DECLARE SUB rs232in (msg$)
DECLARE SUB rs232out (msg$)

'****************************************************************************
'This module contains subroutines to control a Compumotor stepping motor
'with an SX indexer/drive with RS232 interface.
'
'Zachary Wolf
'12/16/97
'****************************************************************************

'Module level parameters
COMMON SHARED /sxdrive/ sxrs232addrP$, ccwsoftlimitP$, cwsoftlimitP$, limitdecelP$, accelP$, decelP$, velP$

'Semi-permanent parameters
CONST nstepsperrevP& = 25000      'motor steps
CONST sxrs232openparP$ = "COM2:9600,N,8,1,ASC,RS,CD0,CS0,DS0"

SUB sxerr

'****************************************************************************
'This subroutine reports any errors from the SX indexer.
'
'Zachary Wolf
'1/6/98
'****************************************************************************

'Clear the input buffer
CALL rs232in(msg$)

'Have the indexer report any errors
CALL rs232out(sxrs232addrP$ + "RSE")

'Get indexer response
CALL rs232in(response$)

'Extract the error message
starpos% = INSTR(response$, "*")
errmsg$ = MID$(response$, starpos% + 1)

'Notify the operator if there is an error
IF INSTR(errmsg$, "NO_ERRORS") <> 0 THEN
 'OK, found NO_ERRORS
ELSE
 PRINT
 PRINT "Error!!! " + errmsg$
END IF

END SUB

SUB sxformata (num!, num$)

'***************************************************************************
'This subroutine formats numbers for the SX drive.
'
'Input:
' num!, number to format
'
'Output:
' num$, formatted number in a string
'
'Zachary Wolf
'1/14/98
'***************************************************************************

'Open a temporary file to write to
filenum% = FREEFILE
OPEN "temp.tmp" FOR OUTPUT AS #filenum%

'Output the number
PRINT #filenum%, USING "####.##"; num!

'Close the file
CLOSE filenum%

'Open the temporary file to read
filenum% = FREEFILE
OPEN "temp.tmp" FOR INPUT AS #filenum%

'Input the number into a string
INPUT #filenum%, strnum$

'Close the file
CLOSE filenum%

'Delete the temporary file
SHELL "del temp.tmp"

'Return the number in a string
num$ = LTRIM$(RTRIM$(strnum$))

END SUB

SUB sxformatv (num!, num$)

'***************************************************************************
'This subroutine formats numbers for the SX drive.
'
'Input:
' num!, number to format
'
'Output:
' num$, formatted number in a string
'
'Zachary Wolf
'1/14/98
'***************************************************************************

'Open a temporary file to write to
filenum% = FREEFILE
OPEN "temp.tmp" FOR OUTPUT AS #filenum%

'Output the number
PRINT #filenum%, USING "##.#####"; num!

'Close the file
CLOSE filenum%

'Open the temporary file to read
filenum% = FREEFILE
OPEN "temp.tmp" FOR INPUT AS #filenum%

'Input the number into a string
INPUT #filenum%, strnum$

'Close the file
CLOSE filenum%

'Delete the temporary file
SHELL "del temp.tmp"

'Return the number in a string
num$ = LTRIM$(RTRIM$(strnum$))

END SUB

SUB sxgetencpos (dlines&)

'****************************************************************************
'This subroutine reports the encoder position in encoder lines from zero.
'Positive dlines& corresponds to CW rotations.  Negative dlines& corresponds
'to CCW rotations.
'
'Output:
' dlines&, encoder position in lines from zero (+ CW, - CCW)
'
'Zachary Wolf
'12/18/97
'****************************************************************************

'Clear the input buffer
CALL rs232in(msg$)

'Have the indexer report the encoder absolute position
CALL rs232out(sxrs232addrP$ + "PX")

'Get indexer response
CALL rs232in(response$)

'Extract encoder position in encoder steps
starpos% = INSTR(response$, "*")
dsteps$ = MID$(response$, starpos% + 1, 11)

'Calculate the position in encoder lines
dsteps& = VAL(dsteps$)
dlines& = dsteps& / 4

END SUB

SUB sxgetinputs (inputs$)

'****************************************************************************
'This subroutine gets the state of the inputs to the SX indexer.
'
'Output:
' inputs$, state of the inputes to the SX indexer
'
'Zachary Wolf
'12/16/97
'****************************************************************************

'Clear the input buffer
CALL rs232in(msg$)

'Have the indexer report the state of the inputs
CALL rs232out(sxrs232addrP$ + "IS")

'Get the indexer response
CALL rs232in(response$)

'Extract the inputs
starpos% = INSTR(response$, "*")
inputs$ = MID$(response$, starpos% + 1, 14)

'Debug
'PRINT "Inputs = " + inputs$

END SUB

SUB sxgetmotpos (d!)

'****************************************************************************
'This subroutine reports the motor position d! in revolutions from zero.
'Positive d! corresponds to CW rotations.  Negative d! corresponds to
'CCW rotations.
'
'Output:
' d!, motor position in revolutions from zero (+ CW, - CCW)
'
'Zachary Wolf
'12/16/97
'****************************************************************************

'Clear the input buffer
CALL rs232in(msg$)

'Have the indexer report the absolute position
CALL rs232out(sxrs232addrP$ + "PR")

'Get indexer response
CALL rs232in(response$)

'Extract motor position
starpos% = INSTR(response$, "*")
dsteps$ = MID$(response$, starpos% + 1, 11)

'Calculate the position in revolutions
dsteps& = VAL(dsteps$)
d! = (1! * dsteps&) / nstepsperrevP&

'Debug
'PRINT "Dsteps = " + dsteps$
'PRINT "D = "; d!; " rev"

END SUB

SUB sxgohome

'****************************************************************************
'This subroutine has the SX motor go the the home position.
'
'Zachary Wolf
'12/17/97
'****************************************************************************

'Go home
CALL rs232out(sxrs232addrP$ + "GH")

END SUB

SUB sxinit

'****************************************************************************
'This subroutine initializes the Compumotor SX indexer/drive.
'
'Zachary Wolf
'12/15/97
'****************************************************************************

'Message
PRINT
PRINT "Resetting SX indexer/drive.";

'Send a command to clear communications
CALL rs232out(sxrs232addrP$ + "V1")
CALL rs232in(msg$)

'Message
PRINT ".";

'Turn on the drive
CALL rs232out(sxrs232addrP$ + "ON")

'Message
PRINT ".";

'Reset the drive
CALL rs232out(sxrs232addrP$ + "Z")
SLEEP 4

'Message
PRINT ".";

'Disable interactive mode
CALL rs232out(sxrs232addrP$ + "SSI1")

'Set the mode to normal
CALL rs232out(sxrs232addrP$ + "MN")

'Message
PRINT ".";

'Enable hardware limit switches
CALL rs232out(sxrs232addrP$ + "LD0")

'Enable software limits
CALL rs232out(sxrs232addrP$ + "SLD0")

'Set software limits
CALL rs232out(sxrs232addrP$ + "SL" + ccwsoftlimitP$ + "," + cwsoftlimitP$)

'Set the deceleration when a limit is reached
CALL rs232out(sxrs232addrP$ + "LAD" + limitdecelP$)

'Message
PRINT ".";

'Set the motion parameters
CALL rs232out(sxrs232addrP$ + "A" + accelP$)     'Acceleration
CALL rs232out(sxrs232addrP$ + "AD" + decelP$)    'Deceleration
CALL rs232out(sxrs232addrP$ + "V" + velP$)       'Velocity

'Message
PRINT ".";

'Enable and set parameters for manual jogs
CALL rs232out(sxrs232addrP$ + "OSE1")            'Enable jogs
CALL rs232out(sxrs232addrP$ + "JA" + accelP$)    'Jog acceleration
CALL rs232out(sxrs232addrP$ + "JAD" + decelP$)   'Jog deceleration
CALL rs232out(sxrs232addrP$ + "JVH" + velP$)     'Jog velocity high
CALL rs232out(sxrs232addrP$ + "JVL" + velP$)     'Jog velocity low
CALL rs232out(sxrs232addrP$ + "MPI")             'Mode incremental
CALL rs232out(sxrs232addrP$ + "D0")              'Distance 0
CALL rs232out(sxrs232addrP$ + "G")               'Go command to initiate jogs
CALL rs232out(sxrs232addrP$ + "IN1J")            'Use input 1 for manual CW jogs
CALL rs232out(sxrs232addrP$ + "IN2K")            'Use input 2 for manual CCW jogs

'Message
PRINT "."

'Set up homing parameters
CALL rs232out(sxrs232addrP$ + "OSB1")            'Back up to home
CALL rs232out(sxrs232addrP$ + "OSG1")            'Final homing direction CCW
CALL rs232out(sxrs232addrP$ + "OSH0")            'Stop at CW edge of switch
CALL rs232out(sxrs232addrP$ + "GHV-" + velP$)    'Go home velocity CCW
CALL rs232out(sxrs232addrP$ + "GHF" + velP$)     'Go home final velocity
CALL rs232out(sxrs232addrP$ + "GHA" + accelP$)   'Go home acceleration
CALL rs232out(sxrs232addrP$ + "GHAD" + decelP$)  'Go home deceleration

'Check for any invalid commands
CALL rs232in(msg$)
IF INSTR(msg$, "*") <> 0 THEN               'Look for invalid commands
 PRINT
 PRINT "Problem in SXINIT initialization commands."
 PRINT MID$(msg$, INSTR(msg$, "*"), 15)
END IF

'Check for system errors
CALL sxerr

'Display SX parameters
CALL rs232in(msg$)
CALL rs232out(sxrs232addrP$ + "DR")
CALL rs232in(msg$)
msgpos% = INSTR(msg$, "*V")
IF msgpos% > 0 THEN PRINT MID$(msg$, msgpos%, 9);

'Report the SX status
CALL rs232in(msg$)
CALL rs232out(sxrs232addrP$ + "R")
CALL rs232in(msg$)
msgpos% = INSTR(msg$, "*")
IF msgpos% > 0 THEN PRINT "     "; MID$(msg$, msgpos%, 2)

END SUB

SUB sxmoveabs (d!)

'****************************************************************************
'This subroutine has the motor turn to position d! revolutions from zero.
'Positive d! is in the clockwise direction.  Negative d! is in the
'counterclockwise direction.
'
'Input:
' d!, number of revolutions to turn to (+ CW, - CCW)
'
'Zachary Wolf
'12/16/97
'****************************************************************************

'Set the mode to position absolute
CALL rs232out(sxrs232addrP$ + "MPA")

'Calculate the number of steps the position corresponds to
dsteps& = d! * nstepsperrevP&

'Turn the motor
CALL rs232out(sxrs232addrP$ + "D" + LTRIM$(STR$(dsteps&)))
CALL rs232out(sxrs232addrP$ + "G")

'Wait until the move is over
CALL sxmoving

'Check for errors
CALL sxerr

END SUB

SUB sxmoverel (d!)

'****************************************************************************
'This subroutine has the motor turn d! revolutions.  Positive d! gives
'clockwise rotation.  Negative d! gives counterclockwise rotation.
'
'Input:
' d!, number of revolutions to turn (+ CW, - CCW)
'
'Zachary Wolf
'12/16/97
'****************************************************************************

'Set the mode to position incremental
CALL rs232out(sxrs232addrP$ + "MPI")

'Calculate the number of steps the motor should take
nsteps& = d! * nstepsperrevP&

'Turn the motor
CALL rs232out(sxrs232addrP$ + "D" + LTRIM$(STR$(nsteps&)))
CALL rs232out(sxrs232addrP$ + "G")

'Wait until the move is over
CALL sxmoving

'Check for errors
CALL sxerr

END SUB

SUB sxmoving

'****************************************************************************
'This subroutine delays program execution until a move is over.
'
'Zachary Wolf
'1/14/98
'****************************************************************************

'Clear the input buffer
checkagain:
CALL rs232in(msg$)

'Have the indexer report its status
CALL rs232out(sxrs232addrP$ + "R")

'Get indexer response
CALL rs232in(response$)
msgpos% = INSTR(response$, "*")
IF msgpos% > 0 THEN response$ = MID$(response$, msgpos%, 2)

'Debug
'PRINT response$

'Analyze the response
IF response$ = "*R" THEN
 EXIT SUB
ELSEIF response$ = "*S" THEN
 PRINT "SX needs attention"
 EXIT SUB
ELSEIF response$ = "*B" THEN
 SLEEP 1
 GOTO checkagain
ELSEIF response$ = "*C" THEN
 PRINT "SX needs attention"
 SLEEP 1
 GOTO checkagain
ELSEIF response$ = "*Z" THEN
 SLEEP 1
 GOTO checkagain
ELSEIF response$ = "*K" THEN
 EXIT SUB
ELSEIF response$ = "*W" THEN
 EXIT SUB
ELSE
 PRINT "Unknown SX status response"
END IF

END SUB

SUB sxsetaddr (rs232addr$)

'****************************************************************************
'This subroutine sets the RS232 address the SX drive sends commands to.
'
'Input:
' rs232addr$, indexer address (1 to 16)
'
'Zachary Wolf
'1/15/98
'****************************************************************************

'Set the parameters
sxrs232addrP$ = rs232addr$

END SUB

SUB sxsetpar (rs232addr$, ccwsoftlimit!, cwsoftlimit!, limitdecel!, accel!, decel!, vel!)

'****************************************************************************
'This subroutine sets parameters for the SXDRIVE module.
'
'Input:
' rs232addr$, indexer address (1 to 16)
' ccwsoftlimit!, CCW software motion limit (rev)
' cwsoftlimit!, CW software motion limit (rev)
' limitdecel!, deceleration when a limit is reached (rps^2)
' accel!, acceleration (rps^2)
' decel!, deceleration (rps^2)
' vel!, velocity (rps)
'
'Zachary Wolf
'12/15/97
'****************************************************************************

'Set the parameters
sxrs232addrP$ = rs232addr$
ccwsoftlimitP$ = LTRIM$(STR$(FIX(ccwsoftlimit! * nstepsperrevP&)))
cwsoftlimitP$ = LTRIM$(STR$(FIX(cwsoftlimit! * nstepsperrevP&)))

IF limitdecel! > 0! THEN
 CALL sxformata(limitdecel!, limitdecel$)
 limitdecelP$ = limitdecel$
ELSE
 PRINT "SXSETPAR: problem with limitdecel!"
END IF

IF accel! > 0! THEN
 CALL sxformata(accel!, accel$)
 accelP$ = accel$
ELSE
 PRINT "SXSETPAR: problem with accel!"
END IF

IF decel! > 0! THEN
 CALL sxformata(decel!, decel$)
 decelP$ = decel$
ELSE
 PRINT "SXSETPAR: problem with decel!"
END IF

IF vel! > 0! THEN
 CALL sxformatv(vel!, vel$)
 velP$ = vel$
ELSE
 PRINT "SXSETPAR: problem with vel!"
END IF

END SUB

SUB sxzero

'****************************************************************************
'This subroutine zeros the motor step counter and the absolute encoder
'position counter.
'
'Zachary Wolf
'12/17/97
'****************************************************************************

'Set the mode to position absolute
CALL rs232out(sxrs232addrP$ + "MPA")

'Zero the motor step counter and encoder position counter
CALL rs232out(sxrs232addrP$ + "PZ")

END SUB