DECLARE SUB sxmoving ()
DECLARE SUB movethzero ()
DECLARE SUB movezzero ()
DECLARE SUB movelogzgohome (logfile$)
DECLARE SUB movelogthgohome (logfile$)
DECLARE SUB sxzero ()
DECLARE SUB movethrel (dth!)
DECLARE SUB movezrel (dz!)
DECLARE SUB movelogmovethabs (logfile$, th!, thmeas!)
DECLARE SUB movelogmovethrel (logfile$, dth!)
DECLARE SUB movezabs (z!)
DECLARE SUB movethabs (th!)
DECLARE SUB movelogmovezrel (logfile$, dz!)
DECLARE SUB sxmoverel (d!)
DECLARE SUB movethgohome ()
DECLARE SUB movezgohome ()
DECLARE SUB sxgohome ()
DECLARE SUB sxgetencpos (dlines&)
DECLARE SUB sxinit ()
DECLARE SUB sxmoveabs (d!)
DECLARE SUB sxsetaddr (rs232addr$)
DECLARE SUB sxsetpar (rs232addr$, ccwsoftlimit!, cwsoftlimit!, limitdecel!, accel!, decel!, vel!)
DECLARE SUB movegetth (th!)
DECLARE SUB movegetz (z!)
DECLARE SUB movelogmoveth (logfile$, th!, thmeas!)
DECLARE SUB movelogmovezabs (logfile$, z!, zmeas!)
DECLARE SUB movesetupth ()
DECLARE SUB movesetupz ()
DECLARE SUB moveth (th!)
DECLARE SUB movez (z!)

'****************************************************************************
'Module MOVE
'
'This module contains routines to move the mapper in Z and Theta.
'
'Zachary Wolf
'1/8/98
'****************************************************************************

'Common block for module level parameters
COMMON SHARED /move/ logfileP$
COMMON SHARED /movez/ zdrivers232addrP$, zdrivenmeterperrevP!, zreadnmeterperlineP!, zccwsoftlimitP!, zcwsoftlimitP!, zlimitdecelP!, zaccelP!, zdecelP!, zvelP!
COMMON SHARED /movet/ tdrivers232addrP$, tdrivendegperrevP!, treadndegperlineP!, tccwsoftlimitP!, tcwsoftlimitP!, tlimitdecelP!, taccelP!, tdecelP!, tvelP!

SUB movecheckpar (ok$)

'****************************************************************************
'This subroutine checks that all required MOVE parameters have been assigned
'values.
'
'Output:
' ok$, "y" if parameter assignments are ok, "n" otherwise
'
'Zachary Wolf
'1/8/98
'****************************************************************************

'Default value
ok$ = "n"

'Logfile
IF logfileP$ = "" THEN
 PRINT "MOVE: Log file not defined"
 EXIT SUB
END IF

'Z motion parameters
IF (zdrivenmeterperrev! <= 0! OR zreadnmeterperline! <= 0!) THEN
 PRINT "MOVE: Z motion parameters undefined"
 EXIT SUB
END IF

'TH motion parameters
IF (tdrivendegperrev! <= 0! OR treadndegperline! <= 0!) THEN
 PRINT "MOVE: TH motion parameters undefined"
 EXIT SUB
END IF

'If we made it this far, all parameters have values
ok$ = "y"

END SUB

SUB movegetth (th!)

'****************************************************************************
'This subroutine gets the theta position of the arm from the
'theta encoder.
'
'Output:
' th!, theta position in degrees
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Get the theta positon in encoder lines
CALL sxgetencpos(dlines&)

'Calculate the theta position in degrees
th! = dlines& * treadndegperlineP!

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movegetz (z!)

'****************************************************************************
'This subroutine gets the z position of the stage from the z encoder.
'
'Output:
' z!, z position in meters
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Get the z positon in encoder lines
CALL sxgetencpos(dlines&)

'Calculate the z position in meters
z! = dlines& * zreadnmeterperlineP!

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB moveinit

'****************************************************************************
'This subroutine does the initialization for all motions.
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'Message
PRINT
PRINT "Z motion initialization..."

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Initialize the Z drive
CALL sxinit

'Message
PRINT
PRINT "Theta motion initialization..."

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Initialize the TH drive
CALL sxinit

END SUB

SUB movelogmovethabs (logfile$, th!, thmeas!)

'****************************************************************************
'This subroutine records the arm move to the log file.
'
'Input:
' logfile$, the name of the log file
' th!, requested theta location
' thmeas!, measured th location of the arm
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested TH move to ";
PRINT #filenum%, USING " ####.####"; th!;
PRINT #filenum%, " deg"
PRINT #filenum%, "          THmeas = ";
PRINT #filenum%, USING " ####.####"; thmeas!;
PRINT #filenum%, " deg"

