;A:\SENPROJ3.ASM
		.ORG $0000                   ;STACK STARTS AT
		.DL $10100                   ;LOCATION 10100
		.DL $400                     ;SRAM FROM 10000-13FFF
		.DL $400
		.EQU    PB,$200000           ;KEYPAD
		.EQU    LED,$400000          ;8 BIT LED BAR
		.EQU    DISPLAY,$100000      ;8 BIT DISPLAY BAR
		.EQU    SWITCH,$800000       ;8 BIT SWITCH
		.EQU    SEND,$30003          ;PORT TO SEND SERIAL DATA
		.EQU    STATUS,$30001        ;CHECK TXRDY
		.EQU    CLAW,$10200          ;START OF MEMORY FOR ROBOT
		.EQU    WRIST,$10210
		.EQU    ELBOW,$10220
		.EQU    SHOULDER,$10230
		.EQU    WAIST,$10240
		.EQU    I_CLAW,$10600
		.EQU    I_WRIST,$10610
		.EQU    I_ELBOW,$10620
		.EQU    I_SHOULDER,$10630
		.EQU    I_WAIST,$10640
		.EQU    STEP_MOTOR1,$80000   ;HARDWARE STEPPER MOTOR 
		.EQU    STEP_MOTOR2,$40000   ;LOCATION
		.EQU    STEP_MOTOR3,$1000000
		.EQU    MEM_MOT1,$10300      ;MEMORY OF STEPPER DATA
		.EQU    MEM_MOT2,$10310
		.EQU    MEM_MOT3,$10320
		.EQU    COUNT_MEM1,$10400    ;STEPPER MOTOR PHYSICAL
		.EQU    COUNT_MEM2,$10410    ;LOCATION
		.EQU    COUNT_MEM3,$10420
		.EQU    MEM_DIRECTION,$10500 ;KEEP IN TRACK FLAG
		.EQU    P1,$01               ;KEYPAD ENCODED HEX VALUE
		.EQU    P2,$02
		.EQU    P3,$03
		.EQU    P4,$04               ;P15 INITILIZE ARM   
		.EQU    P5,$05               ;P16 QUIT EXECUTION
		.EQU    P6,$06               ;SWITCH 3 OR BIT 2 QUIT MODE
		.EQU    P7,$07               ;BIT 0=0 LASER MODE
		.EQU    P8,$08               ;BIT 0=1 MANUAL MODE
		.EQU    P9,$09               ;BIT 1=1 SUPER MANUAL MODE
		.EQU    P10,$10
		.EQU    P11,$20              ;FOR MEM DIRECTION FLAG
		.EQU    P12,$30              ;BIT 0 IF MOTOR3 GOES FWD/BCK
		.EQU    P13,$40              ;BIT 1 EN/DIS INITILIZE MOTOR3
		.EQU    P14,$50              ;BIT 2 LABLE LASER/MANUAL MODE
		.EQU    P15,$60              ;BIT 3 LABLE MAN/SUPR MAN MODE
		.EQU    P16,$70
		
		.ORG $400
		MOVE.B #$13,$30000           ;INITILZE DUART REGISTERS
		MOVE.B #$07,$30000
		MOVE.B #$BB,$30001
		MOVE.B #$05,$30002
		MOVE.B #$AA,$10200           ;INITILIZE MEMORY OF
		MOVE.B #$7D,$10210           ;ROBOT POSITION
		MOVE.B #$E1,$10220
		MOVE.B #$19,$10230
		MOVE.B #$64,$10240
		MOVE.B #$AA,$10600           ;INITILIZE MEMORY OF
		MOVE.B #$7D,$10610           ;ROBOT INITILIZE POSITION
		MOVE.B #$E1,$10620
		MOVE.B #$19,$10630
		MOVE.B #$64,$10640
		MOVE.B #$33,$10300           ;INITILIZE MEMORY STEPPER 
		MOVE.B #$33,$10310           ;MOTOR
		MOVE.B #$33,$10320
		MOVE.B #$00,$10400           ;CLEAR MEMORY OF STEPPER
		MOVE.B #$00,$10410           ;MOTOR PHYSICA LOCATION
		MOVE.B #$00,$10420
		MOVE.L #$7000,D7
