USART_Init:
; Set baud rate
out UBRRH, r17
out UBRRL, r16
; Enable receiver and transmitter
ldi r16, (1<<RXEN)|(1<<TXEN)
out UCSRB,r16
; Set frame format: 8data, 2stop bit
ldi r16, (1<<URSEL)|(1<<USBS)|(3<<UCSZ0)
out UCSRC,r16
ret

USART_Transmit:
; Wait for empty transmit buffer
sbis UCSRA,UDRE
rjmp USART_Transmit
; Put data (r16) into buffer, sends the data
out UDR,r16
ret