コード例 #1
0
int main() {

	Serial *ardu = new Serial("COM4");
	Gamepad *hid = new Gamepad(1);
	unsigned char l_dir, r_dir, l_pow, r_pow;
	unsigned char ardu_msg[5] = {0x05, RELEASE, 0, RELEASE, 0};

	while (1) {
		hid->Update();
		if (get_motor_params(*hid, &l_dir, &r_dir, &l_pow, &r_pow) < 0) {
			break;
		}

		ardu_msg[1] = l_dir;
		ardu_msg[2] = l_pow;
		ardu_msg[3] = r_dir;
		ardu_msg[4] = r_pow;

		ardu->WriteData(ardu_msg, 5);

		cout << l_dir << '\t' << (int)l_pow << '\t';
		cout << r_dir << '\t' << (int)r_pow << endl;

		hid->RefreshState();
	}
	return 0;
}
コード例 #2
0
ファイル: 소스.cpp プロジェクト: CYHan/Algorithm
int main(int argc, char* argv[])
{
	Serial * Arduino = new Serial("COM8");
	cout << "Communicating with COM7 enter data to be sent\n";
	char data[256] = "";
	int nchar = 256;
	char incomingData[256] = "";
	while (Arduino->IsConnected())
	{
		cin >> data;
		Arduino->WriteData(data, nchar);
		Arduino->ReadData(incomingData, nchar);
		cout << incomingData << endl;
	}
	return 0;
}
コード例 #3
0
ファイル: Main.cpp プロジェクト: jobbywl/DirectXAmbi
int *LedAmountTest(char *Comm)
{
	Serial* SP = new Serial(Comm);

	if (SP->IsConnected())
		std::cout << "Connected with COM5" << std::endl;

	bool exit = false;
	//je kan hier op escape drukken om het programma af ste sluiten hier
	std::cout << "Waiting for Arduino. Press ESC to quit" << std::endl;
	char Rx_buffer[2] = "";

	while (Rx_buffer[0] != '0') //blijf hier hangen tot de arduino klaar is
	{
		Sleep(100);
		SP->ReadData(Rx_buffer, 2);
		if (GetAsyncKeyState(VK_ESCAPE))
		{
			exit = true;
		}
		if (exit == true)
		{
			std::cout << "Something went wrong with communication. Check baudrate settings and COM port" << std::endl;
		}
	}

	Rx_buffer[0] = '0';

	std::cout << "Got response from arduino sending amount of leds" << std::endl;

	//Stuur de hoeveelheid leds naar de arduino
	UINT8 Tx_buffer[600 * 3];

	ZeroMemory(Tx_buffer, 600 * 3);
	Tx_buffer[0] = 600 >> 8 & 0x00FF;
	Tx_buffer[1] = 600 & 0x00FF;

	SP->WriteData((char*)Tx_buffer, 2);


	std::cout << "Press up to add a LED and press down to remove one." << std::endl;
	std::cout << "When you are satisfied press SPACEBAR to confirm" << std::endl;
	std::cout << "Press ESC to quit" << std::endl;

	//onderstaande stukje code zal blijven draaien tot je op ESC drukt
	static int leds[4] = { 0 }, i = 0;
	int offset = 0;
	leds[0]++;
	while (i <4)
	{
		if (GetAsyncKeyState(VK_SPACE))						//Als escape is ingedrukt zet exit true
		{
			offset += leds[i];
			i++;
			Sleep(500);
		}
		if (GetAsyncKeyState(VK_UP))
		{
			leds[i]++;
			ZeroMemory(Tx_buffer, 600 * 3);
			for (int j = 0; j < leds[i]; j++)
			{
				for (int k = 0; k < 3; k++)
				{
					Tx_buffer[(j + offset) * 3 + k] = 0xff;
				}

			}

			SP->WriteData((char*)Tx_buffer, 1800);
			Sleep(50);
		}
		if (GetAsyncKeyState(VK_DOWN) && leds[i] > 1)
		{
			leds[i]--;
			ZeroMemory(Tx_buffer, 600 * 3);
			for (int j = 0; j < leds[i]; j++)
			{
				for (int k = 0; k < 3; k++)
				{
					Tx_buffer[(j + offset) * 3 + k] = 0xff;
				}

			}
			SP->WriteData((char*)Tx_buffer, 1800);
			Sleep(50);
		}

	}
	SP->~Serial();
	return leds;
}
コード例 #4
0
ファイル: Main.cpp プロジェクト: jobbywl/DirectXAmbi
int main()
{
	std::cout << "Aero will be disabled for performance reason!" << std::endl;
	DisableAeroTheme();

	bool exit = false;						//dit is voor later een escape variable
	int cap_method = D3D_CAP;
	float gamma = 0.6;

	GDICap Cap;
	Direct3DCap D3DCap;						//init directx9
	JackFFT JACK;


	std::ifstream myinfile;
	myinfile.open("./Config.txt");


	//Als het bestand is geopend bestaat die al dus sla je het maken over
	if (!myinfile.is_open())
	{
		myinfile.close();
		std::ofstream myfile;
		myfile.open("./Config.txt");

		CreateConfig(myfile, D3DCap);

		myfile.close();

		myinfile.open("./Config.txt");
	}

	std::string STRING;
	int Config[9] = { -1 };

	for (int i = 0; i < 9; i++)
	{
		if (myinfile.eof())
			break;
		std::getline(myinfile, STRING);
		Config[i] = atoi(STRING.c_str());
	}
	myinfile.close();

	//Check of config laden goed is gelukt
	int i = 0;
	while (Config[i] != -1 && i < 9)
	{
		i++;
	}
	if (i < 9)
	{
		std::cout << "Config Loading went wrong! Please delete Config.txt and run this software again!" << std::endl;
		return 0;
	}

	std::cout << "Using screen: " << Config[0] << " for capturing" << std::endl;
	D3DCap.init(Config[0]);
	Cap.init(Config[0]);							//

	UINT32 *pBits;
	switch (cap_method)
	{
	case GDI_CAP:
		pBits = Cap.pBits;
		break;
	case D3D_CAP:
		pBits = D3DCap.pBits;
		break;
	case JAC_CAP:
		pBits = JACK.pBits;
	}

	ScreenCalc Scherm(105,					//init de kleur bereken functies
		pBits,			//De PixelData
		D3DCap.return_hres(),	//De Hori Resolutie 
		D3DCap.return_vres(),	//De Verti Resolutie
		Config[1],					//Hoeveel procent die moet nemen aan de bovenkant/onderkant
		Config[2],					//Hoeveel procent die aan de zijkant moet nemen
		Config[3],				//Leds Boven
		Config[5],					//Leds Onder
		Config[4],					//Leds Links
		Config[6],				//Leds Rechts
		Config[7]);

	Scherm.Bereken_Grid();					//stel de hoeveelheid leds in die worden gebruikt en bereken Grid Grootte

	Scherm.set_Gamma(gamma);

	//Het programma moet eerst 0xff binnen krijgen als dat het geval is dan mag die beginnen met het oversturen
	//van de hoeveelheid leds
	//Als die hoeveelheden overeenkomen mag die beginnen met het zenden van led data
	std::string String;
	String = "\\\\.\\COM";
	String += std::to_string(Config[8]);
	char *temp = new char[String.size() + 1];
	std::copy(String.begin(), String.end(), temp);
	temp[String.size()] = '\0';
	Serial* SP = new Serial(temp);

	

	delete[] temp;
	temp = nullptr;

	if (SP->IsConnected())
		std::cout << "Connected with COM5" << std::endl;
	else
	{
		std::cout << "Communication Error. Exiting" << std::endl;
		return 0;
	}

	char Rx_buffer[100] = "";	//Dit is de Rx_buffer deze moet een char zijn

	//je kan hier op escape drukken om het programma af ste sluiten hier
	std::cout << "Waiting for Arduino. Press ESC to quit" << std::endl;

	SP->ReadData(Rx_buffer, 2);
	while (Rx_buffer[0] != '0') //blijf hier hangen tot de arduino klaar is
	{
		Sleep(100);
		SP->ReadData(Rx_buffer, 2);
		if (GetAsyncKeyState(VK_ESCAPE))
		{
			exit = true;
		}
		if (exit == true)
		{
			std::cout << "Something went wrong with communication. Check baudrate settings and COM port" << std::endl;
			return 0;	//beeindig de software
		}
	}

	Rx_buffer[0] = '0';

	std::cout << "Got response from arduino sending amount of leds" << std::endl;

	//Stuur de hoeveelheid leds naar de arduino
	UINT8 Tx_buffer[600 * 3];

	ZeroMemory(Tx_buffer, 600 * 3);
	Tx_buffer[0] = Scherm.geefLeds() >> 8 & 0x00FF;
	Tx_buffer[1] = Scherm.geefLeds() & 0x00FF;

	SP->WriteData((char*)Tx_buffer, 2);

	Sleep(1000);	//Wacht 1 seconde op de arduino

	std::cout << "Press END to quit capturing" << std::endl;

	UINT8 *pointer;						//Pointer voor de led bitstream
	clock_t klok1;
	clock_t klok2 = 50;
	pointer = Scherm.GeefLedPointer();	//Koppel de led bitstream aan de pointer
	// maak een thread die nu nog niks doet
	std::thread uart(send_data,SP,Rx_buffer,Scherm,pointer);

	while (exit == false)
	{
		//std::cout << "                     " << "\r";
		//std::cout << "FPS: " << ((1 * CLOCKS_PER_SEC) / (clock() - klok2)) << "\r";
		klok2 = clock();

		if (GetAsyncKeyState(VK_END))						//Als escape is ingedrukt zet exit true
		{
			exit = true;
		}
		else if (GetAsyncKeyState(VK_F8))
		{
			JACK.stop();
			pBits = Cap.pBits;
			Scherm.set_data(pBits);
			cap_method = GDI_CAP;
			std::cout << "Changed capture method to GDI " << std::endl;
			Sleep(100);

		}
		else if (GetAsyncKeyState(VK_F9))
		{
			JACK.stop();
			pBits = D3DCap.pBits;
			Scherm.set_data(pBits);
			cap_method = D3D_CAP;
			std::cout << "Changed capture method to D3D " << std::endl;
			Sleep(100);
		}
		else if (GetAsyncKeyState(VK_F7))
		{

			cap_method = JAC_CAP;
			JACK.init(pointer);
			std::cout << "Changed capture method to Jack Audio " << std::endl;
			JACK.start();
			Sleep(100);
		}
		else if (GetAsyncKeyState(VK_F12))
		{
			gamma += 0.01;
			Scherm.set_Gamma(gamma);
		}
		else if (GetAsyncKeyState(VK_F11))
		{
			gamma -= 0.01;
			Scherm.set_Gamma(gamma);
		}
		else if (GetAsyncKeyState(VK_F10))
		{
			std::cout << "Gamma : " << gamma << std::endl;
		}


		//start een thread die de data stuurt

//Scherm.Calc_Aspect_ratio();
		//Maak screenshot en sla die op
		switch (cap_method)
		{
		case GDI_CAP:
			Cap.capture();
			break;
		case D3D_CAP:
			D3DCap.capture();
			break;
		case JAC_CAP:
			Sleep(10);
			//when audio is selected everything will be handled in a different thread
			break;
		}
		if (cap_method != JAC_CAP)
		{
			Scherm.Bereken();
			Scherm.Calc_Aspect_ratio();
		}
		
		//wacht tot alle data verzonden is en we weer antwoord hebben gehad dat alles in orde is voordat we weer verder gaan
		
	}
	Thread_comm = 0;
	uart.join();
	return 0;
}
コード例 #5
0
ファイル: Main(teste-biel).cpp プロジェクト: Hefestos/cup2015
int main()
{



	FILE * xFile;
	xFile = fopen("motor1x.csv", "w");

	FILE * yFile;
	yFile = fopen("motor1y.csv", "w");

	FILE * thetaFile;
	thetaFile = fopen("motor1theta.csv", "w");

	FILE * refFile;
	refFile = fopen("ref.csv", "w");

	FILE * f1File;
	f1File = fopen("f1.csv", "w");

	FILE * f2File;
	f2File = fopen("f2.csv", "w");

	FILE * gFile;
	gFile = fopen("total.csv", "w");




	cv::Mat pesos;
	IplImage* pesos3;
	pesos3 = cvCreateImage(cvSize(1, 1), 8, 3);
	// Config Debug
	clock_t visionI, visionF, estraI, estraF, controlI, controlF, commI, commF, totalI, totalF, debugI, debugF, start;
	vector<robot> whishPath, followedPath;
	int p = 0;
	float counter = 0;
	char char_pressed;
	int time = 1;
	int side = 1;
	bool paused = true;
	CvRect ROI = cvRect(0, 0, 0, 0);
	robot canto_area1 = robot(-1, -1);
	robot canto_area2 = robot(-1, -1);
	robot canto_gol1 = robot(-1, -1);
	robot canto_gol2 = robot(-1, -1);
	int amin = 21;
	int amax = 84;
	int radius = 20;
	int min_distance = 54;
	int parametro[36] = { 94, 143, 80, 121, 253, 25,
		23, 134, 126, 52, 255, 255,
		33, 81, 154, 180, 255, 255,
		55, 189, 91, 94, 229, 255,
		180, 255, 255, 0, 0, 0,
		4, 156, 102, 14, 255, 255 };





























	//CvRect ROI = cvRect(36, 46, 266, 162);

	//robot canto_area1 = robot(65, 67);
	//robot canto_area2 = robot(244, 167);
	//robot canto_gol1 = robot(43, 66);
	//robot canto_gol2 = robot(282, 173);

	//int amin = 20;
	//int amax = 250;
	//int radius = 20;
	//int min_distance = 50;
	//int parametro[36] = { 88, 139, 80, 121, 253, 255,
	//	22, 59, 141, 55, 204, 255,
	//	117, 58, 113, 141, 113, 255,
	//	52, 82, 89, 97, 168, 169,
	//	0, 79, 123, 19, 149, 191,
	//	0, 129, 182, 18, 251, 255 };


	int debug_colors[7][3] = { { 128, 0, 128 },		// Roxo
	{ 0, 64, 0 },									// Verde
	{ 117, 58, 0 },									// Marrom
	{ 255, 255, 0 },								// Amarelo
	{ 255, 255, 0 },								// Amarelo
	{ 255, 255, 0 },								// Amarelo
	{ 255, 128, 0 } };								// Laranja

	if (time == 1)
	{
		for (int k = 0; k < 3; k++)
		{
			debug_colors[3 + k][0] = 0;			// Azul
			debug_colors[3 + k][1] = 0;			// Azul
			debug_colors[3 + k][2] = 255;		// Azul
		}
	}


	// Config Controle

	// Parametros do PID na seguinte ordem: PF, IF, DF, PT, IT, DT, PFR, IFR, DFR, PTR, ITR, DTR

	float param0[12] = { 11.45, 1.49, 26.9, 13.59, 1.63, 25.86, 1.03, 0.14, 9.36, 1.4, 0.23, 9.44 };
	float param1[12] = { 25.72, 1.01, 20.07, 22.34, 0.94, 21.1, 10, 0.64, 4.92, 10, 0.61, 6.92 };
	float param2[12] = { 21.86, 1.06, 18, 22, 1.03, 25.03, 3.59, 0.32, 4.94, 2.9, 0.89, 3.79 };

	CvFont fonte;
	cvInitFont(&fonte, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, 8);


	cvNamedWindow("Live", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow("Filtro Atual");
	//cvNamedWindow("Estrategia");

	// Inicialização da Visão
	vision Camera;
	Camera.createCamera(0);
	Camera.getImage(ROI);
	Camera.configParam(parametro, amin, amax, time, min_distance, ROI);

	// Inicialização da Estratégia
	estrategia Estrategia(Camera.h, Camera.w, side, canto_gol1, canto_gol2, canto_area1, canto_area2);
	Estrategia.Config(Camera.h, Camera.w);
	pesos = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, 1);
	Estrategia.ConfigAreas(canto_gol1, canto_gol2, canto_area1, canto_area2);

	// Inicialização do Controle
	

	controle_bili Controle_bili;

	// Inicialização do Serial
	Serial* SP = new Serial("COM4");
	if (SP->IsConnected())
		printf("We're connected\n");




	//============================== VARIÁVEIS  DE TESTE ============================//

	int contador = 0;
	float t;
	float ref = 0.0, T = 40000.0, f1 = 0.0, f2 = 0.0, torque = 0.0, fx_B = 0.0, d = 3.75, KP_w = 0.0, KI_w = 0.0, flag = 0.0, K, KP_u, R = 55.0, a, dist = 0.0;

	//float x = Camera.players[0].x,e =0.0;
	//float y = Camera.players[0].y;
	Camera.players[0].theta_acumulated = 0;
	Camera.players[0].theta_old = 0;
	//float theta = Camera.players[0].theta;
	//float t = visionF - start;
	//float v1 = 15;
	//float v2 = -15;
	float c, alpha = 0.0, biel = 0.0, v1_send = 0.0, v2_send = 0.0;
	float dx0, dy0, Kp, /*v1 = 0.0, v2 = 0.0,*/ theta2 = 0.0, thetad = 0.0, Kt, refaux = 0.0, i = 0.0, o = 0.0, ang_error = 0.0;
	float ct, st, erro_angulo = 0.0, x_bola = 0.0, y_bola = 0.0, thetad_old = 0.0, u_i = 0.0, KI_u = 0.0;
	float w = 0.0, u = 0.0, w_i = 0.0, w_p = 0.0, h = 0.0, wc = 0.0, uc = 7.0, f = 300.0,  u_p = 0.0, x_0 = 155, y_0 = 116;
	h = 1.0 / 12.0;
	int xd = 0, yd = 0;
	char temp[2];
	char tempread[2];
	//char msg[12];
	//byte sum;
	float xd_old = 0.0, yd_old = 0.0;
	float n_xd = Camera.players[0].x, n_yd = Camera.players[0].y;
	start = clock();
	Camera.players[0].refaux_acumulated = 0.0;
	Camera.players[0].refaux_old = 0.0;
	Camera.players[0].refaux = 0.0;
	int contar = 0, inicio_voltas = 0.0;

	char incomingData[256] = "";			// don't forget to pre-allocate memory
	//printf("%s\n",incomingData);
	int dataLength = 256;
	int readResult = 0;

	double t1 = 0;
	double tChecksum_IN;
	double tChecksum_OUT;
	//double tRecieve_IN;
	double tRecieve_OUT;
	int loop = 0;

	byte sum = 0;
	byte recievedCheckSum = 0;
	int left = 0;
	int right = 0;
	int recievedValue = 0;
	float vADC = 0;
	float voltagem = 0;
	int ze = 255;
	int newV1 = 0;
	int newV2 = 0;
	//	int loop;

	int change_loop = 1;	// o valor dessea variavel fica entre 1 e 6, ela escolhe qual loop estaremos usando
	char point		= 'i';	// i-> ponto inicial do teste, f-> ponto final do teste
	char param		= '0';  // 0-> proporcional, 1-> integral, 2-> derivativo
	int step_flag	= 1;	// 1-> passo alto, 2-> passo medio, 3-> passo baixo, 4-> achamos o valor "ótimo";
	int analise		= 0;	// 0->análise do ts, 1->análise do overshoot, 2->análise do upertime

	//=========================== INÍCIO DO LOOP ==============================//
	while (true)
	{
		//====================Leitura Mensagem================================ //
		readResult = SP->ReadData(incomingData, 11);

		/*for (int i = 0; i<11; i++)
		{
		cout << dec << i << ": ";
		cout << hex << (int)incomingData[i] << endl;
		//cout << "haha" << endl;
		//system("pause");

		}*/

		//O endereço do xbee de onde veio a mensagem é incomingData[4] e incomingData[5]
		if (incomingData[0] == 0x7e && incomingData[1] == 0x00 && incomingData[2] == 0x07 && incomingData[3] == -127 && incomingData[4] == -86 && incomingData[5] == -86 && incomingData[7] == 0)
		{
			//cout << "_-_-_-_-_-_" << endl;

			sum = 0;
			recievedCheckSum = 0;
			left = 0;
			right = 0;
			recievedValue = 0;
			voltagem = 0;
			vADC = 0;
			//loop = 0;

			for (int i = 0; i < 11; i++)
			{
				if (i>2 && i < 10)
				{
					sum = sum + incomingData[i];
				}
			}
			sum = 0xFF - sum;
			sum = 0xFF & sum;

			if (incomingData[10] & 0x111111)
			{
				recievedCheckSum = (int)(incomingData[10] - 0xffffff00);
				//cout << "HAHA" << endl;
			}
			else
			{
				recievedCheckSum = (int)(incomingData[10]);
			}

			if (recievedCheckSum == sum)
			{
				//cout << "Mensagem Valida" << endl;
				//system("cls");
				//cout << "left: " << hex << (int)incomingData[8] << endl;
				//cout << "right: " << hex << (int)incomingData[9] << endl;
				//cout << "check: " << dec << ((int)incomingData[9]) << endl;
				if (incomingData[8] & 0x1111111)
				{
					//cout << "caso 1" << endl;
					left = (int)(incomingData[8] - 0xffffff00);
					left = left - 256;

				}
				else if (incomingData[8] & 0x111111)
				{
					//cout << "caso 2" << endl;
					left = (int)(incomingData[8] - 0xffffff00);
					//left = left -256;
				}
				else
				{
					//cout << "caso 3" << endl;
					left = (int)(incomingData[8]);

				}


				//cout << dec << (int)(incomingData[9]) << endl;
				if (incomingData[9] & 0x1111111)
				{
					right = (int)(incomingData[9] - 0xffffff00);
					if ((int)incomingData[9] > 0)
					{
						right = right - 256;
					}
					//right = right - 256;
					//cout << "caso 1" << endl;

				}
				else if (incomingData[9] & 0x111111)
				{
					right = (int)(incomingData[9] - 0xffffff00);
					//left = left -256;
					//cout << "caso 2" << endl;
				}
				else
				{
					right = (int)(incomingData[9]);
					//cout << "caso 3" << endl;

				}


				/*if (incomingData[9] & 0x111111)
				{
				right = (int)(incomingData[9] - 0xffffff00);
				}
				else
				{
				right = (int)(incomingData[9]);
				}*/

				//cout <<"esquerda: " << left<< endl;
				//cout << "direita: " << right << endl;
				recievedValue = 768 + right;
				vADC = recievedValue * 0.00489;
				voltagem = 0.013414862*recievedValue;

				/*
				cout << "Total: " << dec << recievedValue << endl;
				cout << "ADC: " << dec << vADC << endl;
				printf("Voltagem: %0.2f V \n\n", voltagem);
				cout << loop++ << endl;
				*/

				//cout << "Tempo receber: " << clock() - t1 << "ms" << endl;
				//cout << "Recieve: " << tRecieve_OUT - t1 << "ms" << endl;
				//cout << "Checksum: " << tChecksum_IN - tChecksum_IN << "ms" << endl;
				//cout << loop++ << endl;


				/*if (recievedValue < 1024)
				{
				system("cls");
				voltagem = 0.0135*recievedValue;
				cout << "Total: " << dec << recievedValue << endl;
				printf("Voltagem: %0.2f V \n\n", voltagem);
				cout << "Tempo receber: " << clock() - t1 << "ms" <<endl;
				cout << "Recieve: " << tRecieve_OUT - t1 << "ms" << endl;
				cout << "Checksum: " << tChecksum_IN - tChecksum_IN << "ms" << endl;
				cout << loop++ << endl;
				}*/

				//cout << "Voltagem: " << dec << voltagem << endl;

			}
		}
		else //=== Mensagem Inválida 
		{

			char incomingData[256] = "";
		}

		/*for (int i = 0; i < 11; i++)
		{
		cout << i << ": " << hex << (int)incomingData[i] << endl;
		}*/


		//cout << endl << endl << endl << endl;

		//cout << char(incomingData[6]) << endl;

		//std::string test(incomingData);


		//printf("%s", incomingData);
		//test = "";



		//==================================================================== //
		
		
		
		
		
		//cout << "theta  inicio = " << Camera.players[0].theta << endl;

		counter++;
		totalI = clock();



		// =========================== VISÃO =========================== //
		visionI = clock();

		Camera.getImage();
		Camera.dataAcquire();
		Camera.players[1] = robot(-1, -1);
		Camera.players[2] = robot(-1, -1);


		visionF = clock();



		// =========================== ESTRATEGIA =========================== //

		estraI = clock();

		//Atribuição de comportamentos
//		Estrategia.update(Camera.players);

//		Estrategia.comportamento3();
//		Estrategia.Posse();
		//Estrategia.players[0].comportamento = 3;
		//Estrategia.players[1].comportamento = 3;
		//Estrategia.players[2].comportamento = 2;

		// Atribuição de objetivos
//		Estrategia.goleiro4();
//		Estrategia.lateral();
//		Estrategia.atacante2();
//		Estrategia.zagueiro();


		//Estrategia.generalRouting();


		//Estrategia.players[0].setGoal(23, 98);
		//Estrategia.players[1].setGoal(23, 98);


		estraF = clock();



		//  =========================== CONTROLE =========================== //
		/*controlI = clock();
		Controle00.controla(Camera.players[0], robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y), robot(Estrategia.players[0].objective_x, Estrategia.players[0].objective_y));
		Controle01.controla(Camera.players[1], robot(Estrategia.players[1].goal_x, Estrategia.players[1].goal_y), robot(Estrategia.players[1].objective_x, Estrategia.players[1].objective_y));
		Controle02.controla(Camera.players[2], robot(Estrategia.players[2].goal_x, Estrategia.players[2].goal_y), robot(Estrategia.players[2].objective_x, Estrategia.players[2].objective_y));
		controlF = clock();*/

		// =========================== COMUNICAÇÃO =========================== //
		commI = clock();


		


		//if (!clicked.stop)
		//{
		//	xd = clicked.x  -x_0;
		//	yd = -(clicked.y - y_0);
		//}

		//if (clicked.stop)
		//{
		//	xd = Camera.players[0].x - x_0;
		//	yd = -(Camera.players[0].y- y_0);
		//}
		
		


		
		//xd = Camera.players[6].x - x_0;
		//yd = -(Camera.players[6].y - y_0);
		//yd = sin((6.28 / T) * t)*R;
		//xd = cos((6.28 / T) * t)*R;

		/*if (Camera.players[6].x == -1){
			xd = xd_old;
			yd = yd_old;
		}
		else{
			xd_old = xd;
			yd_old = yd;
		}*/


		///// -------------------------------------- MODIFICAÇÃO BIEL -----------------------------------------------------/////

		/* 
		VERIFICAR PRECISAO DAS VARIAVEIS DE REFERÊNCIA -> PARA SABER SE AS CONDIÇÕES SERÃO SATISFEITAS
		VERIFICAR QUAIS SÃO AS VARIAVEIS DE REFERÊNCIA
		*/

		// Apartir daqui procuraremos os ganho para o ajuste angular
		if (change_loop == 1)
		{
			/*Loop para fazer o carro ir ate o ponto inicial do teste*/
			xd = 120;	//valor de x de um extremo do campo
			yd = -9;	//valor de y de um extremo do campo

			if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
			{
				thetad = 0.0;
				if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
				{
					change_loop = 2;
					point = 'f';
				}
			}
		}
		else if (change_loop == 2)
		{
			if (point == 'f')
			{
				/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
				thetad = 60.0;

				// Nesse caso testaremos os ganhos de rotação
				Camera.players[0].gain[0] = param0[3];
				Camera.players[0].gain[1] = param0[4];
				Camera.players[0].gain[2] = param0[5];

				Camera.players[0].group_theta.push_back(Camera.players[0].theta);

				if ((Camera.players[0].theta <= thetad + 0.05) && (Camera.players[0].theta <= thetad + 0.05))
				{
					Camera.players[0].condition_theta[0] = Controle_bili.find_ts('t', Camera.players[0], Camera.players[0].group_theta.size());
					Camera.players[0].condition_theta[1] = Controle_bili.find_overshoot('t', Camera.players[0], Camera.players[0].group_theta.size());
					Camera.players[0].condition_theta[2] = Controle_bili.find_upertime('t', Camera.players[0], Camera.players[0].group_theta.size());
					point = 'i';
				}
			}
			else if (point == 'i')
			{
				thetad = 0.0;
				if ((Camera.players[0].x <= thetad + 0.09) && (Camera.players[0].x >= thetad - 0.09))
				{
					change_loop = 3;
					point = 'f';

					// O teste começara com o proporcional rotacional
					param0[3] += 0.5;
					Camera.players[0].group_theta.clear();

					// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
					Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
					Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
					Camera.players[0].old_gain[2] = Camera.players[0].gain[2];

					Camera.players[0].gain[0] = param0[3];
					Camera.players[0].gain[1] = param0[4];
					Camera.players[0].gain[2] = param0[5];
				}
			}
		}
		else if (change_loop == 3)
		{
			if (point == 'f')
			{
				/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
				thetad = 0.0;
				Camera.players[0].group_theta.push_back(Camera.players[0].theta);

				if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta <= thetad + 0.09))
				{
					// Apartir daqui atribuiremos os valores anteriores de ts, overshoot e upertime
					Camera.players[0].condition_theta[3] = Camera.players[0].condition_theta[0];
					Camera.players[0].condition_theta[4] = Camera.players[0].condition_theta[1];
					Camera.players[0].condition_theta[5] = Camera.players[0].condition_theta[2];
					// Término das atribuições

					//Começamos a medir os novos valores de  ts, overshoot e upertime
					Camera.players[0].condition_theta[0] = Controle_bili.find_ts('t', Camera.players[0], Camera.players[0].group_theta.size());
					Camera.players[0].condition_theta[1] = Controle_bili.find_overshoot('t', Camera.players[0], Camera.players[0].group_theta.size());
					Camera.players[0].condition_theta[2] = Controle_bili.find_upertime('t', Camera.players[0], Camera.players[0].group_theta.size());
					// Término das novas atribuições

					float wanted_ts = 50.0;	// Não sei o valor de ts,  chutei como 50
					float wanted_overshoot = 50.0;	// Não sei o valor de overshoot,  chutei como 50
					float wanted_upertime = 50.0;	// Não sei o valor de upertime,  chutei como 50

					switch (param)					// O switch é para sabermos qual ganho estamos analisando no momento
					{
					case '0':
						// Ganho proporcional
						if (analise == 0)		//se igual a zero fazemos a analise de ts
						{
							step_flag = Controle_bili.improve_p_ts(Camera.players[0], 't', wanted_ts, step_flag);
						}
						else if (analise == 1)	//se igual a 1 fazemos a anlise de overshoot
						{
							step_flag = Controle_bili.improve_p_overshoot(Camera.players[0], 't', wanted_overshoot, step_flag);
						}
						else if (analise == 2)	// se igual a 2 fazemos a analise do tempo de subida (upertime)
						{
							step_flag = Controle_bili.improve_p_upertime(Camera.players[0], 't', wanted_upertime, step_flag);
						}
						if (step_flag == 4)
						{
							param = 1;
						}
						else
						{
							point = 'i';
						}
						param0[3] = Camera.players[0].gain[0];
						break;
					case '1':
						// Ganho integral
						if (analise == 0)		//se igual a zero fazemos a analise de ts
						{
							step_flag = Controle_bili.improve_i_ts(Camera.players[0], 't', wanted_ts, step_flag);
						}
						else if (analise == 1)	//se igual a 1 fazemos a anlise de overshoot
						{
							step_flag = Controle_bili.improve_i_overshoot(Camera.players[0], 't', wanted_overshoot, step_flag);
						}
						else if (analise == 2)	// se igual a 2 fazemos a analise do tempo de subida (upertime)
						{
							step_flag = Controle_bili.improve_i_upertime(Camera.players[0], 't', wanted_upertime, step_flag);
						}
						if (step_flag == 4)
						{
							param = 2;
						}
						else
						{
							point = 'i';
						}
						param0[4] = Camera.players[0].gain[1];
						break;
					case '2':
						// Ganho derivativo
						step_flag = Controle_bili.improve_d_ts(Camera.players[0], 't', wanted_ts, step_flag);
						if ((step_flag == 4) && (analise == 0))
						{
							param = 1;
							analise = 1;
						}
						else if ((step_flag == 4) && (analise == 1))
						{
							param = 1;
							analise = 2;
						}
						else if ((step_flag == 4) && (analise == 2))
						{
							change_loop = 4;
						}
						else
						{
							point = 'i';
						}
						param0[5] = Camera.players[0].gain[2];
						break;
					}
					point = 'i';
				}
			}
			else if (point == 'i')
			{
				thetad = 0.0;
				if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
				{
					point = 'f';
					Camera.players[0].group_theta.clear();

					// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
					Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
					Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
					Camera.players[0].old_gain[2] = Camera.players[0].gain[2];

					Camera.players[0].gain[0] = param0[0];
					Camera.players[0].gain[1] = param0[1];
					Camera.players[0].gain[2] = param0[2];
				}
			}
		}

		// Apartir daqui procuraremos os ganho para o ajuste linear
		// O AJUSTE SERÁ FEITO POR UMA MOVIMENTAÇÃO NO EIXO X LOGO ALTERAREMOS OS GANHOS BASEADOS NESSE EIXO
		else if (change_loop == 4)
		{
			/*Loop para fazer o carro ir ate o ponto inicial do teste*/
			xd = 120;	//valor de x de um extremo do campo
			yd = -9;	//valor de y de um extremo do campo

			if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
			{
				thetad = 0.0;  
				if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
				{
					change_loop = 5;
					point = 'f';
				}
			}
		}
		else if (change_loop == 5)
		{
			if (point == 'f')
			{
				/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
				xd = 155;	//valor de x do centro do campo
				yd = -116;	//valor de y do centro do campo
			
				// Nesse caso testaremos os ganhos de deslocamento linear
				Camera.players[0].gain[0] = param0[0];
				Camera.players[0].gain[1] = param0[1];
				Camera.players[0].gain[2] = param0[2];
				
				Camera.players[0].group_x.push_back(Camera.players[0].x);
				//Camera.players[0].group_y.push_back(Camera.players[0].y);

				if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
				{
					Camera.players[0].condition_x[0] = Controle_bili.find_ts('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[0]	 = control.find_ts('y', Camera.players[0], Camera.players[0].group_y.size());

					Camera.players[0].condition_x[1] = Controle_bili.find_overshoot('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[1]	 = control.find_overshoot('y', Camera.players[0], Camera.players[0].group_y.size());

					Camera.players[0].condition_x[2] = Controle_bili.find_upertime('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[2]	 = control.find_upertime('y', Camera.players[0], Camera.players[0].group_y.size());
					point = 'i';
				}
			}
			else if (point == 'i')
			{
				xd = 120;	//valor de x de um extremo do campo
				yd = -9;	//valor de y de um extremo do campo

				if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
				{
					thetad = 0.0;
					if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
					{
						change_loop = 6;
						point = 'f';
						
						// O teste começara com o proporcional linear
						param0[0] += 0.5;
						Camera.players[0].group_x.clear();
						//Camera.players[0].group_y.clear();

						// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
						Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
						Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
						Camera.players[0].old_gain[2] = Camera.players[0].gain[2];

						Camera.players[0].gain[0] = param0[0];
						Camera.players[0].gain[1] = param0[1];
						Camera.players[0].gain[2] = param0[2];
					}
				}
			}
		}
		else if (change_loop == 6)
		{
			if (point == 'f')
			{
				/*Loop para fazer o teste com os valores inicias dos parametros do PID*/
				xd = 155;	//valor de x do centro do campo
				yd = -116;	//valor de y do centro do campo
				Camera.players[0].group_x.push_back(Camera.players[0].x);
				//Camera.players[0].group_y.push_back(Camera.players[0].y);

				if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
				{
					// Apartir daqui atribuiremos os valores anteriores de ts, overshoot e upertime
					Camera.players[0].condition_x[3]	 = Camera.players[0].condition_x[0];
					//Camera.players[0].condition_y[3]	 = Camera.players[0].condition_y[0];

					Camera.players[0].condition_x[4]	 = Camera.players[0].condition_x[1];
					//Camera.players[0].condition_y[4]	 = Camera.players[0].condition_y[1];

					Camera.players[0].condition_x[5]	 = Camera.players[0].condition_x[2];
					//Camera.players[0].condition_y[5]	 = Camera.players[0].condition_y[2];
					// Término das atribuições

					//Começamos a medir os novos valores de  ts, overshoot e upertime
					Camera.players[0].condition_x[0] = Controle_bili.find_ts('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[0]	 = control.find_ts('y', Camera.players[0], Camera.players[0].group_y.size());

					Camera.players[0].condition_x[1] = Controle_bili.find_overshoot('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[1]	 = control.find_overshoot('y', Camera.players[0], Camera.players[0].group_y.size());

					Camera.players[0].condition_x[2] = Controle_bili.find_upertime('x', Camera.players[0], Camera.players[0].group_x.size());
					//Camera.players[0].condition_y[2]	 = control.find_upertime('y', Camera.players[0], Camera.players[0].group_y.size());
					// Término das novas atribuições

					float wanted_ts = 50.0;	// Não sei o valor de ts,  chutei como 50
					float wanted_overshoot = 50.0;
					float wanted_upertime = 50.0;
					
					// OBS: COMO FAZER PARA ACHAR GANHOS DE X E Y E JUNTA-LOS NUM MOVIMENTO LINEAR?????????????

					switch (param)// O switch é para sabermos qual ganho estamos analisando no momento
					{
					case '0':
						// Ganho proporcional
						if (analise == 0)		//se igual a zero fazemos a analise de ts
						{
							step_flag = Controle_bili.improve_p_ts(Camera.players[0], 'X', wanted_ts, step_flag);
						}
						else if (analise == 1)	//se igual a 1 fazemos a anlise de overshoot
						{
							step_flag = Controle_bili.improve_p_overshoot(Camera.players[0], 'X', wanted_overshoot, step_flag);
						}
						else if (analise == 2)	// se igual a 2 fazemos a analise do tempo de subida (upertime)
						{
							step_flag = Controle_bili.improve_p_upertime(Camera.players[0], 'X', wanted_upertime, step_flag);
						}
						if (step_flag == 4)
						{
							param = 1;
						}
						else
						{
							point = 'i';
						}
						param0[0] = Camera.players[0].gain[0];
						break;
					case '1':
						// Ganho integral
						if (analise == 0)		//se igual a zero fazemos a analise de ts
						{
							step_flag = Controle_bili.improve_i_ts(Camera.players[0], 'X', wanted_ts, step_flag);
						}
						else if (analise == 1)	//se igual a 1 fazemos a anlise de overshoot
						{
							step_flag = Controle_bili.improve_i_overshoot(Camera.players[0], 'X', wanted_overshoot, step_flag);
						}
						else if (analise == 2)	// se igual a 2 fazemos a analise do tempo de subida (upertime)
						{
							step_flag = Controle_bili.improve_i_upertime(Camera.players[0], 'X', wanted_upertime, step_flag);
						}
						if (step_flag == 4)
						{
							param = 2;
						}
						else
						{
							point = 'i';
						}
						param0[1] = Camera.players[0].gain[1];
						break;
					case '2':
						// Ganho derivativo
						step_flag = Controle_bili.improve_d_ts(Camera.players[0], 'x', wanted_ts, step_flag);
						if ((step_flag == 4) && (analise == 0))
						{
							param = 1;
							analise = 1;
						}
						else if ((step_flag == 4) && (analise == 1))
						{
							param = 1;
							analise = 2;
						}
						else if ((step_flag == 4) && (analise == 2))
						{
							change_loop = 7;
						}
						else
						{
							point = 'i';
						}
						break;
					}

					point = 'i';
				}
			}
			else if (point == 'i')
			{
				xd = 120;	//valor de x de um extremo do campo
				yd = -9;	//valor de y de um extremo do campo

				if ((Camera.players[0].x <= xd + 12) && (Camera.players[0].x >= xd - 12) && (Camera.players[0].y <= yd + 12) && (Camera.players[0].y <= yd + 12))
				{
					thetad = 0.0;
					if ((Camera.players[0].theta <= thetad + 0.09) && (Camera.players[0].theta >= thetad - 0.09))
					{
						point = 'f';
						Camera.players[0].group_x.clear();
						//Camera.players[0].group_y.clear();

						// Armazenamos os ganhos atuais, nos ganhos anteriores e atualizamos os ganhos atuais
						Camera.players[0].old_gain[0] = Camera.players[0].gain[0];
						Camera.players[0].old_gain[1] = Camera.players[0].gain[1];
						Camera.players[0].old_gain[2] = Camera.players[0].gain[2];

						Camera.players[0].gain[0] = param0[0];
						Camera.players[0].gain[1] = param0[1];
						Camera.players[0].gain[2] = param0[2];
					}
				}
			}
		}
		else if (change_loop == 7)
		{
			cout << "Os ganhos são :" << endl;
			cout << "GPL " << param0[0] << endl;
			cout << "GIL " << param0[1] << endl;
			cout << "GDL " << param0[2] << endl;
			cout << "GPR " << param0[3] << endl;
			cout << "GIR " << param0[4] << endl;
			cout << "GDR " << param0[5] << endl;
		}


			
		///// -------------------------------------- TÉRMINO DA MODIFICAÇÃO BIEL -----------------------------------------------------/////


		Controle_bili.rotina_controle(0, 0, &Camera, xd, yd);
		newV2 = (0.000001947*pow(recievedValue, 2) - 0.004673*recievedValue + 3.495)*abs(Controle_bili.v2 * 255);
		newV1 = (0.000001947*pow(recievedValue, 2) - 0.004673*recievedValue + 3.495)*abs(Controle_bili.v1 * 255);
		//SP->sendApi(0, Controle_bili.o, abs(Controle_bili.v2 * 255), Controle_bili.i, abs(Controle_bili.v1 * 255));
		SP->sendApi(0, Controle_bili.o, newV2, Controle_bili.i, newV1);



		if (Camera.players[0].x != -1)
			fprintf(gFile, "%f,%f,%f, %i, %f, %f,%f,%f,%f,%f,%f, %f\n", Controle_bili.x*1.3, Controle_bili.y*1.3, Controle_bili.theta, int(visionF - start), Controle_bili.thetad, Controle_bili.v1, Controle_bili.v2, w, xd*1.3, yd*1.3, u, Controle_bili.refaux);

		//SP->sendApi(0, 1, 84, 0, 0);
		//=================== DISPLAY DOS DADOS DO ROBO ============//

		cout << "thetad = " << Controle_bili.thetad*180/M_PI << endl;
		cout << "theta = " << Controle_bili.theta * 180 / M_PI << endl;
		cout << "ang_error = " << Controle_bili.ang_error * 180 / M_PI << endl;
		//cout << "ypc = " << dy0 << endl;
		cout << "v1 = " << Controle_bili.v1 << endl;
		cout << "v2 = " << Controle_bili.v2 << endl;
		//cout << "u_p = " << Controle_bili.u_p << endl;
		
		

		
		//======================== ENVIO DAS VELOCIDADES ====================//

		if (!paused)
		{
			//SP->send(0, v2, v1);
			//SP->send(v2, v1);
			//SP->send(0, Controle00.v.x, Controle00.v.y);
			// whishPath.push_back(robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y));
			//sfollowedPath.push_back(Camera.players[0]);
			//SP->send(1, Controle01.v.x, Controle01.v.y);
			//SP->send(2, Controle02.v.x, Controle02.v.y);
		}
		else
		{
			/*
			SP->send(0, 0, 0);
			SP->send(1, 0, 0);
			SP->send(2, 0, 0);
			*/
		}
		commF = clock();


		// Debug em prompt de comando
		debugI = clock();

		//system("cls");
		//cout << "Roxo: " << Estrategia.players[0].x << ", " << Estrategia.players[0].y << " --> " << Estrategia.players[0].goal_x << ", " << Estrategia.players[0].goal_y << endl;
		//cout << "Verde: " << Estrategia.players[1].x << ", " << Estrategia.players[1].y << " --> " << Estrategia.players[1].goal_x << ", " << Estrategia.players[1].goal_y << endl;
		//cout << "Marrom: " << Estrategia.players[2].x << ", " << Estrategia.players[2].y << " --> " << Estrategia.players[2].goal_x << ", " << Estrategia.players[2].goal_y << endl;

		//cout << "Carrinho: (" << Camera.players[0].x << ", " << Camera.players[0].y << ", " << Camera.players[0].theta *(180/M_PI) << ")" << endl;
		//cout << "Objetivo: (" << (int) Estrategia.ponto_f.x << ", " << (int) Estrategia.ponto_f.y << ")" << endl;
		//cout << "Velocidade (d, e): " << Controle00.v.x << ", "<< Controle00.v.y << ")" <<endl;



		// Debug em janela

		cvSetMouseCallback("Live", mouseEvent, 0);

		// Desenhando nas janelas
//		cv::resize(Estrategia.fieldI[1], pesos, cv::Size(Camera.frame->width, Camera.frame->height), 0, 0);
//		cvReleaseImage(&pesos3);

		pesos3 = cvCreateImage(cvGetSize(Camera.frame), Camera.frame->depth, Camera.frame->nChannels);
		cvMerge(&(IplImage)pesos, &(IplImage)pesos, &(IplImage)pesos, NULL, pesos3);
		//cvAddWeighted(pesos3, 0.9, Camera.frame, 0.8, 80, Camera.frame);

		for (int i = 0; i < 7; i++)
		{
			if (Camera.players[i].x != -1)
			{
				cvCircle(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), radius, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
				cvLine(Camera.frame, cvPoint((int)Camera.players[i].x, (int)Camera.players[i].y), cvPoint((int)Camera.players[i].x + radius*cos(Camera.players[i].theta),
					(int)Camera.players[i].y + radius*sin(Camera.players[i].theta)), CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]), 2);
			}
		}



		cout << "nxd: " << xd << " nyd: " << yd << endl;
		cvCircle(Camera.frame, cvPoint(xd + 155, -yd + 116), 5, CV_RGB(0, 255, 0), -1);

		
		cvShowImage("Live", Camera.frame);



		//Escape Sequence
		char_pressed = cvWaitKey(1);
		if (char_pressed == 27) //esc
		{

			temp[0] = 0x80;
			temp[1] = 0x80;
			SP->WriteData(temp, 2);
			SP->sendApi(0, 0, 0, 0, 0);
			SP->sendApi(0, 0, 0, 0, 0);
			SP->sendApi(0, 0, 0, 0, 0);

			break;


		}
		if (char_pressed == 112) //p
		{
			if (paused)
				paused = false;
			else{
				paused = true;
				flag = 1.0;
				//thetad = alpha;
			}
		}
		if (char_pressed == 99) //c
		{
			whishPath.clear();
			followedPath.clear();
		}
		if (char_pressed == 105) //i
		{

			//e += 1;

		}
		if (char_pressed == 111) //o
		{

			//e -= 1;

		}

		if (char_pressed == 117) //u
		{
			wc = 0.0;


			temp[0] = 0x80;
			temp[1] = 0x80;
			SP->WriteData(temp, 2);

		}
		if (char_pressed == 107) //k
		{



			R += 1;

		}
		if (char_pressed == 108) //l
		{

			R -= 1;
		}

		if (char_pressed == 113) //q
		{
			f += 5;

		}

		if (char_pressed == 119) //w
		{
			f -= 5;

		}


		if (char_pressed == 109) // m
		{
			xd += 10;

		}


		if (char_pressed == 98) // b
		{
			xd -= 10;

		}

		if (char_pressed == 104) // h
		{
			yd += 10;

		}

		if (char_pressed == 110) // n
		{
			yd -= 10;

		}

		debugF = clock();
		totalF = clock();
		//system("cls");

		//if (paused)
		//	cout << "Paused" << endl;
		//else
		system("cls");
		//cout << "Running" << endl;
		//cout << "Tempo de analise de imagem: " << (visionF - visionI) << endl;
		//cout << "Tempo da Estrategia: " << estraF - estraI << endl;
		//cout << "Tempo do Controle: " << controlF - controlI << endl;
		//cout << "posição: " << camera.players[0].x << " , " << camera.players[0].y << endl;
		//cout << "Tempo da Comunicacao: " << commF - commI << endl;
		//cout << "Tempo do Debug:" << debugF - debugI<< endl;
		//cout << "Tempo Total: " << (totalF - totalI) << endl;
		//cout << "Frame Rate: " << 1000 * counter/ (totalF - start) << endl;
		//cout << "Frame Rate Instantanea: " << 1000 / (totalF - visionI) << endl;
		//cout << "F1: " << f1 << endl;
		//cout << "F2: " << f2 << endl;
		//cout << counter << endl;
		//cout << "tempread1 = " << (int)tempread[1] << endl;
		//cout << "tempread2 = " << (int)tempread[0] << endl;
		//cout << "temp = " << (int)temp[1] << endl;
		//cout << "temp = " << (int)temp[0] << endl;
		//cout << "PERÍODO = " << T << endl;
		//cout << "Soma: " << (int)sum << endl;



		//Sleep(100);
	}
}
コード例 #6
0
ファイル: main.cpp プロジェクト: HaoguangYang/AgileVehicle
// application reads from the specified serial port and reports the collected data
int _tmain(int argc, _TCHAR* argv[])
{
// connect the COM
	printf("Welcome to the serial test app!\n\n");
#if defined (_MSC_VER)
	Serial* SP = new Serial("\\\\.\\COM6");    // adjust as needed
#elif defined (__linux__)
    string PORTNAMEIN;
    cout << "Input Serial Port (e.g. /dev/ttyACM0): " << endl;
    cin >> PORTNAMEIN;
    char *PORTNAME = new char [PORTNAMEIN.length() + 1];
    std::strcpy(PORTNAME, PORTNAMEIN.c_str());
	Serial* SP = new Serial(PORTNAME);
	unsigned int JOYSTICKID1;
#endif

	if (SP->IsConnected())
		printf("We're connected!\n");

//data communication
	char incomingData[256] = "";			// don't forget to pre-
	int dataLengthin = 127;
	int readResult = 0;

	int steer=2048;
	int breaking = 255;
	const int encoder_resolution=4096;
	char outSteer[6]="2048s";
	int driveDutycycle=0;
	const int fullDutycycle=255;
	char outDrive[5]="001d";
	char outBreak[5]="100b";

//joystick initialize***********************

	joyinfoex_tag joyinfo;

#ifdef __linux__
    if (SDL_Init(SDL_INIT_JOYSTICK) < 0){
        cout << "Error initializing SDL!" << endl;
        return 1;
    }
	SDL_Joystick *joy;
    int CtrlNum = SDL_NumJoysticks();
    if (CtrlNum == 1)
        JOYSTICKID1 = 0;
    else{
        cout << "There are " << CtrlNum << " controllers found..." << endl;
        for(int i=0;i<CtrlNum;i++)
        {
            joy = SDL_JoystickOpen(i);
            printf("%s\n", SDL_JoystickName(joy));
        }
        cout << "Choose the one you wish to use: " << endl;
        cin >> JOYSTICKID1;
    }
    joy=SDL_JoystickOpen(JOYSTICKID1);
    if (joy) {
        printf("Opened Joystick %d\n",JOYSTICKID1);
        printf("Name: %s\n", SDL_JoystickNameForIndex(0));
        printf("Number of Axes: %d\n", SDL_JoystickNumAxes(joy));
        printf("Number of Buttons: %d\n", SDL_JoystickNumButtons(joy));
        printf("Number of Balls: %d\n", SDL_JoystickNumBalls(joy));
    } else {
        printf("Couldn't open Joystick %d\n", JOYSTICKID1);
        return -1*JOYSTICKID1;
    }
#endif

//button status
	bool is_in_situ=false;
	bool is_any_direction=false;
	bool is_tradition=true;
	bool now_in_situ=false;
	bool now_any_direction=false;
	bool now_tradition=false;
	bool last_in_situ=false;
	bool last_any_direction=false;
	bool last_tradition=false;

	//int count = 0;
//start control
	bool action = 1;
#if defined(_MSC_VER)
	while(SP->IsConnected())                            // CYCLES HERE...
	{
		//acquire joystick info **********************
		joyGetPosEx(JOYSTICKID1, &joyinfo);
#elif defined(__linux__)
    SDL_Event SysEvent;
    bool NoQuit = true;
    while(SP->IsConnected() && NoQuit)
    {
    	if (SDL_PollEvent(&SysEvent))
        {
            switch (SysEvent.type)
            {
                case SDL_JOYAXISMOTION:
                {
                    joyinfo.dwXpos = SDL_JoystickGetAxis(joy,0) + 32767 + 1 ;
	    	        joyinfo.dwYpos = SDL_JoystickGetAxis(joy,1) + 32767 + 1 ;
	    	        joyinfo.dwZpos = SDL_JoystickGetAxis(joy,2) + 32767 + 1 ;
	    	        joyinfo.dwRpos = SDL_JoystickGetAxis(joy,3) + 32767 + 1 ;
	    	        //Only 4 axis.
		            //joyinfo.dwUpos = SDL_JoystickGetAxis(joy,4);
		            //joyinfo.dwVpos = SDL_JoystickGetAxis(joy,5);
	    	        break;
	    	    }
	    	    case SDL_QUIT:
	    	    {
	    	        NoQuit = false;
	    	        break;
	    	    }
	    	}
            SDL_FlushEvents(SDL_APP_TERMINATING, SDL_LASTEVENT); //Flush Old Events
#endif
		cout << "X Steering Wheel:" << joyinfo.dwXpos << endl;
		if(joyinfo.dwZpos != 32767)
			action = 0;
		if(action)
		{
			cout << "Z Throttle:" << 65536 << endl; //The original value of the inactivated throttle is 32767, should be modified to 65535 so that throttle is 0		
			cout << "R Break:" << 0.9*65536 << endl;
		}
		else
		{
			cout << "Z Throttle:" << joyinfo.dwZpos << endl; //If activated (moved), output real value.
			cout << "R Break:" << joyinfo.dwRpos << endl; //The same for breaks.
		}
		cout << "buttonNumber" << joyinfo.dwButtonNumber << endl;
		cout << "buttonStatus" << bitset<64>(joyinfo.dwButtons) << endl; //Output button status
		
		//Buttons:
		now_in_situ=bitset<64>(joyinfo.dwButtons)[0];
		now_any_direction=bitset<64>(joyinfo.dwButtons)[3];
		now_tradition=bitset<64>(joyinfo.dwButtons)[1];
		cout << "is_in_situ:" << is_in_situ << endl;
		cout << "is_any_direction:" << is_any_direction << endl;
		cout << "is_tradition:" << is_tradition << endl;
		
		//PostProcessing********************************************************
		if(now_in_situ== true && last_in_situ ==false)
		{
			is_in_situ =true;
			is_any_direction =false;
			is_tradition =false;

			SP->WriteData("b",1);
		}

		if(now_any_direction== true && last_any_direction ==false)
		{
			is_in_situ =false;
			is_any_direction =true;
			is_tradition =false;

			SP->WriteData("f",1);

		}

		if(now_tradition== true && last_tradition ==false)
		{
			is_in_situ =false;
			is_any_direction =false;
			is_tradition =true;
			SP->WriteData("f",1);

		}

		last_in_situ =now_in_situ;
		last_any_direction =now_any_direction;
		last_tradition =now_tradition;
		
		if(action)//Double-check that the inactivated throttle value is set to 0.
		{
			driveDutycycle = 0;
			breaking = 225;
		}
		else
		{
			breaking = (int)((1-joyinfo.dwRpos/65535.0)*fullDutycycle);			//Modify as necessary.
			if (breaking=0)
				driveDutycycle = (int)((1-joyinfo.dwZpos/65535.0)*fullDutycycle);
			else
				driveDutycycle = 0;
		}
		if(driveDutycycle>=0 && driveDutycycle<=255)
		{
			for (int k=0;k<3;k++)
			{
				outDrive[2-k]=char(driveDutycycle%10 + '0');
				driveDutycycle=driveDutycycle/10;
			}
		}
		if(breaking>=0 && breaking<=255)
		{
			for (int k=0;k<3;k++)
			{
				outBreak[2-k]=char(breaking%10 + '0');
				breaking=breaking/10;
			}
		}
		steer=(int)(joyinfo.dwXpos/65535.0*encoder_resolution);
		if(steer>=0 && steer<=encoder_resolution)
		{
			for (int k=0;k<4;k++)
			{
				outSteer[3-k]=char(steer%10 + '0');
				steer=steer/10;
			}
		}
		
		//Writing to Arduino Seriall**************************************************
		printf("outDrive:%s\n",outDrive);
		bool isWriteDrive = SP->WriteData((char*)&outDrive, strlen(outDrive));
		cout << "WriteSucceed?" << isWriteDrive << endl;
		printf("outSteer:%s\n",outSteer);
		bool isWriteSteer = SP->WriteData((char*)&outSteer, strlen(outSteer));
		cout << "WriteSucceed?" << isWriteSteer << endl;
		printf("outBreak:%s\n",outBreak);
		bool isWriteBreak = SP->WriteData((char*)&outBreak, strlen(outBreak));
		cout << "WriteSucceed?" << isWriteBreak << endl;

#if defined(_MSC_VER)
		Sleep(120);                 //time in ms
		system("cls");
#elif defined (__linux__)
		}       //If have sysevent then update joystick values
		usleep(120000);             //time in us
		system("clear");
#endif
        //Read back from Arduino
		readResult = SP->ReadData(incomingData,dataLengthin);
		///*if(readResult<2*sizeof(serial_format)){
		//	printf("No enough data received. Let's wait.\n");
		//	Sleep(1500);continue;
		//}*/

		int ptr=0;
		while(!(incomingData[ptr]=='?' && incomingData[ptr+1]=='?') && ptr<readResult-sizeof(serial_format)) {
			ptr++;
		}
		serial_format* ptr_to_first_valid = (serial_format*) &incomingData[ptr];

		printf("Actual Steer: %d\n",ptr_to_first_valid->Steer);
		printf("Actual Drive: %d\n",ptr_to_first_valid->Drive);
		printf("Bus Voltage: %f\n", ptr_to_first_valid->Voltage);
		printf("Current on Driving: %f\n", ptr_to_first_valid->CurrentD);
		printf("Current on Steering: %f\n", ptr_to_first_valid->CurrentS);
		printf("Power Consumed on This Unit: %f\n", (ptr_to_first_valid->Voltage)*((ptr_to_first_valid->CurrentD)+(ptr_to_first_valid->CurrentS)));
        //However always read serial data.
#ifdef __linux__
		int errNum = FFupdate(joy,ptr_to_first_valid->Steer);
#endif
    }
    SP->~Serial();
#ifdef __linux__
	SDL_JoystickClose(joy);
#endif
    
	system("pause");
	return 0;
}
コード例 #7
0
 int main( int argc, char** argv )
 {
    VideoCapture cap(0);

    if ( !cap.isOpened() )
    {
         cout << "Cannot open the web cam" << endl;
         return -1;
    }

    Serial* SP = new Serial("COM7");
    if (SP->IsConnected())
		printf("We're connected");

	char outgoingData[1] = "";
	int dataLength = 1;
	int readResult = 0;

	while(SP->IsConnected())
	{

    namedWindow("Control", CV_WINDOW_AUTOSIZE); //create a window called "Control"

  int iLowH = 0;
  int iHighH = 30;

  int iLowS = 150;
  int iHighS = 255;

  int iLowV = 142;
  int iHighV = 255;

 createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179)
 createTrackbar("HighH", "Control", &iHighH, 179);

 createTrackbar("LowS", "Control", &iLowS, 255); //Saturation (0 - 255)
 createTrackbar("HighS", "Control", &iHighS, 255);

 createTrackbar("LowV", "Control", &iLowV, 255);//Value (0 - 255)
 createTrackbar("HighV", "Control", &iHighV, 255);

 int iLastX = 320;
 int iLastY = 240;

 Mat imgTmp;
 cap.read(imgTmp);

 Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );;


    while (true)
    {
        Mat imgOriginal;

        bool bSuccess = cap.read(imgOriginal);

         if (!bSuccess)
        {
             cout << "Cannot read a frame from video stream" << endl;
             break;
        }

    Mat imgHSV;

    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV); //Convert the captured frame from BGR to HSV

    Mat imgThresholded;

    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded); //Threshold the image

    //morphological opening (removes small objects from the foreground)
    erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
    dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );

    //morphological closing (removes small holes from the foreground)
    dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );
    erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) );

    //Calculate the moments of the thresholded image
    Moments oMoments = moments(imgThresholded);

    double dM01 = oMoments.m01;
    double dM10 = oMoments.m10;
    double dArea = oMoments.m00;

    // if the area <= 10000, I consider that the there are no object in the image and it's because of the noise, the area is not zero
    if (dArea > 10000)
    {
        //calculate the position of the ball
        int posX = dM10 / dArea;
        int posY = dM01 / dArea;

    if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
    {
    //Draw a red line from the previous point to the current point
       // line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2);

        Point pt1(posX-10, posY-10);
        Point pt2(posX+10, posY+10);

        rectangle(imgLines, pt1, pt2, cvScalar(0, 255, 0, 0), 1, 8, 0);
    }

        iLastX = posX;
        iLastY = posY;
    }

    Point pt3(100,100);
    Point pt4(520,380);

    rectangle(imgLines, pt3, pt4, cvScalar(0, 0, 255, 0), 1, 8, 0);

            if(iLastX-10 < 100)
                {
                //putText(image, "Your face is on the right side of me!", Point(faces[i].x, faces[i].y - 5), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );
                putText(imgLines, "The object is on the right side of me!", Point(70,90), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );

                outgoingData[0] = 'r';
                if(SP->WriteData(outgoingData, dataLength))

                Sleep(100);

                }

            if(iLastX+10 > 520)
                {
                putText(imgLines, "The object is on the left side of me!", Point(70,90), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );

                outgoingData[0] = 'l';
                if(SP->WriteData(outgoingData, dataLength))

                Sleep(100);

                }

            if(iLastY-10 > 380)
                {
                putText(imgLines, "The object is at the bottom!", Point(70,70), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );

                outgoingData[0] = 'b';
                if(SP->WriteData(outgoingData, dataLength))

                Sleep(100);

                }

            if(iLastY+10 < 100)
                {
                putText(imgLines, "The object is at the top!", Point(70,70), CV_FONT_HERSHEY_PLAIN, 1, Scalar(0,250,0), 1, 8, false );

                outgoingData[0] = 't';
                if(SP->WriteData(outgoingData, dataLength))

                Sleep(100);

                }

    imshow("Thresholded Image", imgThresholded); //show the thresholded image

    imgOriginal = imgOriginal + imgLines;
    imshow("Original", imgOriginal); //show the original image
    imgLines=cv::Scalar(0, 0, 0);

    if (cvWaitKey(33) == 27) //wait for 'esc' key press for 30ms. If 'esc' key is pressed, break loop
       {
            cout << "esc key is pressed by user" << endl;
            return -1;
       }
    }
	}
   return 0;
	}