WAIT            SUB.L #$0001,D7
		BNE WAIT
		MOVE.B #$00,D1               ;INITILEZE ROBOT ARM 
		MOVE.B CLAW,D0               ;POSITION
		BSR SEND_DATA
		MOVE.B #$01,D1
		MOVE.B WRIST,D0
		BSR SEND_DATA
		MOVE.B #$02,D1
		MOVE.B ELBOW,D0
		BSR SEND_DATA
		MOVE.B #$03,D1
		MOVE.B SHOULDER,D0
		BSR SEND_DATA
		MOVE.B #$04,D1
		MOVE.B WAIST,D0
		BSR SEND_DATA

;******************************CHECK WHAT MODE************************************************

CHECK_MODE      MOVE.B #$55,LED             ;00 = LASER MODE
CHECK_MODE1     MOVE.B SWITCH,D6            ;01 = MANUAL MODE
		AND.B #$03,D6               ;02 = SUPER MANUAL MODE
		CMP.B #$00,D6
		BEQ LASER_MODE
		CMP.B #$01,D6
		BEQ MANUAL_MODE
		CMP.B #$02,D6
		BEQ SUPER_MAN_MODE
		JMP CHECK_MODE1

;***********************CHECK KEY PAD FOR EXECUTION********************************

LASER_MODE      MOVE.B #$01,LED
		MOVE.B MEM_DIRECTION,D5      ;WHEN BIT 1=1
		BSET.B #$01,D5               ;ENABLE INITILIZE MOTOR3
		BSET.B #$02,D5               ;LABEL IN LASER MODE
		MOVE.B D5,MEM_DIRECTION
CHECK           CMP.B #P15,PB
		BNE BLA2
		BSR INITILIZE_ARM
BLA2            CMP.B #P1,PB                 ;CONTINOUS KEY PAD CHECKING
		BEQ INITILIZE
		CMP.B #P2,PB
		BEQ START
		MOVE.B SWITCH,D6
		BTST.B #$02,D6
		BNE CHECK_MODE
		JMP CHECK


;*************************INITILIZE ALL STEPPER MOTORS*********************************

INITILIZE       MOVE.B #$03,LED              ;INITILIZE ALL STEPPER
		MOVE.B MEM_MOT1,D0           ;MOTOR POSITION
		MOVE.B MEM_MOT2,D1
		MOVE.B MEM_MOT3,D2
INITILIZE1      BSR DELAYMOT1
		CMP.B #P16,PB                ;WHEN EVER CHECK FOR P16
		BEQ LASER_MODE               ;INTERUPT EXECUTION AND
		MOVE.B SWITCH,D6             ;GOTO CHECK KEYPAD
		BTST.B #$07,D6               ;TEST FOR END OF POSITION
		BNE SKIP1                    ;FOR STEPPER MOTOR1
		ROR.B #$01,D0
		MOVE.B D0,STEP_MOTOR1
		MOVE.B #$05,DISPLAY
SKIP1           MOVE.B SWITCH,D6
		BTST.B #$06,D6               ;TEST FOR END OF POSITION
		BNE SKIP2                    ;FOR STEPPER MOTOR2
		ROR.B #$01,D1
		MOVE.B D1,STEP_MOTOR2
		MOVE.B #$06,DISPLAY
SKIP2           MOVE.B SWITCH,D6             ;TEST FOR END OF POSITION
		BTST.B #$05,D6               ;FOR STEPPER MOTOR3
		BNE SKIP3
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$01,D5               ;TEST FOR INITILIZE OR NOT
		BEQ NOT_INITILIZED           ;BIT 1=O DON'T INITILIZE
		ROR.B #$01,D2                ;BECAUSE IN PROCESS OF SCANNING
		MOVE.B D2,STEP_MOTOR3
		MOVE.B #$07,DISPLAY
SKIP3           MOVE.B SWITCH,D6
		AND.B #$E0,D6
		CMP.B #$E0,D6                ;TEST IF ALL POSITION ARE 
		BNE INITILIZE1               ;INITILIZE
		JMP HERE
NOT_INITILIZED  MOVE.B SWITCH,D6
		AND.B #$C0,D6                ;IF SCANNING, TEST IF 
		CMP.B #$C0,D6                ;MOTOR1 AND 2 ARE INITIALIZED
		BNE INITILIZE1
		JMP HERE1
HERE            MOVE.B D0,MEM_MOT1           ;END OF INITILIZING
		MOVE.B D1,MEM_MOT2           ;PUTING BACK TO ALL MEMORY
		MOVE.B D2,MEM_MOT3           ;LOCATION
		MOVE.L #$00,COUNT_MEM1
		MOVE.L #$00,COUNT_MEM3
		JMP LASER_MODE
