#include <Aria.h>

int main(int argc, char** argv)
{
	Aria::init();
	ArRobot robot;
	ArSonarDevice sonar;
	ArTcpConnection con;
	robot.addRangeDevice(&sonar);
	con.setPort("localhost");
	robot.setDeviceConnection(&con);
	if (!robot.blockingConnect())
	{
		printf("\nNie wykryto uruchomionej symulacji robota!\n\n");
		system("pause");
		Aria::exit(0);
	}
	robot.runAsync(true);
	printf("\nWykonywanie symulacji...\n\n");
	robot.enableMotors();

	//////////////////////////////////////////////////////////////////////////
	robot.setTransVelMax(2500);                // Ustawienie maksymalnej predkosci liniowej na 80 cm/s
	robot.setRotVelMax(30);                   // Ustawienie maksymalnej predkosci katowej na 15 st./s
	int i = 0;

	robot.setDeltaHeading(0);            // Skret o 180 stopni w lewo
	while (1)                             // Petla czekajaca na wykonanie skretu
	{
		if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
		{
			break;
		}
		ArUtil::sleep(200);
	}
	robot.setVel(-2500);                    // Zadanie robotowi ruchu liniowego o predkosci 50 cm/s do przodu
	while (robot.getSonarRange(11) > 200)  // Sprawdzenie pomiarow czujnika nr 3
	{
		ArUtil::sleep(100);
	}
	robot.setVel(0);
	robot.setDeltaHeading(-90);            // Skret o 180 stopni w lewo
	while (1)                             // Petla czekajaca na wykonanie skretu
	{
		if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
		{
			break;
		}
		ArUtil::sleep(200);
	}

	/*robot.setVel(-2500);                    // Zadanie robotowi ruchu liniowego o predkosci 50 cm/s do przodu
	while (robot.getSonarRange(11) > 100)  // Sprawdzenie pomiarow czujnika nr 3
	{
		ArUtil::sleep(100);
	}

	robot.setVel(2500);                    // Zadanie robotowi ruchu liniowego o predkosci 50 cm/s do przodu
	while (robot.getSonarRange(3) > 200)  // Sprawdzenie pomiarow czujnika nr 3
	{
		ArUtil::sleep(100);
	}
	robot.setVel(0);*/

	while (1)//głowny while
	{
		i++;
		robot.setVel(5000);                    // Zadanie robotowi ruchu liniowego o predkosci 50 cm/s do przodu
		while (robot.getSonarRange(3) > 300)  // Sprawdzenie pomiarow czujnika nr 3
		{
			ArUtil::sleep(100);
		}
		robot.setVel(0);

		/*robot.move(1000);                     // Ruch liniowy o 1 m do przodu
		while (1)                             // Petla czekajaca na wykonanie ruchu liniowego
		{
			if (robot.isMoveDone())           // Warunek wykonania zadanego ruchu liniowego
			{
				break;
			}
			ArUtil::sleep(100);               // Petla czekajaca 100 ms
		}*/

		if (i % 2 == 0) {
			robot.setDeltaHeading(-90);            // Skret o 90 stopni w lewo
			while (1)                             // Petla czekajaca na wykonanie skretu
			{
				if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
				{
					break;
				}
				ArUtil::sleep(100);
			}
		}
		else
		{
			robot.setDeltaHeading(90);            // Skret o 90 stopni w lewo
			while (1)                             // Petla czekajaca na wykonanie skretu
			{
				if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
				{
					break;
				}
				ArUtil::sleep(100);
			}
		}
		if (robot.getSonarRange(3) > 400) {
			robot.move(400);                     // Ruch liniowy o 0,4 m do przodu
		}
		while (1)                             // Petla czekajaca na wykonanie ruchu liniowego
		{
			if (robot.isMoveDone())           // Warunek wykonania zadanego ruchu liniowego
			{
				break;
			}
			ArUtil::sleep(100);               // Petla czekajaca 100 ms
		}

		if (i % 2 == 0) {
			robot.setDeltaHeading(-92);            // Skret o 90 stopni w lewo
			while (1)                             // Petla czekajaca na wykonanie skretu
			{
				if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
				{
					break;
				}
				ArUtil::sleep(100);
			}
		}
		else
		{
			robot.setDeltaHeading(92);            // Skret o 90 stopni w lewo
			while (1)                             // Petla czekajaca na wykonanie skretu
			{
				if (robot.isHeadingDone())        // Warunek wykonania zadanego skretu
				{
					break;
				}
				ArUtil::sleep(100);
			}
		}
		/*robot.setVel(2500);                    // Zadanie robotowi ruchu liniowego o predkosci 50 cm/s do przodu
		while (robot.getSonarRange(3) > 300)  // Sprawdzenie pomiarow czujnika nr 3
		{
			ArUtil::sleep(100);
		}*/
		robot.stop();                         // Zatrzymanie wszelkich ruchow robota
	}
	//////////////////////////////////////////////////////////////////////////

	printf("\nZakonczenie symulacji.\n\n");
	Aria::exit(0);
	return 0;
}
#include <Aria.h>
#include <iostream>

using namespace std;

int main(int argc, char** argv)
{
	Aria::init();
	ArRobot robot;
	ArSonarDevice sonar;
	ArTcpConnection con;
	robot.addRangeDevice(&sonar);
	con.setPort("localhost");
	robot.setDeviceConnection(&con);
	if (!robot.blockingConnect())
	{
		printf("\nNie wykryto uruchomionej symulacji robota!\n\n");
		system("pause");
		Aria::exit(0);
	}
	robot.runAsync(true);
	printf("\nWykonywanie symulacji...\n\n");
	robot.enableMotors();

	//////////////////////////////////////////////////////////////////////////
	robot.setTransVelMax(1000);                // Ustawienie maksymalnej predkosci liniowej na 80 cm/s
	robot.setRotVelMax(25);                   // Ustawienie maksymalnej predkosci katowej na 15 st./s


	while (1) {

		robot.lock();
		// int a = robot.getSonarRange(3);
		//cout << "Info z sonaru nr3: " << robot.getSonarRange(3) << endl;
		robot.move(200);
		robot.unlock();
		ArUtil::sleep(200);
		if (robot.getSonarRange(0) <= 250*2) {
			//Manewr omijajacy:
			printf("\nA\n");
			robot.lock();
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2); // Odczekanie 50 ms
		}
		else if (robot.getSonarRange(1) <= 300) {
			printf("\nD\n");
			robot.lock();
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
		else if (robot.getSonarRange(2) <= 400) {
			robot.lock();
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
		else if (robot.getSonarRange(3) <= 750) {
			robot.lock();
			printf("\nO\n");
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
		else if (robot.getSonarRange(4) <= 750) {
			robot.lock();
			printf("\nL\n");
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
		else if (robot.getSonarRange(5) <= 400) {
			robot.lock();
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000/2);
		}
		else if (robot.getSonarRange(6) <= 300) {
			robot.lock();
			printf("\nF\n");
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
		else if (robot.getSonarRange(7) <= 250) {
			robot.lock();
			robot.setDeltaHeading(-90); // Skret o 90 stopni
			robot.unlock();
			ArUtil::sleep(2000 / 2);
		}
	}

	//////////////////////////////////////////////////////////////////////////

	printf("\nZakonczenie symulacji.\n\n");
	Aria::exit(0);
	return 0;
}