#include <avr/io.h>
#include <util/delay.h>
#include "MotorDriver.h"


void Motor_Init()
{
	PORTB = 0x01;	//RST = 1
	_delay_ms(5);	//Wait
	
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Change configuration
	UDR0 = 0x02;
	while ( !(UCSR0A & (1<<UDRE0)) );		//2 Motors Motor0 and Motor1
	UDR0 = 0x00;	
	while ( !(UCSR0A & (1<<UDRE0)) );

	_delay_ms(5);	//Wait
	PORTB = 0x00;	//RST = 0
	_delay_ms(5);	//Wait
	PORTB = 0x01;	//RST = 1
	_delay_ms(5);	//Wait
}

void Motor_spin_left(int Rspeed, int Lspeed)
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Rspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x02;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Lspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_spin_right(int Rspeed, int Lspeed)
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x00;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Rspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Lspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_turn_left(int speed)
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = speed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x00;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_turn_right(int speed)
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x00;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = speed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_stop_left()
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x00;							//Stop
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_stop_right()
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x00;							//Stop
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_forward(int Rspeed, int Lspeed)	//Sends robot forward at speed
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Rspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Lspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );
}

void Motor_reverse(int Rspeed, int Lspeed)	//Sends robot in reverse at speed
{
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x00;							//Motor 0 = Right Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Rspeed;							//Move at speed
	while ( !(UCSR0A & (1<<UDRE0)) );

	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x02;							//Motor 1 = Left Motor
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = Lspeed;							//Move at speed
}

//MOTOR 1 Code:
/*
	_delay_ms(15);
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x01;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x7F;
	while ( !(UCSR0A & (1<<UDRE0)) );
*/

//MOTOR 2 Code:
/*


	_delay_ms(15);
	while ( !(UCSR0A & (1<<UDRE0)) );		//Start byte
	UDR0 = 0x80;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Device type
	UDR0 = 0x00;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor # and Direction
	UDR0 = 0x03;
	while ( !(UCSR0A & (1<<UDRE0)) );		//Motor Speed 0x00 to 0x7F (0 to 127)
	UDR0 = 0x3F;
	while ( !(UCSR0A & (1<<UDRE0)) );

*/