; motor functions

;include "../include/m128def.inc"

.equ	MOT_DIR_L	= 0x02
.equ	MOT_DIR_R	= 0x03
.equ	MOT_PWR_L	= 0x04
.equ	MOT_PWR_R	= 0x07

;.def	tmp		= r17

MOTOR_INIT:

	; ports -> output
	in tmp,DDRB
	sbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
	sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out DDRB,tmp

MOTOR_STOP:

	; output zero
	in tmp,PORTB
	cbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
	cbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out PORTB,tmp

	ret

MOTOR_FWD:

	in tmp,PORTB
	sbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
	sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out PORTB,tmp

	ret

MOTOR_BWD:

	in tmp,PORTB
	cbr tmp,(1<<MOT_DIR_L)|(1<<MOT_DIR_R)
	sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out PORTB,tmp

	ret

MOTOR_RIGHT:

	in tmp,PORTB
	sbr tmp,(1<<MOT_DIR_L)
	cbr tmp,(1<<MOT_DIR_R)
	sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out PORTB,tmp

	ret

MOTOR_LEFT:

	in tmp,PORTB
	sbr tmp,(1<<MOT_DIR_R)
	cbr tmp,(1<<MOT_DIR_L)
	sbr tmp,(1<<MOT_PWR_L)|(1<<MOT_PWR_R)
	out PORTB,tmp

	ret