HERE1           MOVE.B D0,MEM_MOT1
		MOVE.B D1,MEM_MOT2
		MOVE.L #$00,COUNT_MEM1
		JMP DROP_OFF

;***********************START SCANNING OF OBJECT***************************************

START           CMP.B #P5,PB                  ;FIRST LASER DETECTION
		BEQ FOLLOW                
		MOVE.B #$07,DISPLAY
DO_AGAIN1       MOVE.L COUNT_MEM3,D0
		MOVE.B MEM_MOT3,D1
DOO_AGAIN6      MOVE.W #$3FF,D4
		MOVE.B #$00,D2
		MOVE.B #$07,LED
DOO_AGAIN       CMP.B #P16,PB
		BEQ LASER_MODE
		ROL.B #$01,D1
		MOVE.B D1,STEP_MOTOR3
		ADD.L #$01,D0
		BSR DELAYMOT1
		SUB.W #$01,D4                ;MAKE SURE NOT TOUCHING
		BNE WHAT
		MOVE.B #$0F,D2
WHAT            CMP.B #$0F,D2
		BNE MAKE_SURE1
		MOVE.B SWITCH,D6             ;TEST FOR END OF POSITION
		BTST.B #$05,D6               ;IF YES GO BACKWARD TO SCAN
		BNE GO_BACK
MAKE_SURE1      CMP.B #P5,PB                 ;TEST FOR OBJECT DETECTION
		BNE DOO_AGAIN
		MOVE.B D1,MEM_MOT3
		MOVE.L D0,COUNT_MEM3
		MOVE.B MEM_DIRECTION,D5      ;BIT 0=0 FOWARD
		BCLR.B #$00,D5
		MOVE.B D5,MEM_DIRECTION
		JMP FOLLOW

GO_BACK         MOVE.W #$3FF,D4
		MOVE.B #$00,D2
		MOVE.B #$0F,LED
GO_BACK1        CMP.B #P16,PB
		BEQ LASER_MODE
		ROR.B #$01,D1
		MOVE.B D1,STEP_MOTOR3                
		SUB.L #$01,D0
		BSR DELAYMOT1
		SUB.W #$01,D4
		BNE WHAT1
		MOVE.B #$0F,D2
WHAT1           CMP.B #$0F,D2
		BNE MAKESURE3
		MOVE.B SWITCH,D6
		BTST.B #$05,D6                ;IF END OF POSITIION 
		BNE DOO_AGAIN6                ;GO TO DO AGAIN
MAKESURE3       CMP.B #P5,PB                 ;TEST FOR OBJECT DETECTION   
		BNE GO_BACK1                    
		MOVE.B D1,MEM_MOT3
		MOVE.L D0,COUNT_MEM3
		MOVE.B MEM_DIRECTION,D5      ;BIT 0=1 BACKWARDS
		BSET.B #$00,D5
		MOVE.B D5,MEM_DIRECTION

;****************************FOLLOW FIRST DETECTION************************************                

FOLLOW          MOVE.B #$05,DISPLAY
		MOVE.L COUNT_MEM3,D0         ;MOTOR 1 FOLLOW FIRST DETECTION
		MOVE.L COUNT_MEM1,D1
		CMP.L D0,D1
		BEQ SCAN_OBJECT
		MOVE.B MEM_MOT1,D2
		MOVE.W #$3FF,D4
		MOVE.B #$00,D3
		MOVE.B #$1F,LED
DO_AGAIN2       CMP.B #P16,PB
		BEQ LASER_MODE
		MOVE.B D2,STEP_MOTOR1
		ADD.L #$01,D1
		ROL.B #$01,D2
		BSR DELAYMOT2
		SUB.W #$01,D4
		BNE WHAT2
		MOVE.B #$0F,D3
WHAT2           CMP.B #$0F,D3
		BNE MAKE_SURE4
		MOVE.B SWITCH,D6
		BTST.B #$07,D6
		BNE LASER_MODE
MAKE_SURE4      CMP.L D0,D1
		BNE DO_AGAIN2
		MOVE.B D2,MEM_MOT1
		MOVE.L D1,COUNT_MEM1
		
;******************************SCAN FOR OBJECT********************************************

