ControlMotors

*  ***********************
*  ****** All modes ******
*  ***********************
AllModes

WheelControl
	cmp.w	#MinZero,D2
	bge	TestCenterX
	jmp	TestCenterY

TestCenterX
	cmp.w	#MaxZero,D2
	bge	TestCenterY

CenterX
*
*  A	- Joystick is not moving left/right ... only up/down or
*	  at ZERO position.
*
	cmp.w	#MinZero,D3	* If Joystick is between max & min
	blt	CenterXCont	* Y positions (already between max &
	cmp.w	#MaxZero,D3	* min X positions) then set Joystick
	bgt	CenterXCont	* values to ZERO.

	move.w	#$0,D2
	move.w	#$0,D3	

	bset	#4,PORTQS	* 11 both FWD (FWD=1, REV=0)
	bset	#5,PORTQS

	jmp	WheelControlEnd

CenterXCont
	muls.w	#MaxMotor,D3
	divs.w	#$80,D3
	sub.w	#MaxMotor,D3
	
	cmp.w	#$0,D3
	blt	AReverse
	bset	#4,PORTQS	*  11
	bset	#5,PORTQS

	move.w	D3,D2
	jmp	A_end

AReverse
	neg	D3
	move.w	D3,D2
	bclr	#4,PORTQS	* 00
	bclr	#5,PORTQS
A_end
	jmp	WheelControlEnd

TestCenterY
	cmp.w	#MinZero,D3
	bge	TestCenterYTop
	jmp	XRight

TestCenterYTop
	cmp.w	#MaxZero,D3
	bge	XRight
*
*  B
*
	muls.w	#MaxMotor,D2
	divs.w	#$80,D2
	sub.w	#MaxMotor,D2
	
	cmp.w	#$0,D2
	blt	BLeft
	bclr	#4,PORTQS	* 10
	bset	#5,PORTQS

	move.w	D2,D3	
	jmp	B_end
BLeft
	neg	D2
	move.w	D2,D3
	bset	#4,PORTQS	* 01
	bclr	#5,PORTQS
B_end

	jmp	WheelControlEnd

XRight
	cmp.w	#MaxZero,D2
	ble	XLeft
*
*  C
*
	cmp.w	#MaxZero,D3
	blt	Quad4
Quad1
	move.w	D2,D5
	move.w	D3,D6
	sub.w	D2,D3
	muls.w	#MaxMotor,D3
	divs.w	#$80,D3

	cmp.w	#$0,D3
	blt	Quad1Less
	bset	#4,PORTQS
	jmp	XRight_Left

Quad1Less
	neg	D3
	bclr	#4,PORTQS
	jmp	XRight_Left

Quad4
	add.w	D6,D2
	muls.w	#MaxMotor,D2
	divs.w	#$80,D2
	sub.w	#MaxMotor,D2
	sub.w	#MaxMotor,D2
	move.w	D2,D3
	
	cmp.w	#$0,D3
	blt	Quad4Less
	bset	#4,PORTQS
	jmp	XRight_Left
Quad4Less
	neg	D3
	bclr	#4,PORTQS
	jmp	XRight_Left

XRight_Left
	move.w	D5,D2
	muls.w	#MaxMotor,D2
	divs.w	#$80,D2
	sub.w	#MaxMotor,D2
	
	cmp.w	#$0,D2
	blt	CLeft
	bset	#5,PORTQS
	jmp	C_end
CLeft
	neg	D2
	bclr	#5,PORTQS
C_end

	jmp	WheelControlEnd

XLeft
*
*  D
* 
	cmp.w	#MinZero,D3
	bgt	Quad2
Quad3
	move.w	D2,D5
	move.w	D3,D6
	sub.w	D2,D3
	muls.w	#MaxMotor,D3
	divs.w	#$80,D3

	cmp.w	#$0,D3
	blt	Quad3Less
	move.w	D3,D2
	bset	#5,PORTQS
	jmp	XLeft_Right

Quad3Less
	neg	D3
	move.w	D3,D2
	bclr	#5,PORTQS
	jmp	XLeft_Right

Quad2
	move.w	D5,D2
	add.w	D6,D2
	muls.w	#MaxMotor,D2
	divs.w	#$80,D2
	sub.w	#MaxMotor,D2
	sub.w	#MaxMotor,D2
	
	cmp.w	#$0,D2
	blt	Quad2Less
	bset	#5,PORTQS
	jmp	XLeft_Right
Quad2Less
	neg	D2
	bclr	#5,PORTQS
	jmp	XLeft_Right

XLeft_Right
	move.w	D6,D3
	muls.w	#MaxMotor,D3
	divs.w	#$80,D3
	sub.w	#MaxMotor,D3
	
	cmp.w	#$0,D3
	blt	DLeft
	bset	#4,PORTQS
	jmp	D_end
DLeft
	neg	D3
	bclr	#4,PORTQS
D_end

WheelControlEnd

*
* MOTOR #1 (LEFT)
*
Mot1
	mulu.w	#PERIOD_0,D2
	divu.w	#$ff,D2
	move.w	D2,(HIGH_TIME_0).L

	move.b	PORTQS,D0
	andi.w	#$20,D0
	beq	Mot1_Reverse
Mot1_Forward
	bset	#1,PORTQS
	jmp	Mot1_End
Mot1_Reverse
	bclr	#1,PORTQS
Mot1_End
	
* 
* MOTOR #2 (RIGHT)
*
Mot2
	mulu.w	#PERIOD_1,D3
	divu.w	#$ff,D3
	move.w	D3,(HIGH_TIME_1).L

	move.b	PORTQS,D0
	andi.w	#$10,D0
	beq	Mot2_Reverse
Mot2_Forward
	bset	#0,PORTQS
	jmp	Mot2_End
Mot2_Reverse
	bclr	#0,PORTQS
Mot2_End

ControlEnd
	rts				* End ( ControlMotors )