//#include <stdafx.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "funkce.h"
//#include "tracking.h"
HANDLE hFile;

int sig(int i)
{
	if (i == 0)
		return 0;
	if ( i < 0)
		return -1;
	else
		return +1;
}

const float K = 1 / (32768.0f - 256.0f);

int motor_out(int smer,int rychlost)
{
	char buffer[200];
	int V = rychlost;
	int U = smer;

	float Hf = smer     / 2.0f;
	float Vf = rychlost / 2.0f;

	float L = Vf + Hf;
	float R = Vf - Hf;

	bool sL = (L < 0);
	bool sR = (R < 0);

	if (sL)	L = -L;
	if (sR) R = -R;

	if (L > 255) L = 255;
	if (R > 255) R = 255;





	sprintf(buffer,"#4%c%.2X\r#5%c%.2X\r",
							sL ? '+' : '-',(unsigned char)(L * 0xA0),
							sR ? '-' : '+',(unsigned char)(R * 0xA0) );


   //      sprintf(buffer,"#4+%AA\r#5%-.55\r");



	DWORD temp;
	WriteFile(hFile,buffer,12,&temp,NULL);
	return 0;
}

void motor_init()
{
	hFile = CreateFile("COM2",GENERIC_READ | GENERIC_WRITE,0,NULL,OPEN_EXISTING,0,NULL);

//	if (hFile == INVALID_HANDLE_VALUE)
//		MessageBox("Error opening COM1");

	DCB dcb;

	ZeroMemory(&dcb,sizeof(DCB));
	GetCommState(hFile,&dcb);
	BuildCommDCB("57600,N,8,1",&dcb);
	SetCommState(hFile,&dcb);
}


 