SCAN_OBJECT     MOVE.B #$06,DISPLAY
		CMP.B #P5,PB
		BEQ PICK_UP
		MOVE.B MEM_MOT2,D0
		MOVE.W #$1FF,D4
		MOVE.B #$00,D2
		MOVE.B #$3F,LED
DO_AGAIN3       CMP.B #P16,PB
		BEQ LASER_MODE
		ROL.B #$01,D0
		MOVE.B D0,STEP_MOTOR2
		BSR DELAYMOT1
		SUB.W #$01,D4                ;MAKE SURE NOT TOUCHING
		BNE WHAT3
		MOVE.B #$0F,D2
WHAT3           CMP.B #$0F,D2
		BNE MAKESURE2
		MOVE.B SWITCH,D6
		BTST.B #$06,D6               ;IF NO OBJECT
		BNE LASER_MODE               ;GOTO LASER MODE
MAKESURE2       CMP.B #P5,PB                 ;OBJECT DETECTION
		BNE DO_AGAIN3
		MOVE.B D0,MEM_MOT2
		
;*********************WHEN OBJECT IS DETECTED PICKUP THE OBJECT*******************************

PICK_UP         MOVE.B ELBOW,D0
		MOVE.B #$02,D1
L3              CMP.B #P16,PB
		BEQ PUT_BACK1
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$49,D0
		BNE L3
		MOVE.B D0,ELBOW
		JMP NEXT1

PUT_BACK1       MOVE.B D0,ELBOW
		JMP CHECK_MODE

NEXT1           MOVE.B WAIST,D0
		MOVE.B #$04,D1
L1              CMP.B #P16,PB
		BEQ PUT_BACK2
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$BF,D0
		BNE L1
		MOVE.B D0,WAIST
		JMP NEXT2

PUT_BACK2       MOVE.B D0,WAIST
		JMP CHECK_MODE

NEXT2           MOVE.B WRIST,D0
		MOVE.B #$01,D1
LL1             CMP.B #P16,PB
		BEQ PUT_BACK3
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$00,D0
		BNE LL1
		MOVE.B D0,WRIST
		JMP NEXT3

PUT_BACK3       MOVE.B D0,WRIST
		JMP CHECK_MODE

NEXT3           MOVE.B SHOULDER,D0
		MOVE.B #$03,D1
L2              CMP.B #P16,PB
		BEQ PUT_BACK4
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$63,D0
		BNE L2
		MOVE.B D0,SHOULDER
		JMP NEXT4

PUT_BACK4       MOVE.B D0,SHOULDER
		JMP CHECK_MODE


NEXT4           MOVE.B CLAW,D0
		MOVE.B #$00,D1
L4              CMP.B #P16,PB
		BEQ PUT_BACK5
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$64,D0
		BNE L4
		MOVE.B D0,CLAW
		JMP NEXT5

PUT_BACK5       MOVE.B D0,CLAW
		JMP CHECK_MODE

NEXT5           MOVE.B WRIST,D0
		MOVE.B #$01,D1
L5              CMP.B #P16,PB
		BEQ PUT_BACK6
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$B6,D0
		BNE L5
		MOVE.B D0,WRIST
		JMP NEXT6

PUT_BACK6       MOVE.B D0,WRIST
		JMP CHECK_MODE

NEXT6           MOVE.L #$2FFFF,D7
T2              SUB.L #$01,D7
		BNE T2

		
		MOVE.B CLAW,D0
		MOVE.B #$00,D1
N1              CMP.B #P16,PB
		BEQ PUT_BACK7
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$AA,D0
		BNE N1
		MOVE.B D0,CLAW
		JMP NEXT7

PUT_BACK7       MOVE.B D0,CLAW
		JMP CHECK_MODE

NEXT7           MOVE.B WRIST,D0
		MOVE.B #$01,D1
N2              CMP.B #P16,PB
		BEQ PUT_BACK8
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$7D,D0
		BNE N2
		MOVE.B D0,WRIST
		JMP NEXT8

PUT_BACK8       MOVE.B D0,WRIST
		JMP CHECK_MODE

NEXT8           MOVE.B WAIST,D0
		MOVE.B #$04,D1
N5              CMP.B #P16,PB
		BEQ PUT_BACK9
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$64,D0
		BNE N5
		MOVE.B D0,WAIST
		JMP NEXT9

PUT_BACK9       MOVE.B D0,WAIST
		JMP CHECK_MODE


