historicalsource
/
cruisin-usa
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
cruisin-usa/MOTION.ASM

2411 lines
42 KiB
NASM
Executable File
Raw Permalink Blame History

.FILE "MOTION.ASM"
*----------------------------------------------------------------------------
*
*
*COPYRIGHT (C) 1994 BY TV GAMES, INC.
*ALL RIGHTS RESERVED
*
*
*
*Misc Notes:
*
* The Galil board is unreliable under conditions of abort, and can
* become confused. To illustrate this point D/L a program, send a
* HW Abort, then send an XQ command, and send a HW Abort. We have been
* able to repeatedly shut the Galil board down, so that it will not
* respond (under normal AB circumstances even under abort, Galil will
* still return a prompt).
*
* We have to place the following redundancies into our code to assure
* safe handling of the motion control:
*
* 1) within the program the TL (torque limit) must be tested to
* insure no runaway motors (test incombination of movement)
* 2) check HW value for safety controls
* 3) every thirty frames the Galil board is polled for abort status
* 4) every sixty frames (when motion is OK) we must make sure
* that we are receiving characters
*
*
*
.include MACS.EQU
.include MPROC.EQU
.include VUNIT.EQU
.include CMOS.EQU
.include SYSID.EQU
.include SYS.EQU
.include GLOBALS.EQU
.include SNDTAB.EQU
.include PALL.EQU
.include OBJECTS.EQU
.include TEXT.EQU
.include DIAG.EQU
.globl CHECK_MOTION_DIP
.globl CHECK_MOTION_PRESENT
.globl CHECK_MOTION_DIP,CHECK_MOTION_PRESENT
MIN_MOVE_DIST .set 200
.bss MOTION_NOT_ON,1
.bss MOTION_STOP_HIT,1
.bss MOTION_RCV_TIKS,1
.text
*----------------------------------------------------------------------------
*This is the initial check of the galil board.
*The CMOS value will be set to ON if cool else OFF
*No other routine may turn it ON.
*No other routine will ever download or initialize galil
*This routine may only be called at powerup
*
*
*
*
INITIALIZATION_MOTION_CHECK:
CALL CLEAR_LATCH_ERROR
LDI 1,R2
SETAUD ADJ_MOTION_PRESENT
LDI 0,R2
SETAUD AUD_RESET_TOTALLY
CALL CHECK_MOTION_DIP
RETSNZ
LDI 1,RC
TEXTITT "MOTION SETUP TESTING...",1,250
CALL RESET_GALIL
CALL WAIT_ACK_REAL
CALL CHECK_MOTION_PRESENT
BNE INIT_MOTION_ERROROUT
;send it UP, for .5 inch (1000)
;
romdata
MINIT10 .string "SP 15000, 15000, 15000",13,0
MINIT11 .string "AC 105000,105000,105000",13,0
MINIT12 .string "DC 435000,435000,435000",13,"TL 7,7,7",13,0
MINIT13 .string "IT 0.1,0.1,0.1",13,0
MINIT9 .string "SH;TL 7,7,7;ER 1000,1000,1000;OE 1,1,1;PR 4000,4000,4000;BG",13,0
MINIT9DIAG .string "SH;PR 4000,4000,4000;BG",13,0
MINIT1 .string "OE 0,0,0;TL 5,5,5;JG 13000,13000,13000;BG;",13,0
MINITQX .string "MG ",34,"X",34,", _TSX {$2.0}",13,0
MINITQY .string "MG ",34,"Y",34,", _TSY {$2.0}",13,0
MINITQZ .string "MG ",34,"Z",34,", _TSZ {$2.0}",13,0
.text
;test limit switches if not found in 3 seconds shutdown
;motion
;
;
LDI 1,RC
TEXTITT "TESTING UPPER LIMITS",50,260
LDL MINIT10,AR2 ;send init strings first
CALL SEND_CMD
CALL WAIT_ACK
LDL MINIT11,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL MINIT12,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL MINIT13,AR2
CALL SEND_CMD
CALL WAIT_ACK
READAUD ADJ_OUTOFDIAG
CMPI 1,R0
BNE JAJA66
LDL MINIT9DIAG,AR2
BU JAJA887
JAJA66 LDL MINIT9,AR2 ;lift off the failsafes (PR 4000,4000,4000)
JAJA887 CALL SEND_CMD
CALL WAIT_ACK
LDI 100,AR3
GLLLT CALL WAIT_FOR_VBLANK
DBU AR3,GLLLT
CALL ABORT_RESET_GALIL
;
;check for safety errors
;hold for 10 seconds until all errors
;are cleared.
;
SAFETY_ERROR_CHK
LDI 57*10,AR3
MILOOP2
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
AND @CME_MASK,R0
BZ NO_ERRORMIL
LDI 1,RC
TEXTITT "MOTION ERROR",50,270
TEXTITT "CLEAR MATS AND BEAM TO CONTINUE",50,280
CALL WAIT_FOR_VBLANK
DBU AR3,MILOOP2
BU INIT_MOTION_ERROROUT
;
;no safety errors, continue on
;
NO_ERRORMIL
LDI 2000,R0
CALL WAITMSEC
CALL ABORT_RESET_GALIL
FILL 50,270,500,289,0
LDL MINIT1,AR2 ;send it up
CALL SEND_CMD
CALL WAIT_ACK
LDI 10,AR3
LLLT CALL WAIT_FOR_VBLANK
DBU AR3,LLLT
LDI -1,R0
STI R0,@GALIL_STATUS_X
STI R0,@GALIL_STATUS_Y
STI R0,@GALIL_STATUS_Z
LDI 57*9,AR3
MILOOP1
PUSH AR3
LDL MINITQX,AR2 ;query X
CALL SEND_CMD
CALL WAIT_ACK
LDL MINITQY,AR2 ;query Y
CALL SEND_CMD
CALL WAIT_ACK
LDL MINITQZ,AR2 ;query Z
CALL SEND_CMD
CALL WAIT_ACK
POP AR3
CALL WAIT_FOR_VBLANK
;insert checking for limit switches
;
;
LDI @GALIL_STATUS_X,R0
TSTB 08h,R0
BNZ NO_UP_LIM_YET
LDI @GALIL_STATUS_Y,R0
TSTB 08h,R0
BNZ NO_UP_LIM_YET
LDI @GALIL_STATUS_Z,R0
TSTB 08h,R0
BZ GOT_ALL_UPPERS
;if all limit switches are activated, then exit
NO_UP_LIM_YET
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
AND @CME_MASK,R0
BZ NO_UP_SAFETY_ERROR
BU SAFETY_ERROR_CHK
;check for mats or optos
;start over if error
NO_UP_SAFETY_ERROR
DBU AR3,MILOOP1
BU UPPER_LIMIT_ERROR
;here we would error out
GOT_ALL_UPPERS
;check failsafes
;
;
CALL CHECK_MOTION_ERROR
BC INIT_MOTION_ERROROUT
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
AND @CME_MASK,R0
RS 8,R0
AND 0F0h,R0
CMPI 060h,R0
BGE INIT_MOTION_ERROROUT
;
;---> BR if FAILSAFES STILL ON
;
romdata
MINIT1L .string "SH;JG -13000,-13000,-13000;BG",13,0
.text
LDI 1,RC
TEXTITT "LOWER LIMIT TEST",250,260
LDL MINIT1L,AR2 ;send it up
CALL SEND_CMD
CALL WAIT_ACK
LDI 10,AR3
LLLLT CALL WAIT_FOR_VBLANK
DBU AR3,LLLLT
;
;check lower limits
;
LDI 57*9,AR3
MILOOP1LOW
PUSH AR3
LDL MINITQX,AR2 ;query X
CALL SEND_CMD
CALL WAIT_ACK
LDL MINITQY,AR2 ;query Y
CALL SEND_CMD
CALL WAIT_ACK
LDL MINITQZ,AR2 ;query Z
CALL SEND_CMD
CALL WAIT_ACK
POP AR3
CALL WAIT_FOR_VBLANK
;insert checking for limit switches
;
;
LDI @GALIL_STATUS_X,R0
TSTB 04h,R0
BNZ NO_LO_LIM_YET
LDI @GALIL_STATUS_Y,R0
TSTB 04h,R0
BNZ NO_LO_LIM_YET
LDI @GALIL_STATUS_Z,R0
TSTB 04h,R0
BZ GOT_ALL_LOWERS
;if all limit switches are activated, then exit
NO_LO_LIM_YET
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
AND @CME_MASK,R0
BZ NO_LO_SAFETY_ERROR
BU SAFETY_ERROR_CHK
;check for mats or optos
;start over if error
NO_LO_SAFETY_ERROR
DBU AR3,MILOOP1LOW
BU LOWER_LIMIT_ERROR
;here we would error out
GOT_ALL_LOWERS
;
;initialize the stuff
;
LDL PP,AR2
CALL SEND_CMD
CALL WAIT_ACK
CALL CHECK_MOTION_PRESENT
BNE INIT_MOTION_ERROROUT
LDI 57*3,AR3
LLLTA CALL WAIT_FOR_VBLANK
DBU AR3,LLLTA
LDL PP1,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP2,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP3,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP4,AR2
CALL SEND_CMD
CALL WAIT_ACK
;hang out for a moment
CALL WAIT_FOR_VBLANK
;
;now dl the program.
;
CALL CLEAR_LATCH_ERROR
CALL ABORT_RESET_GALIL
CALL MOTION_DLPROG
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
AND @CME_MASK,R0
BNZ INIT_MOTION_ERROROUT
LDI @LATCHED_ERROR,R0
BNZ INIT_MOTION_ERROROUT
;
;execute the program
;we are now guarenteed to be cool
;
FILL 1,250,500,350,0
LDI 1,RC
TEXTITT "MOTION SYSTEM OK",300,240
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*
*an intialization error has occurred:
* shutdown and dont use the galil
*
*
romdata
;MOTOROFF .string "ST;MO",13,0
;MOTOROFF .string "ST",13,0
;MOTOROFF .string "TL 0.6,0.6,1.8;JG 90000,90000,90000;BG;WT 4000;TL 0.5,0.5,1.6;WT 4000;TL 0.3,0.3,1.4;WT 4000;ST XYZ;MO",13,0
;
;MOTOROFF1 .string "TL 0.6,0.6,1.8;JG 90000,90000,90000;BG",13,0
;MOTOROFF2 .string "TL 0.5,0.5,1.6;WT 3000;TL 0.4,0.4,1.4;WT 3000;TL 0.3,0.3,1.2",13,0
;MOTOROFF3 .string "TL 0.2,0.2,1.0;WT 3000;TL 0.1,0.1,0.8;WT 3000;TL 0.0,0.0,0.6",13,0
;MOTOROFF4 .string "ST XYZ;MO",13,0
;MOTOROFF4 .string 0
;
MOTOROFF0 .string "JG 90000,90000,90000;BG;TL 0.6,0.6,1.8",13,0
MOTOROFF1 .string "TL 0.55,0.55,1.7;WT 1000",13,0
MOTOROFF2 .string "TL 0.50,0.50,1.6;WT 1000",13,0
MOTOROFF3 .string "TL 0.45,0.45,1.5;WT 1000",13,0
MOTOROFF4 .string "TL 0.40,0.40,1.4;WT 1000",13,0
MOTOROFF5 .string "TL 0.35,0.35,1.3;WT 1000",13,0
MOTOROFF6 .string "TL 0.30,0.30,1.2;WT 1000",13,0
MOTOROFF7 .string "TL 0.25,0.25,1.1;WT 1000",13,0
MOTOROFF8 .string "TL 0.20,0.20,1.0;WT 1000",13,0
MOTOROFF9 .string "TL 0.15,0.15,0.9;WT 1000",13,0
MOTOROFF10 .string "TL 0.10,0.10,0.8;WT 1000",13,0
MOTOROFF11 .string "TL 0.05,0.05,0.7;WT 1000",13,0
MOTOROFF12 .string "ST XYZ;MO",13,0
MOTOFF_PROG
.word MOTOROFF1
.word MOTOROFF2
.word MOTOROFF3
.word MOTOROFF4
.word MOTOROFF5
.word MOTOROFF6
.word MOTOROFF7
.word MOTOROFF8
.word MOTOROFF9
.word MOTOROFF10
.word MOTOROFF11
.word MOTOROFF12
.word 0
.text
.globl SEND_MOTOR_OFF
SEND_MOTOR_OFF:
CALL RESET_GALIL
CALL WAIT_ACK_REAL
;bong sound
LDI 984,AR2
CALL SENDSND
;2.5 sec delay
LDI 2500,R0
CALL WAITMSEC
LDL MOTOROFF12,AR2
CALL SEND_CMD
CALL WAIT_ACK
RETS
SEND_MOTOR_OFF_NO_RESET:
LDL MOTOROFF0,AR2
CALL SEND_CMD
CALL WAIT_ACK
FILL 1,200,500,390,0
LDI 500,R0 ;WAIT
CALL WAITMSEC
LDL MOTOFF_PROG,AR4
MOTOFF_LOOP
LDI 1,RC
TEXTITT "SLOWLY TURNING OFF MOTORS",1,200
LDI 984,AR2 ;BONG
CALL SENDSND
LDI 2000,R0 ;WAIT
CALL WAITMSEC
LDI *AR4++,AR2 ;SEND NEXT TORQUE LEVEL
CMPI 0,AR2
BEQ MOTOFF_DONE
CALL SEND_CMD
CALL WAIT_ACK
FILL 1,200,500,390,0 ;CLEAR MESSAGE
LDI 500,R0 ;WAIT
CALL WAITMSEC
LDI *AR4++,AR2 ;SEND NEXT TORQUE LEVEL
CMPI 0,AR2
BEQ MOTOFF_DONE
CALL SEND_CMD
CALL WAIT_ACK
BU MOTOFF_LOOP
MOTOFF_DONE
FILL 1,200,500,390,0
RETS
INIT_MOTION_ERROROUT:
CALL SEND_MOTOR_OFF_NO_RESET
CLRI R2
SETAUD ADJ_MOTION_PRESENT
;show message on screen
;
FILL 1,250,500,350,0
LDI 4,AR5
GDLPAA LDI 1,RC
TEXTITT "MOTION SYSTEM HAS ERRORS",100,250
TEXTITT "MOTION SYSTEM DEACTIVATED",100,260
LDI 984,AR2
CALL SENDSND
LDI 450,R0
CALL WAITMSEC
FILL 1,250,500,350,0
LDI 150,R0
CALL WAITMSEC
DBU AR5,GDLPAA
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
RS 12,R0 ;right justify
AND 0Fh,R0
CMPI 8,R0
BLT NNE1
LDI 1,RC
TEXTITT "BOARD NOT PLUGGED IN",100,250
BU FFFD
NNE1
LDI R0,AR2
ADDI @BABAID,AR2
LDI *AR2,AR2
LDI 1,RC
LDI 100,R2
LDI 250,R3
CALL _outtextxyc
FFFD
LDI 3000,R0
CALL WAITMSEC
FILL 1,250,500,350,0
RETS
BABAID .word JJG
romdata
JJG .word MM0,MM1,MM2,MM3,MM4,MM5,MM6,MM7
MM0 .string "UNKNOWN ERROR",0
MM1 .string "MAT NOT PLUGGED IN",0
MM2 .string "MAT STEPPED ON",0
MM3 .string "SAFETY BEAM PATH BROKEN",0
MM4 .string "SAFETY BEAM DETECTOR NOT RECEIVING",0
MM5 .string "SAFETY BEAM LIGHT NOT EMITTING",0
MM6 .string "FAIL SAFE SWITCH ENGAGED",0
MM7 .string "FAIL SAFE SWITCH NOT CONNECT PROPERLY",0
;maybe reported as 'error 48'
.text
*----------------------------------------------------------------------------
;if EVER failsafe problem MOTOR OFF
;now message on screen
;countdown 10 seconds
;if in 10 seconds error not cleared
;no motion for game
;move it up
;read status
;check for upper limit switches
;if not found in 3 seconds shut down NO MOTION
;check failsafes and all errors
;if failsafes still engaged msg "NO MOTION FOR THIS GAME"
;other safety error 10 seconds to clear if not clear NO MOTION FOR GAME
;dl the program
;has error been latched?
;F->continue
;T->dl program again if > 2x than NO MOTION
;xq the program
;if ST from galil (during gameplay) MOTION OFF until reset
; hit the ABORT LINE ???
; stop sending commands
;TS
*----------------------------------------------------------------------------
bbd .string "MOTION BURNIN",0
*----------------------------------------------------------------------------
*
*WAIT 500
*
WAIT500:
; RPTS 490
RPTS 100
NOP
PUSH R0
LDI @_MODE,R0
AND MMODE,R0
CMPI MDIAG,R0
BNE WT500X
CALL FEED_WATCHDOG ;FEED THE DOG WHEN IN DIAGNOSTICS
WT500X
POP R0
RETS
*----------------------------------------------------------------------------
*
*CHECK FOR MOTION DIPSWITCH
*RET NE IF MOTION DIPSWITCH OFF
*
CHECK_MOTION_DIP:
PUSH AR2
CLRI AR2
LDP @DIPSW
LDI @DIPSW,R0 ;READ DIPSWITCH
LDI *AR2,AR2
POP AR2
RS 16,R0
SETDP
TSTB DIP_MOTION,R0 ;MOTION CABINET?
RETS
*
*CHECK IF MOTION PRESENT
*RET NE IF MOTION NOT PRESENT
*
CHECK_MOTION_PRESENT:
READAUD ADJ_MOTION_PRESENT
CMPI 1,R0
RETS
DIAG_MOTION_TESTZ:
LDI 1,IR1
BU MOTION_BURNIN_J
DIAG_MOTION_TESTX:
LDI 2,IR1
BU MOTION_BURNIN_J
DIAG_MOTION_TESTY:
LDI 4,IR1
BU MOTION_BURNIN_J
DIAG_MOTION_BURNIN:
LDI 7,IR1
MOTION_BURNIN_J
CALL CHECK_MOTION_DIP
BNZ GALIL_ERR1 ;RETURN IF NON MOVING
CALL CHECK_MOTION_PRESENT
BNE GALIL_ERROR
; CALL INITIALIZE_GALIL
CALL INITIALIZATION_MOTION_CHECK
CALL CHECK_MOTION_PRESENT
BNE GALIL_ERROR
CALL CLRSCRN
CALL SETPAGE0
LDIL SCREEN0,R0 ;set active screen to 1 (writeable)
STPI R0,@ACTIVE_SCREEN
CLRI AR4 ;page index
LDI DEFAULT_COLOR,RC
TEXTIT bbd,50,330
CLRF R5
CLRF R6
CLRF R7
MBI_LP
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
RS 8,R0
LS 24,R0
CMPI 0,R0
BEQ JAJA
.globl MOTION_SAFETY_SWITCHES_DIAG
CALL MOTION_SAFETY_SWITCHES_DIAG
BU ENTER_DIAG
JAJA
TSTB 1,IR1
BZ JJ5
ADDFL 0.03,R5
CMPF TWOPI,R5
BLT JJ5
SUBF TWOPI,R5
JJ5
TSTB 2,IR1
BZ JJ6
ADDFL 0.05,R6
CMPF TWOPI,R6
BLT JJ6
SUBF TWOPI,R6
JJ6
TSTB 4,IR1
BZ JJ7
ADDFL 0.04,R7
CMPF TWOPI,R7
BLT JJ7
SUBF TWOPI,R7
JJ7
LDF R7,R2
PUSHFL R5
PUSHFL R6
PUSHFL R7
CALL _HPsin
LDF R0,R4
POPFL R7
POPFL R6
POPFL R5
LDF R6,R2
PUSHFL R4
PUSHFL R5
PUSHFL R6
PUSHFL R7
CALL _HPsin
LDF R0,R3
POPFL R7
POPFL R6
POPFL R5
POPFL R4
LDF R5,R2
PUSHFL R3
PUSHFL R4
PUSHFL R5
PUSHFL R6
PUSHFL R7
CALL _HPsin
LDF R0,R2
POPFL R7
POPFL R6
POPFL R5
POPFL R4
POPFL R3
PUSHFL R5
PUSHFL R6
PUSHFL R7
;R2 front
;R3 left
;R4 right
;
;-1>=x>=1
MPYF 0.8,R2
MPYF 0.8,R3
MPYF 0.8,R4
;-.8>=x>=.8
CALL MOTION_SCALE_ENTER
CALL WAIT_FOR_VBLANK
POPFL R7
POPFL R6
POPFL R5
LDI @_newbut,R0
LDI R0,R1
TSTB SW_DIAG,R1 ;move up
BNZ MBI_LP
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*PLAYER CABINET MOTION
*
*PARAMETERS
* AR4 PLAYER CAR OBJECT
* AR5 PLAYER CAR STRUCTURE
*
*CONTROL AXES:
* X=LFT REAR 0=GROUND, 50000=FULL HEIGHT
* Y=RT REAR 0=GROUND, 50000=FULL HEIGHT
* Z=FRONT 0=GROUND, 50000=FULL HEIGHT
*
PLMOTION:
CALL CHECK_MOTION_DIP
RETSNZ ;RETURN IF NON MOVING
LDI @MOTION_STOP_HIT,R0
RETSNZ
;
;check for an abort
;
LDI @ABORT_QUERY,R0
DEC R0
STI R0,@ABORT_QUERY
BGT NOCHECK
LDI 30,R0
STI R0,@ABORT_QUERY
LDL ABORTQ,AR2
CALL SEND_CMD
CALL WAIT_ACK
NOCHECK
.bss ABORT_QUERY,1
romdata
ABORTQ .string "#IBO;IBO=_SCX|_SCY|_SCZ;MG ",34,"V",34," IBO {$2.0}",13,0
.text
;
;MOTION SAFETY CODE
;
MOTION_ERROR_TIKS .set (57*5)
.globl WAITTIK
.bss WAITTIK,1
LDI @WAITTIK,R0 ;is counter in a previous 5 second wait?
BZ NO_PREVMOTION_ERRORS ;br -> NO, continue error checking
LDI 1,R1
STI R1,@MOTION_SAFETY_ON
CLRI AR2
LDP @991030h
LDI @991030h,R2
LDI *AR2,AR2
SETDP
LDL 0FF80h,R1
AND R1,R2
BZ NO_NEW_MOTION_ERRORS
LDI MOTION_ERROR_TIKS,R1
STI R1,@WAITTIK
RETS
NO_NEW_MOTION_ERRORS
SUBI @NFRAMES,R0
LDILT 0,R0
STI R0,@WAITTIK
CMPI 0,R0
RETSGT
;here is where we want to test to see if we are done with the error
;and how we are to recover
CALL ABORT_RESET_GALIL ;recover latch from mark
CLRI AR2
LDP @991030h ;IF we have a motion error after
LDI @991030h,R0 ;we attempted to reset, THEN
LDI *AR2,AR2
SETDP ;try again next frame
LDL 0FF80h,R1 ;
AND R1,R0 ;
RETSNZ ;
; CALL MOTION_DLPROG
RESETMOTIONANYWAY
.globl XQ,SEND_CMD,WAIT_ACK
LDL XQ,AR2 ;tell galil to continue executing program
CALL SEND_CMD
CALL WAIT_ACK
CLRI R0
STI R0,@MOTION_SAFETY_ON
; LDI 300,R0
; STI R0,@MOTION_RCV_TIKS
RETS
NO_PREVMOTION_ERRORS
; LDI @LATCHED_ERROR,R0
; BZ NONELAT
;
; CALL CLEAR_LATCH_ERROR
; CALL ABORT_RESET_GALIL ;recover latch from mark
; CALL MOTION_DLPROG
;
; BU RESETMOTIONANYWAY
;NONELAT
;IF galil is NOT responding
;attempt to reset serious
;
LDI @MOTION_RCV_TIKS,R0
BGT NOTTLRST
LDI 1,R2
SETAUD AUD_RESET_TOTALLY
LDI 0,R2
SETAUD ADJ_MOTION_PRESENT
RETS
;
; CALL RESET_GALIL
; CALL WAIT_ACK_REAL
;
;
; LDS "JG -26000,-2,AR2
; CALL SEND_CMD
; CALL WAIT_ACK
;
; CALL CHECK_MOTION_PRESENT
; BEQ KKGK
;
; LDI 1,R2
; SETAUD ADJ_MOTION_PRESENT
RETS
KKGK
LDL PP1,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP2,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP3,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDL PP4,AR2
CALL SEND_CMD
CALL WAIT_ACK
;
;dl the program.
;
CALL CLEAR_LATCH_ERROR
CALL MOTION_DLPROG
LDI 300,R0
STI R0,@MOTION_RCV_TIKS
NOTTLRST
.globl MOTION_SAFETY_ON
.bss MOTION_SAFETY_ON,1
.globl MOTION_SAFETY_TYPE
.bss MOTION_SAFETY_TYPE,1
CLRI AR2
LDP @991030h
LDI @991030h,R0
LDI *AR2,AR2
SETDP
LDL 0FF80h,R1
AND R1,R0
BZ NO_MOTION_ERRORS
;we do have an error
;
LDI MOTION_ERROR_TIKS,R1
STI R1,@WAITTIK
LDI 1,R1
STI R1,@MOTION_SAFETY_ON
LDI R0,R2
RS 12,R2
AND 0Fh,R2
LDI 0,R3 ;assume its a mat
CMPI 3,R2
LDIEQ 1,R3
CMPI 4,R2
LDIEQ 1,R3
CMPI 5,R2
LDIEQ 1,R3
CMPI 6,R2
LDIEQ 2,R3
CMPI 7,R2
LDIEQ 2,R3
STI R3,@MOTION_SAFETY_TYPE
LDI R0,R1
RS 16,R1
RETSC ;BOARD NOT PLUGGED IN OR PAL NOT INSTALLED -> DONT DO ANYTHING
TSTB 080h,R0 ;MOTION STOP IF HELD DOWN -> DONT DO ANYTHING
BZ NOTTHIS
LDI 3,R0
STI R0,@MOTION_SAFETY_TYPE
LDI 1,R0
STI R0,@MOTION_STOP_HIT
romdata
MSSM .string "MOTION STOP BUTTON HIT",0
.text
LDL MSSM,AR2
FLOAT 256,R2
FLOAT 310,R3
LDI 57*3,RC
CALL TEXT_ADDDS
ORM TXT_CENTER,*+AR0(TEXT_COLOR)
ORM TXT_CENTER,*+AR1(TEXT_COLOR)
RETS
NOTTHIS
KDS
NO_MOTION_ERRORS
LDI @MOTION_NOT_ON,R0
RETSNZ
CALL CHECK_MOTION_PRESENT
RETSNE
LDF *+AR5(RF_PY),R5
ADDF *+AR5(LF_PY),R5
ADDF *+AR5(LR_PY),R5
ADDF *+AR5(RR_PY),R5
MPYF 0.25,R5
*FIND AVERAGE Y POSITION = R5
LDF *+AR5(RF_PY),R2
ADDF *+AR5(LF_PY),R2
MPYF 0.5,R2
LDF *+AR5(LR_PY),R3
LDF *+AR5(RR_PY),R4
*GET Y HEIGHT DELTA TO CENTER
SUBRF R5,R2 ;Z (FRONT) AXIS DELTA
SUBRF R5,R3 ;X (LREAR) AXIS DELTA
SUBRF R5,R4 ;Y (RREAR) AXIS DELTA
FLOAT 150,R0 ;DIVIDE BY RADIUS FOR PROPORTIONALITY
CALL INV_F30
*COMPUTE EQUATION M=2X-X*X/150
LDF 2,R1
MPYF R1,R2,R5 ;2*Z
MPYF R2,R2
MPYF R0,R2
SUBF R2,R5,R2
MPYF R0,R2
MPYF R1,R3,R5 ;2*X
MPYF R3,R3
MPYF R0,R3
SUBF R3,R5,R3
MPYF R0,R3
MPYF R1,R4,R5 ;2*Y
MPYF R4,R4
MPYF R0,R4
SUBF R4,R5,R4
MPYF R0,R4
; MPYF R0,R2
; MPYF R0,R3
; MPYF R0,R4
;
CMPF 0.9,R2 ;RESTRICT RANGE TO -0.9->0.9
LDFGT 0.9,R2
CMPF -0.9,R2
LDFLT -0.9,R2
CMPF 0.9,R3 ;RESTRICT RANGE TO -0.9->0.9
LDFGT 0.9,R3
CMPF -0.9,R3
LDFLT -0.9,R3
CMPF 0.9,R4 ;RESTRICT RANGE TO -0.9->0.9
LDFGT 0.9,R4
CMPF -0.9,R4
LDFLT -0.9,R4
*GET LEAN ANGLES 20% OF MOTION (10% OF MOTION RESERVED)
*FRONT-BACK (XLEAN)
LDF *+AR5(CARXLEAN),R1 ;RADIAN LEAN -0.1->0.1
MPYF 6,R1
ADDF R1,R3
ADDF R1,R4
SUBF R1,R2
*LEFT-RIGHT (ZLEAN)
LDF *+AR5(CARZLEAN),R1 ;RADIAN LEAN -0.1->0.1
MPYF 6,R1
ADDF R1,R4
SUBF R1,R3
*LIMIT MOTION +-1.0
CMPF 1.0,R2
LDFGT 1.0,R2
CMPF 1.0,R3
LDFGT 1.0,R3
CMPF 1.0,R4
LDFGT 1.0,R4
CMPF -1.0,R2
LDFLT -1.0,R2
CMPF -1.0,R3
LDFLT -1.0,R3
CMPF -1.0,R4
LDFLT -1.0,R4
*SEND STUFF TO MOTION PLATFORM
*R2=Z FRONT -1.0->1.0
*R3=X LFT REAR -1.0->1.0
*R4=Y RT REAR -1.0->1.0
MOTION_SCALE_ENTER:
;NOW SCALE INTO THE RANGE USEABLE
;BY OR MOTION SYSTEM
;
FLOAT 17000,R1
MPYF R1,R2
MPYF R1,R3
MPYF R1,R4
FLOAT 26000,R1
ADDF R1,R2
ADDF R1,R3
ADDF R1,R4
FIX R2
FIX R3
FIX R4
JAJA555
.bss sPOTVALUE,15
.bss MOTION_CMDSTR,80
.bss LAST_ZF,1
.bss LAST_XL,1
.bss LAST_YR,1
PUSH R3
PUSH R4
; PUSHFL R3
; PUSHFL R4
LDI @LAST_ZF,R1
SUBI R2,R1
ABSI R1
CMPI MIN_MOVE_DIST,R1
BLT N76
STI R2,@LAST_ZF
LDL sPOTVALUE,AR2
CALL _itoa
LDL PZC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PZC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
N76
POP R2 ;RT REAR (Y)
LDI @LAST_YR,R1
SUBI R2,R1
ABSI R1
CMPI MIN_MOVE_DIST,R1
BLT N77
STI R0,@LAST_YR
LDL sPOTVALUE,AR2
CALL _itoa
LDL PYC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PYC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
N77
POP R2 ;LT REAR (X)
LDI @LAST_XL,R1
SUBI R2,R1
ABSI R1
CMPI MIN_MOVE_DIST,R1
BLT N78
STI R0,@LAST_XL
LDL sPOTVALUE,AR2
CALL _itoa
LDL PXC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PXC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
N78
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
LEVEL_THE_MOTION:
CALL CHECK_MOTION_DIP
RETSNZ ;RETURN IF NON MOVING
CALL CHECK_MOTION_PRESENT
RETSNE
; LDF -1.26,R2
; LDF -1.26,R3
; LDF -1.26,R4
; BU MOTION_SCALE_ENTER
LDI 0,R2
STI R2,@LAST_ZF
LDL sPOTVALUE,AR2
CALL _itoa
LDL PZC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PZC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
LDI 0,R2 ;RT REAR (Y)
LDL sPOTVALUE,AR2
CALL _itoa
LDL PYC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PYC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
CLRI R2 ;LT REAR (X)
LDL sPOTVALUE,AR2
CALL _itoa
LDL PXC1,AR0
LDL MOTION_CMDSTR,AR1
CALL STRCPY
LDL MOTION_CMDSTR,AR0
LDL sPOTVALUE,AR1
CALL STRCAT
LDL MOTION_CMDSTR,AR0
LDL PXC2,AR1
CALL STRCAT
LDI AR0,AR2
CALL SEND_CMD
CALL WAIT_ACK
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
SH .string "SH;JG 10000,10000,10000;BG",13,0
THEPROGL
.word EDIT,T5,T6,T7,T8,T9
.word T10,T11,T12,T13,T14,T15,T16,T17,T18,T19
.word T20,T21,T22,T23,T24,T25,T26,T27,T28,T29
.word T30,T31,T32,T33,T34,T35,T36,T37,T38,T39
.word T40,T41,T42,T43,T44,T45,T46,T47,T48,T49
.word T50,T51,T52,T53,T54,T55,T56,T57,T58,T59
.word T60
; .word T60,T1,T2,T3,T4,T61,T63,T64,T65,0
; .word T60,T1,T2,T3,T4,T61,T62,T63,T64,T65,0
.word T1,T2,T3
.word T66X,T67X,T68X
.word T66Y,T67Y,T68Y
.word T66Z,T67Z,T68Z
.word T69
.word T70,T71,T72,T73,T74,T75,T76,T77,T78,T79
.word T80,T81,T82,T83,T84
.word 0
;INITPROGL
; .word EDIT,T5,T6,T7,T8,T9
; .word T10,T11,T13,T14,T15,T16,T17,T18,T19
; .word T20,T21,T22,T23,T25,T26,T27,T28,T29
; .word T30,T31,T32,T33,T34,T35,T36,T37,T38,T39
; .word T40,T41,T42,T43,T44,T45,T46,T47,T48,T49
; .word T50,T51,T52,T53,T54,T55,T56,T57,T58,T59
; .word T60
;
; .word T1,T2,T3
; .word T66,T67,T68,T69
; .word T70,T71,T72,T73,T74,T75,T76,T77,T78,T79
; .word T80,T81,T82,T83,T84
; .word 0
;
*----------------------------------------------------------------------------
DOWNLOAD_PROGRAM:
IBO654
LDI *AR4++,AR2
CMPI 0,AR2
BEQ DNF
CALL SEND_CMD
CALL WAIT_ACK ;#
CALL WAIT_FOR_VBLANK
BU IBO654
DNF
LDL ENDED,AR2
CALL SEND_CMD
CALL WAIT_ACK ;:
LDL XQ,AR2
CALL SEND_CMD
CALL WAIT_ACK
;
;eric, is this correct? - ml
;
; LDL T10,AR2 ;;set actuators
; CALL SEND_CMD
; CALL WAIT_ACK
;
; LDL T11,AR2
; CALL SEND_CMD
; CALL WAIT_ACK
;
; LDL T12,AR2
; CALL SEND_CMD
; CALL WAIT_ACK
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*SEND COMMAND TO GALIL MOTION CONTROL BOARD
*
*
*FF09
*F709
*FF09
*
*FF00
*FB00
*FF00
*
*
*PARAMETERS
* AR2 PTR TO STR
*
*
SEND_CMD:
PUSH R0
PUSH R1
PUSH R2
PUSH R3
PUSH R4
PUSH R5
PUSH R6
PUSH R7
DINT
LDIL SOUND,AR3
LDI 0FF09h,R0
LDI 0F709h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R2
LDI 0FB00h,R3
CLRI R5 ;SHIFT COUNT
LLKK CMPI -32,R5
BNE REGLP
CLRI R5
NOP *AR2++
REGLP
LDI *AR2,R4
LSH R5,R4
SUBI 8,R5
AND 0FFh,R4
CMPI 0,R4
BZ KKKII
OR R4,R2,R6
OR R4,R3,R7
STI R6,*AR3
CALL WAIT500
STI R7,*AR3
CALL WAIT500
STI R6,*AR3
CALL WAIT500
BU LLKK
KKKII
EINT
POP R7
POP R6
POP R5
POP R4
POP R3
POP R2
POP R1
POP R0
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
RESET_GALIL:
DINT
LDIL SOUND,AR3
LDI 0FF0Ch,R0
LDI 0F70Ch,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R0
LDI 0FB00h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF0bh,R0
LDI 0F70bh,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R0
LDI 0FB00h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
EINT
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*
*clear the abort (restore communication to galil)
*
*
.globl ABORT_RESET_GALIL
ABORT_RESET_GALIL:
DINT
LDIL SOUND,AR3
;
;in essence we are clearing the abort, but NOT reseting galil
;
LDI 0FF0eh,R0 ; 0Ch = reset bit addr
LDI 0F70eh,R1 ; 0Eh = abort/reset bit addr
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R0
LDI 0FB00h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
EINT
LDL XQ,AR2 ;tell galil to continue executing program
CALL SEND_CMD
CALL WAIT_ACK
RETS
*----------------------------------------------------------------------------
.globl DIAG_MOTION_TESTX,DIAG_MOTION_TESTY,DIAG_MOTION_TESTZ
.globl DIAG_MOTION_BURNIN
*----------------------------------------------------------------------------
MOTION_DLPROG:
CALL CHECK_MOTION_DIP
RETSNZ ;RETURN IF NON MOVING
CALL CHECK_MOTION_PRESENT
RETSNE
LDL THEPROGL,AR4
CALL DOWNLOAD_PROGRAM
CLRI R0
STI R0,@LAST_ZF
STI R0,@LAST_XL
STI R0,@LAST_YR
CLRI R0
STI R0,@MOTION_NOT_ON
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*MOTION DIPSWITCH OFF
GALIL_ERR1
CALL SETPAGE0
LDIL SCREEN0,R0
STI R0,@ACTIVE_SCREEN
LDI 2,AR5
GDLP
LDI 0,RC
TEXTITT "WARNING MOTION DIP SWITCH OFF",150,130
LDI 250,R0
CALL WAITMSEC
LDI COL_WHITE,RC
TEXTITT "WARNING MOTION DIP SWITCH OFF",150,130
LDI 984,AR2
CALL SENDSND
LDI 250,R0
CALL WAITMSEC
DBU AR5,GDLP
RETS
*MOTION NO DETECTED
GALIL_ERROR:
; .globl MOTION_SAFETY_SWITCHES_DIAG
; CALL MOTION_SAFETY_SWITCHES_DIAG
; BU ENTER_DIAG
; CALL CLRSCRN
CALL SETPAGE0
LDIL SCREEN0,R0
STI R0,@ACTIVE_SCREEN
LDI 2,AR5
GLP
LDI 0,RC
TEXTITT "WARNING MOTION NOT DETECTED",150,130
LDI 250,R0
CALL WAITMSEC
LDI COL_WHITE,RC
TEXTITT "WARNING MOTION NOT DETECTED",150,130
LDI 984,AR2
CALL SENDSND
LDI 250,R0
CALL WAITMSEC
DBU AR5,GLP
RETS
*
*WAIT R0 X MSEC
*
WAITMSEC
MPYI 5000,R0
WTMLP
SUBI 1,R0
BGE WTMLP
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*
*
*RETURNS
* C IF ERROR
* NC IF NOT ERROR
*
*
CME_MASK .word 0FF80h
CHECK_MOTION_ERROR:
PUSH R0
PUSH AR0
CLRI AR0
LDP @991030h
LDI @991030h,R0
LDI *AR0,AR0
SETDP
AND @CME_MASK,R0
BZ CME_NO_MOTION_ERRORS
SETC
POP AR0
POP R0
RETS
CME_NO_MOTION_ERRORS
CLRC
POP AR0
POP R0
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
.globl CLEAR_LATCH_ERROR
CLEAR_LATCH_ERROR:
DINT
CLRI R0
STI R0,@LATCHED_ERROR
EINT
RETS
.bss LATCHED_ERROR,1
LATCH_ERROR:
CALL CHECK_MOTION_DIP
RETSNZ ;RETURN IF NON MOVING
CALL CHECK_MOTION_ERROR
RETSNC
LDI 1,R0
STI R0,@LATCHED_ERROR
RETS
*----------------------------------------------------------------------------
*
*CHECK GALIL READY
*RET CS IF GALIL READY W/ CHAR
*
G_READY:
LDIL SOUND,AR3
LDI 0FF0Ah,R0
LDI 0F70ah,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R0
LDI 0Fb00h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
LDI *AR3,R1 ;read status
STI R0,*AR3
CALL WAIT500
LSH 17,R1 ;PUT INTO CARRY
RETS
*
*GET GALIL CHARACTER
*RET R1= 8 BIT ASCII CODE B0-B7
*RET CS IF CHAR PRESENT
*RET CC IF NO CHAR PRESENT
*
G_CHAR:
CALL G_READY
RETSNC ;RETURN IF NOTHING
CALL G_READY
RETSNC ;RETURN IF NOTHING
LDIL SOUND,AR3
LDI 0FF08h,R0
LDI 0F708h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
STI R0,*AR3
CALL WAIT500
LDI 0FF00h,R0
LDI 0Fb00h,R1
STI R0,*AR3
CALL WAIT500
STI R1,*AR3
CALL WAIT500
LDI *AR3,R1
RS 8,R1
AND 0FFh,R1
STI R0,*AR3
CALL WAIT500
SETC
RETS
*
*GET GALIL STRING
*
G_STRING:
GS0
CALL G_CHAR ;CHARACTER READY?
BNC GSX ;NO...
CMPI 'G',R1 ;position error
BNE NOTPERR ;attempt motion recalibration
LDI 1,R2
SETAUD AUD_RESET_TOTALLY
LDI 0,R2
SETAUD ADJ_MOTION_PRESENT
BU GS0
NOTPERR
CMPI 'V',R1 ;STATUS ANYONE?
BNE NOTQSTATUS
CALL G_HEX
AND 6h,R1
CMPI 6h,R1
BZ GS0
LDI @WAITTIK,R1
CMPI 2,R1
LDILT 2,R1
STI R1,@WAITTIK
BU GS0
NOTQSTATUS
CMPI 'W',R1 ;IS IT AN ERROR??
BNE NOTQQERRORX ;
LDI @LLG6X,AR2
BU SHOWENCODER
NOTQQERRORX
CMPI 'S',R1 ;IS IT AN ERROR??
BNE NOTQQERRORY ;
LDI @LLG6Y,AR2
BU SHOWENCODER
NOTQQERRORY
CMPI 'Q',R1 ;IS IT AN ERROR??
BNE NOTQQERRORZ ;
LDI @LLG6Z,AR2
BU SHOWENCODER
.data
LLG6X .word L4ABX
LLG6Y .word L4ABY
LLG6Z .word L4ABZ
romdata
L4ABX .string "X ENCODER FAILURE",0
L4ABY .string "Y ENCODER FAILURE",0
L4ABZ .string "Z ENCODER FAILURE",0
.text
SHOWENCODER
FLOAT 256,R2
FLOAT 110,R3
LDI 60,RC
CALL TEXT_ADDDS
ORM TXT_CENTER,*+AR0(TEXT_COLOR)
ORM TXT_CENTER,*+AR1(TEXT_COLOR)
CALL SET40FONTDS
CLRI R2
SETAUD ADJ_MOTION_PRESENT
;
; galil program will take care of this - ml
;
; ;send shutdown commands
; LDL MOTOROFF,AR2
; CALL SEND_CMD
; RETS
RETS
NOTQQERRORZ
NOTQQERROR
CMPI 'X',R1
BNE GS1
CALL G_HEX
BNC GSX
STI R1,@GALIL_STATUS_X
B GS0
GS1
CMPI 'Y',R1
BNE GS2
CALL G_HEX
BNC GSX
STI R1,@GALIL_STATUS_Y
B GS0
GS2
CMPI 'Z',R1
BNE GS0
CALL G_HEX
BNC GSX
STI R1,@GALIL_STATUS_Z
B GS0
GSX
RETS
*----------------------------------------------------------------------------
.bss GALIL_STATUS_X,1
.bss GALIL_STATUS_Y,1
.bss GALIL_STATUS_Z,1
*----------------------------------------------------------------------------
*
*GET HEX STRING
*
G_HEX
CALL G_CHAR
BNC GHX ;NO...
CMPI '$',R1
BNZ GHX
CALL G_CHAR
BNC GHX ;NO...
CALL ASCIIHEX
BNC GHX
LDI R1,R2
LSH 4,R2
CALL G_CHAR
BNC GHX ;NO...
CALL ASCIIHEX
BNC GHX
ADDI R2,R1
SETC
RETS
GHX
CLRC
RETS
*
*CHANGE ASCIIHEX TO HEX 4 BITS
*R1=ASCII
*RET R1 =HEX
*CC IF INVALID
*CS IF VALID
*
ASCIIHEX:
CMPI 47H,R1
BGE ASCERR
SUBI 30H,R1
BN ASCERR
CMPI 9,R1
BLE AH1
CMPI 11H,R1
BLT ASCERR
SUBI 7,R1
AH1
SETC
RETS
ASCERR
CLRC
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
WAIT_ACK:
DINT
LDI 1000,R6 ;EXCESS CHARACTER TIMEOUT
WAIT_ACK_LOOP
CALL G_READY
BNC EXITL2 ;NOTHING THERE DUDES...
LDI 300,R0
STI R0,@MOTION_RCV_TIKS
CALL G_STRING
BNC EXITL2 ;NOTHING THERE DUDES...
DEC R6
BZ NO_RESPONSE ;TOO MANY CHARACTERS
BU WAIT_ACK_LOOP
EXITL2
EINT
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
NO_RESPONSE:
EINT
LDI 0,R2
SETAUD ADJ_MOTION_PRESENT
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
WAIT_ACK_REAL:
LDI 10000,R6 ;NO RESPONSE TIMEOUT
WAR0
CALL G_READY
BC WAR1
DEC R6
BZ NO_RESPONSE
B WAR0
WAR1
LDI 1000,R6 ;EXCESS CHARACTER TIMEOUT
WAR1L
CALL G_CHAR
BNC EXITL2A ;NO MORE CHARACTERS
DEC R6
BZ NO_RESPONSE ;TOO MANY CHARACTERS
BU WAR1L
EXITL2A
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*
*
*
UPPER_LIMIT_ERROR:
;
; CALL RESET_GALIL
; CALL WAIT_ACK_REAL
;
; ;send shutdown commands
; LDL MOTOROFF,AR2
; CALL SEND_CMD
;
CALL SEND_MOTOR_OFF_NO_RESET
CLRI R2
SETAUD ADJ_MOTION_PRESENT
LDI 5,AR5
ULLERR
FILL 1,200,500,390,0
LDI 50,R0
CALL WAITMSEC
LDI 1,RC
TEXTITT "UPPER LIMIT SWITCH ERROR",1,200
LDI @GALIL_STATUS_X,R0
TSTB 08h,R0
BZ ULL1
TEXTITT "X AXIS LIMIT SWITCH NOT FOUND",1,210
ULL1 LDI @GALIL_STATUS_Y,R0
TSTB 08h,R0
BZ ULL2
TEXTITT "Y AXIS LIMIT SWITCH NOT FOUND",1,220
ULL2 LDI @GALIL_STATUS_Z,R0
TSTB 08h,R0
BZ ULL3
TEXTITT "Z AXIS LIMIT SWITCH NOT FOUND",1,230
ULL3
LDI 450,R0
CALL WAITMSEC
DBU AR5,ULLERR
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
LOWER_LIMIT_ERROR:
;
; CALL RESET_GALIL
; CALL WAIT_ACK_REAL
;
; ;send shutdown commands
; LDL MOTOROFF,AR2
; CALL SEND_CMD
;
CALL SEND_MOTOR_OFF_NO_RESET
CLRI R2
SETAUD ADJ_MOTION_PRESENT
FILL 1,200,500,390,0
LDI 5,AR5
LLLERR
FILL 1,200,500,390,0
LDI 50,R0
CALL WAITMSEC
LDI 1,RC
TEXTITT "LOWER LIMIT SWITCH ERROR",1,200
LDI @GALIL_STATUS_X,R0
TSTB 04h,R0
BZ LLL1
TEXTITT "X AXIS LIMIT SWITCH NOT FOUND",1,210
LLL1 LDI @GALIL_STATUS_Y,R0
TSTB 04h,R0
BZ LLL2
TEXTITT "Y AXIS LIMIT SWITCH NOT FOUND",1,220
LLL2 LDI @GALIL_STATUS_Z,R0
TSTB 04h,R0
BZ LLL3
TEXTITT "Z AXIS LIMIT SWITCH NOT FOUND",1,230
LLL3
LDI 450,R0
CALL WAITMSEC
DBU AR5,LLLERR
RETS
*----------------------------------------------------------------------------
*----------------------------------------------------------------------------
*We must take into consideration:
* IF a limit switch is bogus, we can compensate by
* doing:
*
* "JG -26000,-26000,-26000;BG",13,"WT 3100",13,
* "SH",13
* "PR 500,500,500" ;just to relieve the pressure a bit...
* "DP 0,0,0",13
* "SP 15000,15000,15000",13,0
* etc... etc...
*
* NO NOT TRUE
*
*AC ACCERATION
*BG BEGIN MOTION
*DC DECELERATION
*DP DEFINE POSITION AS
*IT INDEPENDANT TIME CONSTANT (SMOOTHING FUNCTION)
*JG JOG (MOVE AT SPEED)
*JP JUMP
*MO MOTOR OFF
*PA POSITION ABSOLUTE
*PR POSITION RELATIVE
*SP SPEED
*WT WAIT
*XQ EXECUTE (LABEL)
*#<> LABEL
*EN END OF PROGRAM
*ED EDIT (OPEN BUFFER)
*^Q (17/CTRL-Q) (END OF BUFFER)
*
romdata
PPSAF .string "PR 4000,4000,4000;BG",13,"WT 500",13,"PR -12000,-12000,-12000",13,0
PP .string "JG -26000,-26000,-26000;BG",13,"WT 3100",13,"DP 0,0,0",13,"SP 15000,15000,15000",13,0
PP1 .string "AC 105000,105000,105000",13,"ER 8000,8000,8000",13,0
PP2 .string "DC 435000,435000,435000",13,"TL 7,7,7",13,0
PP3 .string "IT 0.1,0.1,0.1",13,"PA 0,0,0;BG",13,0
PP4 .string "JG 0,0,0;BG",13,0
EDIT .string "ED",13,0
T5 .string "#PROG",13,0
T6 .string "#XE",13,0
T7 .string "#XA",13,0
T8 .string "#YE",13,0
T9 .string "#YA",13,0
T10 .string "#ZE",13,0
T11 .string "#ZA",13,0
T12 .string "JG 0,0,0",13,0
T13 .string "BG XYZ",13,0
T14 .string "XA=0",13,0
T15 .string "YA=0",13,0
T16 .string "ZA=0",13,0
T17 .string "#LOOP",13,0
T18 .string "XE=XA-_RPX",13,0
T19 .string "XE=XE*5",13,0
T20 .string "YE=YA-_RPY",13,0
T21 .string "YE=YE*5",13,0
T22 .string "ZE=ZA-_RPZ",13,0
T23 .string "ZE=ZE*5",13,0
T24 .string "JG XE,YE,ZE",13,0
;T25 .string "JP #CHKX,(_TTX*_TTX)>21",13,0
T25 .string "JP #CHKX,(_TTX*_TTX)>23",13,0
;T26 .string "JP #CHKY,(_TTY*_TTY)>21",13,0
T26 .string "JP #CHKY,(_TTY*_TTY)>23",13,0
;T27 .string "JP #CHKZ,(_TTZ*_TTZ)>21",13,0
T27 .string "JP #CHKZ,(_TTZ*_TTZ)>26",13,0 ;LOFF DLTA November 9,1994
T28 .string "JP #LOOP",13,0
T29 .string "#INITP",13,0
T30 .string "#COUNT",13,0
T31 .string "#CHKX",13,0
T32 .string "ACC=0",13,0
T33 .string "INITP=_TPX",13,0
T34 .string "#MLPX",13,0
;T35 .string "JP #LOOP,(_TTX*_TTX)<7",13,0
T35 .string "JP #LOOP,(_TTX*_TTX)<10",13,0
T36 .string "JP #LOOP,(_TPX-INITP)>5",13,0
T37 .string "JP #LOOP,(_TPX-INITP)<-8",13,0
T38 .string "COUNT=COUNT+1",13,0
T39 .string "JP #ERROUTX,COUNT>5",13,0
T40 .string "JP #MLPX",13,0
T41 .string "#CHKY",13,0
T42 .string "COUNT=0",13,0
T43 .string "INITP=_TPY",13,0
T44 .string "#MLPY",13,0
;T45 .string "JP #LOOP,(_TTY*_TTY)<7",13,0
T45 .string "JP #LOOP,(_TTY*_TTY)<10",13,0
T46 .string "JP #LOOP,(_TPY-INITP)>5",13,0
T47 .string "JP #LOOP,(_TPY-INITP)<-8",13,0
T48 .string "COUNT=COUNT+1",13,0
T49 .string "JP #ERROUTY,COUNT>5",13,0
T50 .string "JP #MLPY",13,0
T51 .string "#CHKZ",13,0
T52 .string "COUNT=0",13,0
T53 .string "INITP=_TPZ",13,0
T54 .string "#MLPZ",13,0
;T55 .string "JP #LOOP,(_TTZ*_TTZ)<7",13,0 ;LOFF DLTA November 9,1994
T55 .string "JP #LOOP,(_TTZ*_TTZ)<17",13,0
T56 .string "JP #LOOP,(_TPZ-INITP)>5",13,0
T57 .string "JP #LOOP,(_TPZ-INITP)<-8",13,0
T58 .string "COUNT=COUNT+1",13,0
T59 .string "JP #ERROUTZ,COUNT>5",13,0
T60 .string "JP #MLPZ",13,0
T1 .string "#POSERR",13,0
T2 .string "MG ",34,"G",34,13,0
T3 .string "JP #MOTOFF",13,0
T66X .string "#ERROUTX",13,0
T67X .string "MG ",13,0
T68X .string "MG ",34,"W",34,13,0
T66Y .string "#ERROUTY",13,0
T67Y .string "MG ",13,0
T68Y .string "MG ",34,"S",34,13,0
T66Z .string "#ERROUTZ",13,0
T67Z .string "MG ",13,0
T68Z .string "MG ",34,"Q",34,13,0
T69 .string "#MOTOFF",13,0
T70 .string "JG 90000,90000,90000;BG;TL 0.6,0.6,1.8",13,0
T71 .string "WT 2000;TL 0.55,0.55,1.6",13,0
T72 .string "WT 2000;TL 0.50,0.50,1.5",13,0
T73 .string "WT 2000;TL 0.45,0.45,1.4",13,0
T74 .string "WT 2000;TL 0.40,0.40,1.3",13,0
T75 .string "WT 2000;TL 0.35,0.35,1.2",13,0
T76 .string "WT 2000;TL 0.30,0.30,1.1",13,0
T77 .string "WT 2000;TL 0.25,0.25,1.0",13,0
T78 .string "WT 2000;TL 0.20,0.20,0.9",13,0
T79 .string "WT 2000;TL 0.15,0.15,0.8",13,0
T80 .string "WT 2000;TL 0.10,0.10,0.7",13,0
T81 .string "WT 2000;TL 0.05,0.05,0.6",13,0
T82 .string "WT 2000",13,0
T83 .string "OE 1,1,1;AB",13,0
T84 .string "TL 7,7,7;EN",13,0
ENDED .string 17,0
XQ .string "XQ",13,0 ;"XQ #PROG",13,0
PXC1 .string "XA=",0
PXC2 .string 13,"BG XYZ",13,0
PYC1 .string "YA=",0
PYC2 .string 13,0
PZC1 .string "ZA=",0
PZC2 .string 13,0
.text
*----------------------------------------------------------------------------
.END
Reference in New Issue View Git Blame Copy Permalink