; motor functions

.equ	MOT_1_1		= 0x04
.equ	MOT_1_2		= 0x02
.equ	MOT_2_1		= 0x07
.equ	MOT_2_2		= 0x03

MOTOR_INIT:

	; ports -> output
	in tmp1,DDRB
	sbr tmp1,(1<<MOT_1_1)|(1<<MOT_1_2)
	sbr tmp1,(1<<MOT_2_1)|(1<<MOT_2_2)
	out DDRB,tmp1

MOTOR_STOP:

	; output zero
	in tmp1,PORTB
	cbr tmp1,(1<<MOT_1_1)|(1<<MOT_1_2)
	cbr tmp1,(1<<MOT_2_1)|(1<<MOT_2_2)
	out PORTB,tmp1

	ret

MOTOR_FWD:

	in tmp1,PORTB
	sbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_1)
	cbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_2)
	out PORTB,tmp1

	ret

MOTOR_BWD:

	in tmp1,PORTB
	sbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_2)
	cbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_1)
	out PORTB,tmp1

	ret

MOTOR_RIGHT:

	in tmp1,PORTB
	sbr tmp1,(1<<MOT_2_2)|(1<<MOT_1_1)
	cbr tmp1,(1<<MOT_2_1)|(1<<MOT_1_2)
	out PORTB,tmp1

	ret

MOTOR_LEFT:

	in tmp1,PORTB
	sbr tmp1,(1<<MOT_1_2)|(1<<MOT_2_1)
	cbr tmp1,(1<<MOT_1_1)|(1<<MOT_2_2)
	out PORTB,tmp1

	ret