NEXT9           MOVE.B ELBOW,D0
		MOVE.B #$02,D1
N3              CMP.B #P16,PB
		BEQ PUT_BACK10
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$E1,D0
		BNE N3
		MOVE.B D0,ELBOW
		JMP NEXT10

PUT_BACK10      MOVE.B D0,ELBOW
		JMP CHECK_MODE

NEXT10          MOVE.B SHOULDER,D0
		MOVE.B #$03,D1
N4              CMP.B #P16,PB
		BEQ PUT_BACK11
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$19,D0
		BNE N4
		MOVE.B D0,SHOULDER
		JMP NEXT11

PUT_BACK11      MOVE.B D0,SHOULDER
		JMP CHECK_MODE

					     ;MEM DIRECTION
NEXT11          MOVE.B MEM_DIRECTION,D5      ;WHEN BIT 2=0 GOTO MANUAL
		BTST.B #$02,D5               ;MODE, ELSE LASER MODE
		BEQ MANUAL_MODE


		CMP.B #P16,PB
		BEQ LASER_MODE
		MOVE.B MEM_DIRECTION,D5      ;WHEN BIT 1=0 OF MEM_DIRECTION
		BCLR.B #$01,D5               ;DISABLE INITILIZE OF MOTOR3
		MOVE.B D5,MEM_DIRECTION
		JMP INITILIZE

;*******************WHEN OBJECT AT DISTINATION, DROP OFF***********************************

DROP_OFF        MOVE.B CLAW,D0
		MOVE.B #$00,D1
T1              CMP.B #P16,PB
		BEQ PUT_BACK20
		SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B #$64,D0
		BNE T1
		JMP NEXT20

PUT_BACK20      MOVE.B D0,CLAW
		JMP CHECK_MODE
		
NEXT20          MOVE.L #$2FFFF,D7
LT2             SUB.L #$01,D7
		BNE LT2

T3              CMP.B #P16,PB
		BEQ PUT_BACK21
		ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B #$AA,D0
		BNE T3
		MOVE.B D0,CLAW
		JMP NEXT21

PUT_BACK21      MOVE.B D0,CLAW
		JMP CHECK_MODE
		
NEXT21          MOVE.W COUNT_MEM3,D0
		MOVE.B MEM_MOT3,D1
					     ;MEM DIRECTION
		MOVE.B MEM_DIRECTION,D5      ;WHEN BIT 2=0 GOTO MANUAL
		BTST.B #$02,D5               ;MODE, ELSE LASER MODE
		BEQ MANUAL_MODE
					     ;BIT 0 OF MEM DIRECTION
		MOVE.B MEM_DIRECTION,D5      ;CHECK TO SEE IF
		BTST.B #$00,D5               ;GO BACK OR
		BNE GO_BACK                  ;DO AGAIN
		JMP DO_AGAIN1

;****************************NOW ENTERING MANUAL MODE********************************

MANUAL_MODE     CMP.B #P15,PB
		MOVE.B #$7F,LED
		MOVE.B MEM_DIRECTION,D5
		BCLR.B #$02,D5               ;CLR BIT 2 FOR MAN MODE PICKUP  
		BCLR.B #$03,D5               ;CLR BIT 3 FOR MAN MODE STEPPER
		MOVE.B D5,MEM_DIRECTION
MANUAL_REPEAT   CMP.B #P1,PB
		BEQ ROLEFT1
		CMP.B #P2,PB
		BEQ RORIGHT1
		CMP.B #P3,PB
		BEQ ROLEFT2
		CMP.B #P4,PB
		BEQ RORIGHT2
		CMP.B #P5,PB
		BEQ ROLEFT3
		CMP.B #P6,PB
		BEQ RORIGHT3
		CMP.B #P7,PB
		BEQ PICK_UP
		CMP.B #P8,PB
		BEQ DROP_OFF
		CMP.B #P15,PB 
		BNE BBLA
		BSR INITILIZE_ARM
BBLA            MOVE.B SWITCH,D6
		BTST.B #$02,D6
		BNE CHECK_MODE
		JMP MANUAL_REPEAT

;*************************ALL MANUAL MODES*****************************************

ROLEFT1         MOVE.B MEM_MOT1,D1
		MOVE.B D1,STEP_MOTOR1
		MOVE.B D1,LED
		MOVE.B #$05,DISPLAY
		ROL.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT1
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE

