;multiplexer values
cs0		equ $B580
On_cnts_m	RMB 2
Off_cnts_m	RMB 2







MAIN		
		;jsr go_stop		;Set up On/Off times for stop
		;jsr TOC3_init		;init left motor
		;jsr TOC2_init		;init right motor
		jsr TOC4_init		;init mutiplex
		jsr TOF_init		;init Timer Overflow (for 0.5 sec Timer)
		jsr A2D			;init A/D PE0..PE3








;**************************************************************************
;Name:		Multiplex Drive Motor Functions
;Author:	Michael McNeill
;Date:		March, 25 2005
;Functions:	Each separtate function will do the indicated function 
;               to the robot drive motors
;
;		go_Mstop
;		go_Clockw
;		go_Counterw
;		
;		
;
;		
;**************************************************************************


go_Mstop	pshx
		ldx #Bstop
		bra robot_mulit

go_Clockw	pshx
		ldx #Clockw
		bra robot_mulit

go_Counterw	pshx
		ldx #Counterw
		bra robot_mulit




robot_mulit	ldd 0,x
		std On_cnts_B
		ldd 2,x
		std Off_cnts_B
		
		pulx
		rts


;Table for multiplex motors control Functions
;			On_cnts_m	Off_cnts_m
Clockw		fdb	2000,		18000		;Clockwise maybe
Counterw	fdb	4500,		14800		;Counterwise maybe
Mstop		fdb	3000,		17000
;Servo Motor notes:
;	1ms on time = 2000 counts = forward
;	2ms on time = 4000 counts = reverse
;	total counts for a 50hz square wave (20mSec - suggested for servos) is 40000 counts

;************************************************************************************************
marmup:
		;ldaa #$0000
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Clockw		;Raises the Arm		

		pula
		rts
;***************************************************************************************************
;**************************************************************************
marmDown:	psha

		LDAA #$0000
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Counterw		;Lowers the Arm
		

		pula
		rts
;**************************************************************************
;;**************************************************************************
mBinchClock:	psha

		LDAA #$0001
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Clockw		;Turns Bin Clockwise
	

		pula
		rts
;**************************************************************************
;;**************************************************************************
mBinchCounter:	psha

		LDAA #$0001
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Counterw		;Turns Bin Counter Clockwise
		

		pula
		rts
;****************************************************************************
8888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888
;;**************************************************************************
msbin1Clock:	psha

		LDAA #$0002
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Clockw		;Turns storage Bin1 Clockwise		

		pula
		rts
;**************************************************************************
;;**************************************************************************
mBin1Counter:	psha

		LDAA #$0002
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Counterw		;Turns storage Bin1 Counter Clockwise
		

		pula
		rts
;****************************************************************************
;;**************************************************************************
msbin2Clock:	psha

		LDAA #$0003
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Clockw		;Turns storage Bin2 Clockwise
		

		pula
		rts
;**************************************************************************
;;**************************************************************************
mBin2Counter:	psha

		LDAA #$0003
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Counterw		;Turns storage Bin2 Counter Clockwise
		

		pula
		rts
;****************************************************************************
;;**************************************************************************
msbin3Clock:	psha

		LDAA #$0004
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Clockw		;Turns storage Bin3 Clockwise
		

		pula
		rts
;**************************************************************************
;;**************************************************************************
mBin3Counter:	psha

		LDAA #$0004
		STAA cs0
		ldaa #1
		jsr pause

		jsr go_Counterw		;Turns storage Bin3 Counter Clockwise
		

		pula
		rts
;****************************************************************************