'Close the log file
CLOSE filenum%

END SUB

SUB movelogmovethrel (logfile$, dth!)

'****************************************************************************
'This subroutine records the arm move to the log file.
'
'Input:
' logfile$, the name of the log file
' dth!, requested theta move amount
'
'Zachary Wolf
'2/28/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested TH relative move of ";
PRINT #filenum%, USING " ####.####"; dth!;
PRINT #filenum%, " deg"

'Close the log file
CLOSE filenum%

END SUB

SUB movelogmovezabs (logfile$, z!, zmeas!)

'****************************************************************************
'This subroutine records the stage move to the log file.
'
'Input:
' logfile$, the name of the log file
' z!, requested z location
' zmeas!, measured z location of the stage
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested Z move to ";
PRINT #filenum%, USING " ####.####"; z!;
PRINT #filenum%, " m"
PRINT #filenum%, "          Zmeas = ";
PRINT #filenum%, USING " ####.####"; zmeas!;
PRINT #filenum%, " m"

'Close the log file
CLOSE filenum%

END SUB

SUB movelogmovezrel (logfile$, dz!)

'****************************************************************************
'This subroutine records the stage move to the log file.
'
'Input:
' logfile$, the name of the log file
' dz!, requested distance to move
'
'Zachary Wolf
'2/28/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested Z relative move of ";
PRINT #filenum%, USING " ####.####"; dz!;
PRINT #filenum%, " m"

'Close the log file
CLOSE filenum%

END SUB

SUB movelogthgohome (logfile$)

'****************************************************************************
'This subroutine records the stage move to the log file.
'
'Input:
' logfile$, the name of the log file
'
'Zachary Wolf
'3/6/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested TH go home."

'Close the log file
CLOSE filenum%

END SUB

SUB movelogzgohome (logfile$)

'****************************************************************************
'This subroutine records the stage move to the log file.
'
'Input:
' logfile$, the name of the log file
'
'Zachary Wolf
'3/6/98
'****************************************************************************

'Open the log file
filenum% = FREEFILE
OPEN logfile$ FOR APPEND AS filenum%

'Write the locations
PRINT #filenum%, TIME$; "  Requested Z go home."

'Close the log file
CLOSE filenum%

END SUB

SUB movesetparth (logfile$, tdrivers232addr$, tdrivendegperrev!, treadndegperline!, tccwsoftlimit!, tcwsoftlimit!, tlimitdecel!, taccel!, tdecel!, tvel!)

'****************************************************************************
'This subroutine sets the parameters for theta motions.
'
'Input:
' logfile$, log file
' tdrivers232addr$, TH drive RS232 address
' tdrivendegperrev!, TH drive, number of degrees per motor rev
' treadndegperline!, TH read, number of degrees per encoder line
' tccwsoftlimit!, TH drive, CCW software motion limit (deg)
' tcwsoftlimit!, TH drive, CW software motion limit (deg)
' tlimitdecel!, TH drive, deceleration when a limit is reached (deg per s^2)
' taccel!, TH drive, acceleration (deg per s^2)
' tdecel!, TH drive, deceleration (deg per s^2)
' tvel!, TH drive, velocity (deg per s)
'
'Zachary Wolf
'1/8/98
'****************************************************************************

'Save the information in a common block for future use
logfileP$ = logfile$
tdrivers232addrP$ = tdrivers232addr$
tdrivendegperrevP! = tdrivendegperrev!
treadndegperlineP! = treadndegperline!
tccwsoftlimitP! = tccwsoftlimit!
tcwsoftlimitP! = tcwsoftlimit!
tlimitdecelP! = tlimitdecel!
taccelP! = taccel!
tdecelP! = tdecel!
tvelP! = tvel!

END SUB

SUB movesetparz (logfile$, zdrivers232addr$, zdrivenmeterperrev!, zreadnmeterperline!, zccwsoftlimit!, zcwsoftlimit!, zlimitdecel!, zaccel!, zdecel!, zvel!)

'****************************************************************************
'This subroutine sets the parameters for z motions.
'
'Input:
' logfile$, log file
' zdrivers232addr$, Z drive RS232 address
' zdrivenmeterperrev!, Z drive, number of meters per motor rev
' zreadnmeterperline!, Z read, number of meters per encoder line
' zccwsoftlimit!, Z drive, CCW software motion limit (m)
' zcwsoftlimit!, Z drive, CW software motion limit (m)
' zlimitdecel!, Z drive, deceleration when a limit is reached (mps^2)
' zaccel!, Z drive, acceleration (mps^2)
' zdecel!, Z drive, deceleration (mps^2)
' zvel!, Z drive, velocity (mps)
'
'Zachary Wolf
'1/8/98
'****************************************************************************