RORIGHT1        MOVE.B MEM_MOT1,D1
		MOVE.B D1,STEP_MOTOR1
		MOVE.B D1,LED
		MOVE.B #$05,DISPLAY
		ROR.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT1
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE

ROLEFT2         MOVE.B MEM_MOT2,D1
		MOVE.B D1,STEP_MOTOR2
		MOVE.B D1,LED
		MOVE.B #$06,DISPLAY
		ROL.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT2
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE

RORIGHT2        MOVE.B MEM_MOT2,D1
		MOVE.B D1,STEP_MOTOR2
		MOVE.B D1,LED
		MOVE.B #$06,DISPLAY
		ROR.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT2
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE

ROLEFT3         MOVE.B MEM_MOT3,D1
		MOVE.B D1,STEP_MOTOR3
		MOVE.B D1,LED
		MOVE.B #$07,DISPLAY
		ROL.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT3
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE

RORIGHT3        MOVE.B MEM_MOT3,D1
		MOVE.B D1,STEP_MOTOR3
		MOVE.B D1,LED
		MOVE.B #$07,DISPLAY
		ROR.B #$01,D1
		BSR DELAYMOT1
		MOVE.B D1,MEM_MOT3
		MOVE.B MEM_DIRECTION,D5
		BTST.B #$03,D5
		BNE SUPER_MAN_MODE
		JMP MANUAL_MODE


;****************************NOW ENTERING SUPER MANUAL MODE****************************


SUPER_MAN_MODE  MOVE.B MEM_DIRECTION,D5
		BSET.B #$03,D5
		MOVE.B D5,MEM_DIRECTION
		CMP.B #P1,PB            ;KEYPAD SCAN
		BEQ OPEN_CLAW 
		CMP.B #P2,PB
		BEQ CLOSE_CLAW
		CMP.B #P3,PB
		BEQ WRIST_UP
		CMP.B #P4,PB
		BEQ WRIST_DOWN
		CMP.B #P5,PB
		BEQ ELBOW_UP
		CMP.B #P6,PB
		BEQ ELBOW_DOWN
		CMP.B #P7,PB
		BEQ SHOULDER_FOWARD
		CMP.B #P8,PB
		BEQ SHOULDER_BACK
		CMP.B #P9,PB
		BEQ WAIST_RIGHT
		CMP.B #P10,PB
		BEQ WAIST_LEFT
		CMP.B #P11,PB
		BEQ ROLEFT1
		CMP.B #P12,PB
		BEQ RORIGHT1
		CMP.B #P13,PB
		BEQ ROLEFT2
		CMP.B #P14,PB
		BEQ RORIGHT2
		CMP.B #P15,PB
		BEQ ROLEFT3
		CMP.B #P16,PB
		BEQ RORIGHT3
		MOVE.B SWITCH,D6
		BTST.B #$02,D6
		BEQ BLA
		BSR INITILIZE_ARM
		JMP CHECK_MODE
BLA             JMP SUPER_MAN_MODE
	       
OPEN_CLAW       CMP.B #$64,CLAW
		BEQ SUPER_MAN_MODE
		MOVE.B #$00,D1
		MOVE.B CLAW,D0
		SUB.B #$01,D0
		MOVE.B D0,CLAW
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

CLOSE_CLAW      CMP.B #$AA,CLAW
		BEQ SUPER_MAN_MODE
		MOVE.B #$00,D1
		MOVE.B CLAW,D0
		ADD.B #$01,D0
		MOVE.B D0,CLAW
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

WRIST_UP        CMP.B #$FF,WRIST
		BEQ SUPER_MAN_MODE
		MOVE.B #$01,D1
		MOVE.B WRIST,D0
		ADD.B #$01,D0
		MOVE.B D0,WRIST
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

WRIST_DOWN      CMP.B #$00,WRIST
		BEQ SUPER_MAN_MODE
		MOVE.B #$01,D1
		MOVE.B WRIST,D0
		SUB.B #$01,D0
		MOVE.B D0,WRIST
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

ELBOW_UP        CMP.B #$1E,ELBOW
		BEQ SUPER_MAN_MODE
		MOVE.B #$02,D1
		MOVE.B ELBOW,D0
		SUB.B #$01,D0
		MOVE.B D0,ELBOW
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

