#include <avr/io.h>
#include <avr/interrupt.h>

#define M1 0
#define M2 1
#define MOTOR_PORT PORTB
#define ENAB(i) ((i) ? PB2 : PB0)
#define DIR_A(i) ((i) ? PB3 : PB1)
#define DIR_B(i) ((i) ? PB5 : PB4)

unsigned char velocity[2];
unsigned char cycleCounter = 0;

int main(void)
{
	// Set USART, enable "databyte received" interrupt
	UBRRH=0;
	UBRRL=51;
	UCSRB=(1<<RXCIE)|(1<<RXEN)|(1<<TXEN);
	UCSRC=(1<<URSEL)|(1<<USBS)|(3<<UCSZ0);

	// Set IO
	DDRB=DDRC=0xFF;

	// Set motor direction
	MOTOR_PORT=(1<<DIR_A(M1)|(1<<DIR_A(M2));

	// Set initial velocity
	velocity[M1] = 0;
	velocity[M2] = 0;

	// Enable interrupt system	
	sei();

	// PWM generation loop
	while(1) 
	{
		// rotate from 0 to 255
		cycleCounter++;
		
		if(cycleCounter < velocity[M1])
			MOTOR_PORT |= (1<<ENAB(M1));
		else	
			MOTOR_PORT &= ~(1<<ENAB(M1));	

		if(cycleCounter < velocity[M2])
			MOTOR_PORT |= (1<<ENAB(M2));
		else	
			MOTOR_PORT &= ~(1<<ENAB(M2));			
	}

	return 0;
}

// Interrupt handler - databyte received
SIGNAL(SIG_UART_RECV)
{
	// Wait for receive completion
	while ( !(UCSRA & (1<<RXC)) );
	unsigned char data = UDR;
	
	// Set velocity
	velocity[M1] = ((data &0x07) << 5);
	velocity[M2] = ((data &0x70) << 1);
	
	// wipe previous configuration of driving direction
	MOTOR_PORT &= ~((1<<DIR_B(M1) | (1<<DIR_A(M1) |
		 (1<<DIR_B(M2)) | (1<<DIR_A(M2));
	
	// Set driving direction	
	MOTOR_PORT |= ((data & 0x08) > 0) ? 
		(1 << DIR_A(M1)) : (1 << DIR_B(M1));
	MOTOR_PORT |= ((data & 0x80) > 0) ? 
		(1 << DIR_A(M2)) : (1 << DIR_B(M2));	
}