'Save the information in a common block for future use
logfileP$ = logfile$
zdrivers232addrP$ = zdrivers232addr$
zdrivenmeterperrevP! = zdrivenmeterperrev!
zreadnmeterperlineP! = zreadnmeterperline!
zccwsoftlimitP! = zccwsoftlimit!
zcwsoftlimitP! = zcwsoftlimit!
zlimitdecelP! = zlimitdecel!
zaccelP! = zaccel!
zdecelP! = zdecel!
zvelP! = zvel!

END SUB

SUB movesetupth

'***************************************************************************
'This subroutine prepares the SX module for TH motion or reading.
'
'Zachary Wolf
'1/19/98
'***************************************************************************

'Set up the SX module for TH motion
rs232addr$ = tdrivers232addrP$
ccwsoftlimit! = tccwsoftlimitP! / tdrivendegperrevP!
cwsoftlimit! = tcwsoftlimitP! / tdrivendegperrevP!
limitdecel! = tlimitdecelP! / tdrivendegperrevP!
accel! = taccelP! / tdrivendegperrevP!
decel! = tdecelP! / tdrivendegperrevP!
vel! = tvelP! / tdrivendegperrevP!
CALL sxsetpar(rs232addr$, ccwsoftlimit!, cwsoftlimit!, limitdecel!, accel!, decel!, vel!)

END SUB

SUB movesetupz

'***************************************************************************
'This subroutine prepares the SX module for Z motion or reading.
'
'Zachary Wolf
'1/19/98
'***************************************************************************

'Set up the SX module for Z motion
rs232addr$ = zdrivers232addrP$
ccwsoftlimit! = zccwsoftlimitP! / zdrivenmeterperrevP!
cwsoftlimit! = zcwsoftlimitP! / zdrivenmeterperrevP!
limitdecel! = zlimitdecelP! / zdrivenmeterperrevP!
accel! = zaccelP! / zdrivenmeterperrevP!
decel! = zdecelP! / zdrivenmeterperrevP!
vel! = zvelP! / zdrivenmeterperrevP!
CALL sxsetpar(rs232addr$, ccwsoftlimit!, cwsoftlimit!, limitdecel!, accel!, decel!, vel!)

END SUB

SUB movethabs (th!)

'***************************************************************************
'This subroutine moves the arm in theta to a given position.
'
'Input:
' th!, desired theta position of the arm (degrees)
'
'Zachary Wolf
'1/9/98
'***************************************************************************

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Move the arm to the desired theta position
drev! = th! / tdrivendegperrevP!
CALL sxmoveabs(drev!)

'Check the actual theta position
CALL movegetth(thmeas!)

'Check that the arm is near the desired location
'IF ABS(thmeas! - th!) > 5! THEN
' PRINT
' PRINT "Problem with the theta movement of the arm."
' PRINT "THdesired = "; th!; " deg, THmeas = "; thmeas!; " deg"
'END IF

'Log the move
CALL movelogmovethabs(logfileP$, th!, thmeas!)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movethgohome

'***************************************************************************
'This subroutine moves the stage in TH to its home position.
'
'Zachary Wolf
'2/9/98
'***************************************************************************

'Move forward 2.5 degrees
CALL movethrel(2.5)

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Move the stage to its home position
CALL sxgohome

'Wait until the move is over
CALL sxmoving

'Log the go home operation
CALL movelogthgohome(logfileP$)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movethrel (dth!)

'***************************************************************************
'This subroutine moves the arm in theta a given amount.
'
'Input:
' dth!, desired theta motion of the arm (degrees)
'
'Zachary Wolf
'2/28/98
'***************************************************************************

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Move the arm the desired relative amount
drev! = dth! / tdrivendegperrevP!
CALL sxmoverel(drev!)

'Check the actual theta position
'CALL movegetth(thmeas!)

'Check that the arm is near the desired location
'IF ABS(thmeas! - th!) > 5! THEN
' PRINT
' PRINT "Problem with the theta movement of the arm."
' PRINT "THdesired = "; th!; " deg, THmeas = "; thmeas!; " deg"
'END IF

'Log the move
CALL movelogmovethrel(logfileP$, dth!)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movethuser

'****************************************************************************
'This subroutine lets the user control the theta position of the arm.
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'See what the user wants to do
beginth:
PRINT
PRINT "Type"
PRINT "  P to move in phi"
PRINT "  R for a relative move in phi"
PRINT "  H to move to the phi home position"
PRINT "  M to measure the phi position with the encoder"
PRINT "  Z to make the present position the zero position"
PRINT "  Q to quit"
INPUT ">", cmd$

'Do as the user requested
IF cmd$ = "P" OR cmd$ = "p" THEN
 INPUT "Enter a phi value in degrees: ", thval$
 thval! = VAL(thval$)
 CALL movethabs(thval!)