ELBOW_DOWN      CMP.B #$E1,ELBOW
		BEQ SUPER_MAN_MODE
		MOVE.B #$02,D1
		MOVE.B ELBOW,D0
		ADD.B #$01,D0
		MOVE.B D0,ELBOW
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

SHOULDER_FOWARD CMP.B #$FF,SHOULDER
		BEQ SUPER_MAN_MODE
		MOVE.B #$03,D1
		MOVE.B SHOULDER,D0
		ADD.B #$01,D0
		MOVE.B D0,SHOULDER
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

SHOULDER_BACK   CMP.B #$19,SHOULDER
		BEQ SUPER_MAN_MODE
		MOVE.B #$03,D1
		MOVE.B SHOULDER,D0
		SUB.B #$01,D0
		MOVE.B D0,SHOULDER
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

WAIST_RIGHT     CMP.B #$00,WAIST
		BEQ SUPER_MAN_MODE
		MOVE.B #$04,D1
		MOVE.B WAIST,D0
		SUB.B #$01,D0
		MOVE.B D0,WAIST
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

WAIST_LEFT      CMP.B #$FF,WAIST
		BEQ SUPER_MAN_MODE
		MOVE.B #$04,D1
		MOVE.B WAIST,D0
		ADD.B #$01,D0
		MOVE.B D0,WAIST
		BSR SEND_DATA1
		JMP SUPER_MAN_MODE

;*****************************INITILIZE ROBOT ARM******************************
INITILIZE_ARM   MOVE.B #$04,D1
		MOVE.B I_WAIST,D2
		MOVE.B WAIST,D0
		BSR CHECK_IT
		MOVE.B D0,WAIST

		MOVE.B #$03,D1
		MOVE.B I_SHOULDER,D2
		MOVE.B SHOULDER,D0
		BSR CHECK_IT
		MOVE.B D0,SHOULDER


		MOVE.B #$00,D1
		MOVE.B I_CLAW,D2
		MOVE.B CLAW,D0
		BSR CHECK_IT
		MOVE.B D0,CLAW

		MOVE.B #$01,D1
		MOVE.B I_WRIST,D2
		MOVE.B WRIST,D0
		BSR CHECK_IT
		MOVE.B D0,WRIST

		MOVE.B #$02,D1
		MOVE.B I_ELBOW,D2
		MOVE.B ELBOW,D0
		BSR CHECK_IT
		MOVE.B D0,ELBOW

		RTS
		
;***************************ALL SUBROUTINES**********************************************

CHECK_IT        CMP.B D2,D0
		BLS PP
SUB_IT          SUB.B #$01,D0
		BSR SEND_DATA
		CMP.B D2,D0
		BNE SUB_IT
PP              CMP.B D2,D0
		BCC DD
ADD_IT          ADD.B #$01,D0
		BSR SEND_DATA
		CMP.B D2,D0
		BNE ADD_IT
DD              NOP
		RTS


SEND_DATA       BSR TXRDY                    ;SEND SERIAL DATA
		MOVE.B #$FF,SEND             ;TO ROBOT
		MOVE.B D1,DISPLAY
		BSR TXRDY
		MOVE.B D1,SEND
		MOVE.B D0,LED
		BSR TXRDY
		MOVE.B D0,SEND
		MOVE.W #$0500,D7             ;DELAY FOR ROBOT
DE2             SUB.W #$0001,D7
		BNE DE2
		NOP
		RTS
		
SEND_DATA1      BSR TXRDY                    ;SEND SERIAL DATA
		MOVE.B #$FF,SEND             ;TO ROBOT
		MOVE.B D1,DISPLAY
		BSR TXRDY
		MOVE.B D1,SEND
		MOVE.B D0,LED
		BSR TXRDY
		MOVE.B D0,SEND
		MOVE.W #$0F00,D7             ;DELAY FOR ROBOT
DEE3            SUB.W #$0001,D7
		BNE DEE3
		NOP
		RTS



TXRDY           MOVE.B STATUS,D7             ;CHECK FOR DUART
		AND.B #$04,D7                ;READY
		BEQ TXRDY
		RTS



DELAYMOT1       MOVE.W #$0600,D7             ;SLOW DELAY FOR STEPPER
DELM            SUB.W #$0001,D7              ;MOTOR
		BNE DELM
		RTS

DELAYMOT2       MOVE.W #$300,D7              ;FAST DELAY FOR STEPPER
DELM2           SUB.W #$0001,D7              ;MOTOR
		BNE DELM2
		RTS