;Initilization
$ NOMOD51
$ INCLUDE (REG515.inc)

PUBLIC FORWARD, BACKWARD

EXTRN DATA(CONTSIG1TMP, CONTSIG2TMP,CONTSIG1, CONTSIG2, GO_MOVE_HIGH, GO_MOVE_LOW, MOVE_LOW_MEASURE, MOVE_HIGH_MEASURE, KEYPAD_FLAG)
EXTRN CODE(DLAYA, DLAYZ, DISPLAY_STATUS)


MOVE	SEGMENT CODE
RSEG 	MOVE
USING 	0


FORWARD:
	CLR		P4.0
	LCALL	DLAYA
	LCALL	DLAYA
	SETB	P4.0
	SETB	P4.1				;SET MOTOR 1 DIRECTION TO FORWARD
	SETB	P4.2                ;SET MOTOR 2 DIRECTION TO FORWARD
	MOV		A,#200				;TURN ON MOTOR 1 AND 2 IN HIGH SPEED MODE
	MOV		CONTSIG1,A
	MOV		CONTSIG2,A
	MOV		CONTSIG1TMP,A
	MOV		CONTSIG2TMP,A
	LJMP	BOTH_OFF_CHECK
BACKWARD:
	CLR		P4.0
	LCALL	DLAYA
	LCALL	DLAYA
	SETB	P4.0
	CLR		P4.1				;SET MOTOR 1 DIRECTION TO BACKWARD
	CLR		P4.2                ;SET MOTOR 2 DIRECTION TO BACKWARD
	MOV		A,#100				;TURN ON MOTOR 1 AND 2 IN HIGH SPEED MODE
	MOV		CONTSIG1,A
	MOV		CONTSIG2,A
	MOV		CONTSIG1TMP,A
	MOV		CONTSIG2TMP,A
	LJMP	BOTH_OFF_CHECK	

  BOTH_OFF_CHECK:				;CHECKS TO SEE IF BOTH MOTORS HAVE STOPPED AND EXITS FUNCTION IF BOTH ARE STOPPED
    LCALL	DISPLAY_STATUS
    MOV		A,KEYPAD_FLAG       ;CHECK FOR EMERGENCY STOP BUTTON
	CJNE	A,#0FH,BOC_CONTINUE2
	MOV		A,#0
	MOV		CONTSIG1,A
	MOV		CONTSIG2,A
	MOV		A,#0FFH
	MOV		KEYPAD_FLAG,A
	RET
  BOC_CONTINUE2:
    MOV		A,CONTSIG1TMP
  	JNZ     COMPARE_HIGH_DISTANCE
  	MOV		A,CONTSIG2TMP
  	JNZ		COMPARE_HIGH_DISTANCE
	CLR		P4.0				;CLEAR DISTANCE COUNT
  	RET
  COMPARE_HIGH_DISTANCE:
   	MOV		A,CONTSIG1TMP
 	MOV		CONTSIG1,A
 	MOV		A,CONTSIG2TMP
 	MOV		CONTSIG2,A
  	MOV		DPH,#77H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 2
    MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		B,A
 	MOV		DPH,#75H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 1
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	CLR		C
 	SUBB	A,B
 	JZ      COMPARE_LOW_DISTANCE
 	JNC 	SPEED_UP_MOT2
 	JC		SPEED_UP_MOT1
  COMPARE_LOW_DISTANCE:	
 	MOV		DPH,#76H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 1
    MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		B,A
 	MOV		DPH,#74H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 2
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	CLR		C
 	SUBB	A,B
 	JZ		BOC_CONTINUE
 	JNC 	SPEED_UP_MOT2
 	JC		SPEED_UP_MOT1
  SPEED_UP_MOT1:
    MOV		A,CONTSIG1
    MOV		CONTSIG1TMP,A
    ADD		A,#20
    MOV		CONTSIG1,A
    LJMP	BOC_CONTINUE
  	
  SPEED_UP_MOT2:
    MOV		A,CONTSIG2
    MOV		CONTSIG2TMP,A
    ADD		A,#20
    MOV		CONTSIG2,A
    LJMP	BOC_CONTINUE
  BOC_CONTINUE:
  	MOV		A,CONTSIG1TMP
  	JNZ     MOVE_CHECK1_MOTOR_1
  	MOV		A,CONTSIG2TMP
  	JNZ		MOVE_CHECK1_MOTOR_2
  	RET
  MOVE_CHECK1_MOTOR_1:	
	MOV		DPH,#75H	  		;GET CURRENT HIGH ORDER DISTANCE MOTOR 1
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		MOVE_HIGH_MEASURE,A
 	MOV		DPH,#74H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 1
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		MOVE_LOW_MEASURE,A
 	CLR		C
	MOV		A,GO_MOVE_LOW
	SUBB	A,MOVE_LOW_MEASURE
	JZ		TURN_OFF_MOTOR1
	JC		TURN_OFF2_MOTOR1
	SUBB	A,#10
	JC		MOTOR1_SLOW_SPEED
	SUBB	A,#40
	JC		MOTOR1_MEDIUM_SPEED
	LJMP	MOVE_CHECK1_MOTOR_2
  TURN_OFF_MOTOR1:
  	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		MOVE_CHECK1_MOTOR_2
	MOV		A,#0
	MOV		CONTSIG1,A
	MOV		CONTSIG1TMP,A
	LJMP    MOVE_CHECK1_MOTOR_2
  TURN_OFF2_MOTOR1:
    MOV		A,GO_MOVE_HIGH
  	CLR		C
  	SUBB	A,MOVE_HIGH_MEASURE
  	SUBB	A,#1
  	JNZ	    MOVE_CHECK1_MOTOR_2
  	MOV		A,GO_MOVE_LOW
  	ADD		A,#100
  	CLR		C
  	SUBB	A,MOVE_LOW_MEASURE
  	JZ		TURN_OFF_MOTOR1
  	JC      TURN_OFF_MOTOR1
  	SUBB	A,#10
	JC		MOTOR1_SLOW_SPEED_SKIP
	SUBB	A,#40
	JC		MOTOR1_MEDIUM_SPEED_SKIP
	LJMP	MOVE_CHECK1_MOTOR_2
  MOTOR1_SLOW_SPEED:
  	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		MOVE_CHECK1_MOTOR_2
  MOTOR1_SLOW_SPEED_SKIP:	
  	MOV		A,#30
  	MOV		CONTSIG1,A
  	MOV		CONTSIG1TMP,A
  	LJMP	MOVE_CHECK1_MOTOR_2
  MOTOR1_MEDIUM_SPEED:
  	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		MOVE_CHECK1_MOTOR_2
  MOTOR1_MEDIUM_SPEED_SKIP:	
  	MOV		A,#40
  	MOV		CONTSIG1,A
  	MOV		CONTSIG1TMP,A
  	LJMP	MOVE_CHECK1_MOTOR_2
  	;***************************************************************************  
  MOVE_CHECK1_MOTOR_2:	
	MOV		A,CONTSIG2TMP
	JZ		BOTH_OFF_CHECK_JMP
	MOV		DPH,#77H	  		;GET CURRENT HIGH ORDER DISTANCE MOTOR 1
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		MOVE_HIGH_MEASURE,A
 	MOV		DPH,#76H	  		;GET CURRENT LOW ORDER DISTANCE MOTOR 1
   	MOV		DPL,#00H
 	MOVX 	A,@DPTR
 	MOV		MOVE_LOW_MEASURE,A
 	CLR		C
	MOV		A,GO_MOVE_LOW
	SUBB	A,MOVE_LOW_MEASURE
	JZ		TURN_OFF_MOTOR2
	JC		TURN_OFF2_MOTOR2
	SUBB	A,#10
	JC		MOTOR2_SLOW_SPEED
	SUBB	A,#40
	JC		MOTOR2_MEDIUM_SPEED
	LJMP	BOTH_OFF_CHECK
  TURN_OFF_MOTOR2:
	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		BOTH_OFF_CHECK_JMP
	MOV		A,#0
	MOV		CONTSIG2,A
	MOV		CONTSIG2TMP,A
	LJMP    BOTH_OFF_CHECK
  TURN_OFF2_MOTOR2:
  	MOV		A,GO_MOVE_HIGH
  	CLR		C
  	SUBB	A,MOVE_HIGH_MEASURE
  	SUBB	A,#1
  	JNZ		BOTH_OFF_CHECK_JMP
  	MOV		A,GO_MOVE_LOW
  	ADD		A,#100
  	SUBB	A,MOVE_LOW_MEASURE
  	JZ		TURN_OFF_MOTOR2
  	JC      TURN_OFF_MOTOR2
  	SUBB	A,#10
	JC		MOTOR2_SLOW_SPEED_SKIP
	SUBB	A,#40
	JC		MOTOR2_MEDIUM_SPEED_SKIP
	LJMP	BOTH_OFF_CHECK_JMP
  MOTOR2_SLOW_SPEED:
  	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		BOTH_OFF_CHECK_JMP
  MOTOR2_SLOW_SPEED_SKIP:
  	MOV		A,#30
  	MOV		CONTSIG2,A
  	MOV		CONTSIG2TMP,A
  	LJMP	BOTH_OFF_CHECK
  MOTOR2_MEDIUM_SPEED:
  	CLR		C
  	MOV		A,GO_MOVE_HIGH
  	SUBB	A,MOVE_HIGH_MEASURE
	JNZ		BOTH_OFF_CHECK_JMP
  MOTOR2_MEDIUM_SPEED_SKIP:
  	MOV		A,#40
  	MOV		CONTSIG2,A
  	MOV		CONTSIG2TMP,A
  	LJMP	BOTH_OFF_CHECK
  BOTH_OFF_CHECK_JMP:
  	LJMP	BOTH_OFF_CHECK
END