/*
 * Motors.c
 *
 * Created: 17.04.2021 8:23:52
 *  Author: vstrasil
 */ 
#include "Header/Motors.h"

void motors_init()
{
	PORTE.DIR = 0b00001111;            
	TCE0.PER = 0xFF;            
	TCE0.CTRLB |= TC_WGMODE_SS_gc;          
	TCE0.CTRLB |= TC0_CCBEN_bm|TC0_CCAEN_bm|TC0_CCCEN_bm|TC0_CCDEN_bm;
	TCE0.CTRLA = TC_CLKSEL_DIV256_gc;
	}
	
void motors_inter()
{
	while((TCE0_INTFLAGS & 0x01) == 0);
	TCE0_INTFLAGS = 0x00;
}

void motorR_forward(uint8_t aSPEED){
		if (aSPEED<=30)
		{
			aSPEED = 30;
		}

		if (aSPEED>=255)
		{
			aSPEED = 255;
		}
	
		TCE0_CCCBUF = 0;
		TCE0_CCDBUF = aSPEED;
		motors_inter();
}

void motorR_backward(uint8_t aSPEED){
	if (aSPEED<=30)
	{
		aSPEED = 30;
	}

	if (aSPEED>=255)
	{
		aSPEED = 255;
	}
	
	TCE0_CCCBUF = aSPEED;
	TCE0_CCDBUF = 0;
	motors_inter();
}

void motorR_break(){
	TCE0_CCCBUF = 255;
	TCE0_CCDBUF = 255;
	motors_inter();
}


void motorR_stop(){
	TCE0_CCCBUF = 0;
	TCE0_CCDBUF = 0;
	motors_inter();
}

void motorL_forward(uint8_t aSPEED){
	if (aSPEED<=30)
	{
		aSPEED = 30;
	}

	if (aSPEED>=255)
	{
		aSPEED = 255;
	}
	
	TCE0_CCABUF = 0;
	TCE0_CCBBUF = aSPEED;
	motors_inter();
}

void motorL_backward(uint8_t aSPEED){
	if (aSPEED<=30)
	{
		aSPEED = 30;
	}

	if (aSPEED>=255)
	{
		aSPEED = 255;
	}
	
	TCE0_CCABUF = aSPEED;
	TCE0_CCBBUF = 0;
	motors_inter();
}

void motorL_break(){
	TCE0_CCABUF = 255;
	TCE0_CCBBUF = 255;
	motors_inter();
}


void motorL_stop(){
	TCE0_CCABUF = 0;
	TCE0_CCBBUF = 0;
	motors_inter();
}