; motor functions

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

MOTOR_INIT:

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

MOTOR_STOP:

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

	ret

MOTOR_FWD:

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

	ret

MOTOR_BWD:

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

	ret

MOTOR_RIGHT:

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

	ret

MOTOR_LEFT:

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

	ret