ELSEIF cmd$ = "R" OR cmd$ = "r" THEN
 INPUT "Enter a phi relative move distance in degrees: ", thval$
 thval! = VAL(thval$)
 CALL movethrel(thval!)
ELSEIF cmd$ = "H" OR cmd$ = "h" THEN
 CALL movethgohome
ELSEIF cmd$ = "M" OR cmd$ = "m" THEN
 CALL movegetth(thmeas!)
 PRINT
 PRINT "Pmeas = "; thmeas!; " deg"
ELSEIF cmd$ = "Z" OR cmd$ = "z" THEN
 CALL movethzero
 PRINT "The present position is now the zero position."
ELSEIF cmd$ = "Q" OR cmd$ = "q" THEN
 GOTO endth
ELSE
 PRINT "Unknown command."
END IF
GOTO beginth

'Done
endth:

END SUB

SUB movethzero

'***************************************************************************
'This subroutine zeros the theta encoder.
'
'Zachary Wolf
'2/28/98
'***************************************************************************

'Prepare the SX module for accessing the TH drive
CALL movesetupth

'Zero the encoder
CALL sxzero

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movezabs (z!)

'***************************************************************************
'This subroutine moves the stage in z to a given position.
'
'Input:
' z!, desired z position of the stage (meters)
'
'Zachary Wolf
'1/9/98
'***************************************************************************

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Move the stage to the desired z position
drev! = z! / zdrivenmeterperrevP!
CALL sxmoveabs(drev!)

'Check the actual z position
CALL movegetz(zmeas!)

'Check that the stage is near the desired location
'IF ABS(zmeas! - z!) > .05 THEN
' PRINT
' PRINT "Problem with the z movement of the stages."
' PRINT "Zdesired = "; z!; " m, Zmeas = "; zmeas!; " m"
'END IF

'Log the move
CALL movelogmovezabs(logfileP$, z!, zmeas!)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movezgohome

'***************************************************************************
'This subroutine moves the stage in z to its home position.
'
'Zachary Wolf
'2/9/98
'***************************************************************************

'Move forward 1 cm
CALL movezrel(.01)

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Move the stage to its home position
CALL sxgohome

'Wait until the move is over
CALL sxmoving

'Log the go home operation
CALL movelogzgohome(logfileP$)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movezrel (dz!)

'***************************************************************************
'This subroutine moves the stage in z a given amount.
'
'Input:
' dz!, desired distance to move the stage (meters)
'
'Zachary Wolf
'2/28/98
'***************************************************************************

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Move the stage the desired amount
drev! = dz! / zdrivenmeterperrevP!
CALL sxmoverel(drev!)

'Log the move
CALL movelogmovezrel(logfileP$, dz!)

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB

SUB movezuser

'****************************************************************************
'This subroutine lets the user control the z position of the stage.
'
'Zachary Wolf
'1/9/98
'****************************************************************************

'See what the user wants to do
beginz:
PRINT
PRINT "Type"
PRINT "  Z to move in z"
PRINT "  R for a relative move in z"
PRINT "  H to move to the home position"
PRINT "  M to measure the z position with the encoder"
PRINT "  O to make the present position the zero position"
PRINT "  Q to quit"
INPUT ">", cmd$

'Do as the user requested
IF cmd$ = "Z" OR cmd$ = "z" THEN
 INPUT "Enter a z value in meters: ", zval$
 zval! = VAL(zval$)
 CALL movezabs(zval!)
ELSEIF cmd$ = "R" OR cmd$ = "r" THEN
 INPUT "Enter a z relative move distance in meters: ", zval$
 zval! = VAL(zval$)
 CALL movezrel(zval!)
ELSEIF cmd$ = "H" OR cmd$ = "h" THEN
 CALL movezgohome
ELSEIF cmd$ = "M" OR cmd$ = "m" THEN
 CALL movegetz(zmeas!)
 PRINT
 PRINT "Zmeas = "; zmeas!; " m"
ELSEIF cmd$ = "O" OR cmd$ = "o" THEN
 CALL movezzero
 PRINT "The present position is now the zero position."
ELSEIF cmd$ = "Q" OR cmd$ = "q" THEN
 GOTO endz
ELSE
 PRINT "Unknown command."
END IF
GOTO beginz

'Done
endz:

END SUB

SUB movezzero

'***************************************************************************
'This subroutine zeros the z encoder.
'
'Zachary Wolf
'2/28/98
'***************************************************************************

'Prepare the SX module for accessing the Z drive
CALL movesetupz

'Zero the encoder
CALL sxzero

'Make sure SX commands are only sent to the desired drive
CALL sxsetaddr("-1")

END SUB