Пример #1
0
int main()
{

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

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

	FILE * gFile;
	xFile = fopen("geral.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;


	// Config Vision

	CvRect ROI = cvRect(25, 49, 295, 184);
	robot canto_area1 = robot(49, 35);
	robot canto_area2 = robot(244, 146);
	robot canto_gol1 = robot(23, 59);
	robot canto_gol2 = robot(276, 126);
	int amin = 23;
	int amax = 173;
	int radius = 20;
	int min_distance = 12;
	int parametro[36] = { 88, 139, 80, 121, 253, 255,
		21, 62, 114, 60, 243, 255,
		146, 98, 164, 180, 162, 255,
		55, 189, 91, 94, 229, 255,
		3, 102, 83, 17, 199, 229,
		0, 85, 135, 21, 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(1);
	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 Controle00((float)0.2, (float)0.007, (float)6, (float)0.3, (float)0.000001, (float)4);
	controle Controle00(param0);
	controle Controle01(param1);
	controle Controle02(param2);
	Controle00.distance_lim = 50;
	Controle01.distance_lim = 50;
	Controle02.distance_lim = 50;

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


	start = clock();
	while (1)
	{

		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();

		//if (Camera.players[0].x != -1)
		//	fprintf(xFile, "%f, %i\n", Camera.players[0].x * (184 / ROI.width), int(visionF - start));
		//if (Camera.players[0].y != -1)
		//	fprintf(yFile, "%f, %i\n", Camera.players[0].y * (95 / ROI.height), int(visionF - start));
		//if (Camera.players[0].x != -1)
		//	fprintf(thetaFile, "%f, %i\n", Camera.players[0].theta, int(visionF - start));
		//if (Camera.players[0].x != -1)
		//	fprintf(gFile, "%f,%f,%f %i\n", Camera.players[0].x * (184 / ROI.width), Camera.players[0].y * (95 / ROI.height), Camera.players[0].theta, int(visionF - start));

		// =========================== 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 (!paused)
		{
			//SP->send(0, 0, 15);
			SP->send(0, Controle00.v.x, Controle00.v.y);
			whishPath.push_back(robot(Estrategia.players[0].goal_x, Estrategia.players[0].goal_y));
			followedPath.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);
			}
		}

		for (int i = 0; i < 3; i++)
		{
			if (Estrategia.players[i].x != -1)
				cvLine(Camera.frame, cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), cvPoint((int)Estrategia.players[i].goal_x, (int)Estrategia.players[i].goal_y), CV_RGB(255, 255, 255), 3);
		}

		for (int n = 0; n < 3; n++)
		{
			if (Estrategia.path[n].size() > 1)
			{
				for (int i = 0; i < (Estrategia.path[n].size() - 1); i++)
				{
					//cout << "( " << Estrategia.path[n][i].x << ", " << Estrategia.path[n][i].y << " ) ; " << endl;
					cvLine(Camera.frame, cvPoint((int)Estrategia.path[n][i].x, (int)Estrategia.path[n][i].y), cvPoint((int)Estrategia.path[n][i + 1].x, (int)Estrategia.path[n][i + 1].y), CV_RGB(debug_colors[n][0], debug_colors[n][1], debug_colors[n][2]), 2);
				}
			}
		}
		for (int i = 0; i < 3; i++)
		{
			if (Estrategia.players[i].x != -1)
			{
				int comportamento = Estrategia.players[i].comportamento;
				switch (comportamento)
				{
				case(1) :
					cvPutText(Camera.frame, "G", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
				//case(2) :
				//	cvPutText(Camera.frame, "A", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
				//case(3) :
				//	cvPutText(Camera.frame, "L", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
				//case(4) :
				//	cvPutText(Camera.frame, "Z", cvPoint((int)Estrategia.players[i].x, (int)Estrategia.players[i].y), &fonte, CV_RGB(debug_colors[i][0], debug_colors[i][1], debug_colors[i][2]));
				}
			}
		}

		if (whishPath.size() > 1)
		{
			for (int i = 0; i < (whishPath.size() - 1); i++)
			{
				//cout << "( " << Estrategia.path[n][i].x << ", " << Estrategia.path[n][i].y << " ) ; " << endl;
				cvLine(Camera.frame, cvPoint((int)whishPath[i].x, (int)whishPath[i].y), cvPoint((int)whishPath[i + 1].x, (int)whishPath[i + 1].y), CV_RGB(221, 0, 0, 2));			//Vermelho
				cvLine(Camera.frame, cvPoint((int)followedPath[i].x, (int)followedPath[i].y), cvPoint((int)followedPath[i + 1].x, (int)followedPath[i + 1].y), CV_RGB(94, 83, 138, 2));		//Roxo
			}
		}

		cvShowImage("Live", Camera.frame);



		//Escape Sequence
		char_pressed = cvWaitKey(1);
		if (char_pressed == 27)
		{
			SP->send(0, 0, 0);
			SP->send(1, 0, 0);
			SP->send(2, 0, 0);
			break;
		}
		if (char_pressed == 112)
		{
			if (paused)
				paused = false;
			else
				paused = true;
		}
		if (char_pressed == 99)
		{
			whishPath.clear();
			followedPath.clear();
		}
		


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

		//if (paused)
		//	cout << "Paused" << endl;
		//else
			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 << "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 << "posicao: "<< camera.players[0].x << "," << camera.players[0].y <<endl;
		//cout << counter << endl;
		//system("cls");
	}
}
Пример #2
0
int main(){
	init();
		usb.send(PINA & 0x1F);

	MoteurManager engines = MoteurManager();
	


    enum States {
        AVANCER,
        ADJUST_INTER,
        TURN_LEFT,
        TURN_RIGHT
    };

    // Set the initial state
    States state = AVANCER;
		
	for(;;) {

 		switch (state) {
            case AVANCER:
				engines.avancer(140);

				// 00100
				// 01100
				// 00110
				if (((PINC & 0x1F) == 0x04) || ((PINC & 0x1F) == 0x06) || ((PINC & 0x1F) == 0x0C))  {
					// PORTA = 0x0;
					engines.adjustcenter();
					_delay_ms(10);
				}

				// 00010
				// 00001
				// 00011
				if (((PINC & 0x1F) == 0x01) || ((PINC & 0x1F) == 0x03) || ((PINC & 0x1F) == 0x02)) {
					//PORTA = 0x2;
					engines.adjustToLeft();
					_delay_ms(10);
				}

				// 11000
				// 10000
				// 01000
				if (((PINC & 0x1F) == 0x18) || ((PINC & 0x1F) == 0x10) || ((PINC & 0x1F) == 0x08)) {
				// PORTA = 0x1;
					engines.adjustToRight();
					_delay_ms(10);
				}

				// 11111
				// 00111
				// 11100
				// 01111
				// 11110
				if (((PINC & 0x1F) == 0x1F) || ((PINC & 0x1F) == 0x07) || ((PINC & 0x1F) == 0x1C) ||((PINC & 0x1F) == 0x0F)||((PINC & 0x1F) == 0x1E)) {
					_delay_ms(15);
		
					if(((PINC & 0x1F) == 0x1F) || ((PINC & 0x1F) == 0x07) || ((PINC & 0x1F) == 0x1C) ||((PINC & 0x1F) == 0x0F)||((PINC & 0x1F) == 0x1E)) {
						state = ADJUST_INTER;
					}
				}
				break;
			case ADJUST_INTER:
				engines.avancer(160);
				_delay_ms(1000);
				state = TURN_LEFT;

			case TURN_LEFT:
				
				engines.arreter();
				_delay_ms(500);
				engines.tournerGauche(40);

				while ((PINC & 0x1F) == 0x00) {
					//engines.tournerGauche(1);
					engines.adjustToLeft();
					
				}

				state = AVANCER;

				break;

			case TURN_RIGHT:
				engines.arreter();
				_delay_ms(500);
				engines.tournerDroite(45);

				while ((PINC & 0x1F) == 0x00) {
					//engines.tournerDroite(1);
					engines.adjustToRight();
				}

				state = AVANCER;

				break;


				break;
		}
	}

	return 0;
}