본문 바로가기
Enginius/Robotics

Robot Simualtor - Grid, Spiral, As a whole

해리s 2011. 2. 10. 14:16



기존의 방식에서 내부적으로 가상의 GRID를 만들고, 로봇이 이를 인식해서 이동할 수 있게 함.
위의 알고리즘은 벽이나 이전에 갔던 GRID를 만나면 좌로 회전하고 그렇지 않을 경우는 직진하게 한 것이다.
(가장 간단한 Spiral Algorithm)

내부적으로 다음과 같은 step을 갖는다. 

 

// STEP 정리 (uGB_Step)
#define	START			0
#define	MANAGER			1

#define	MOVE_RIGHT		11
#define	MOVE_LEFT		12
#define	MOVE_UPWARD		13
#define	MOVE_DOWNWARD	14
#define	MOVE_FORWARD	15
#define	TURN_RIGHT		21
#define	TURN_LEFT		22
#define	TURN_BACKWARD	23

#define MOVEMENT		101
#define TURN			102
#define END				200


// WallFront 의 인자 정리
#define CHECK_WALL		1
#define CHECK_GRID		2


// 로봇의 Heading(현재 방향) 정리
#define HEADING_RIGHT	1
#define HEADING_UP		2
#define HEADING_LEFT	3
#define HEADING_DOWN	4

 

위에서부터 MOVE 단계는 시뮬레이터를 보는 것을 기준으로 상하좌우로 한 GRID SIZE만큼 이동하는 것이다.
TURN 단계는 처음 시작 reference각도를 기준으로 90도씩 좌, 우로 회전하는 단계이다.

로봇의 현재 헤딩을 기준으로 앞에 벽이나 이전에 갔던 그리드가 있는지 확인하는 함수는 다음과 같다. 

Bool ROBOT::WallFront(int type)
{
	if(IR[1] < 280 && (type & CHECK_WALL))
	{
		return True;
	}
	if(type & CHECK_GRID)
	{
		if(nGM_Heading == HEADING_RIGHT && nGD_GRID[nGM_Grid_X + 1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_UP && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + 1 + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_LEFT && nGD_GRID[nGM_Grid_X - 1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_DOWN && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y - 1 + MAX_HALF_GRID_NUMBER])
			return True;
	}
	return False;
}

Bool ROBOT::WallRight(int type)
{
	if(IR[12] < 280 && (type & CHECK_WALL))
	{
		return True;
	}
	if(type & CHECK_GRID)
	{
		if(nGM_Heading == HEADING_RIGHT && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y -1 + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_UP && nGD_GRID[nGM_Grid_X +1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_LEFT && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y +1 + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_DOWN && nGD_GRID[nGM_Grid_X -1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
	}
	return False;
}

Bool ROBOT::WallLeft(int type)
{
	if(IR[13] < 280 && (type & CHECK_WALL))
	{
		return True;
	}
	if(type & CHECK_GRID)
	{
		if(nGM_Heading == HEADING_RIGHT && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y +1 + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_UP && nGD_GRID[nGM_Grid_X -1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_LEFT && nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y -1 + MAX_HALF_GRID_NUMBER])
			return True;
		if(nGM_Heading == HEADING_DOWN && nGD_GRID[nGM_Grid_X +1 + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER])
			return True;
	}
	return False;
}
실제로 로봇이 움직여야 할 알고리즘이 들어가는 부분은 
	case MANAGER:
		DriveFastStop();
		if(WallFront(CHECK_GRID | CHECK_WALL))
		{
			DriveFastStop();
			uGM_Step = TURN_LEFT;
		}
		else
		{
			uGM_Step = MOVE_FORWARD;
		}
		break;
위의 부분 안이다. 현재 들어가 있는 코드는 앞에 벽이나 이전에 갔던 그리드가 있으면 왼쪽으로 90도 회전하고, 그렇지 않을 경우에는 한 그리드 씩 앞으로 가는 것이다. 이를 구현하기 위해서 전체 소스는 다음과 같다. 
Bool ROBOT::GridMove()
{

	CString Debug_STR;

	switch(uGM_Step)
	{
	case START:
// ...
// 위에 생략
// ...
	case MOVE_RIGHT:
		nGM_Grid_X++;
		fGM_Stage_X = fGM_Start_Ref_X + nGM_Grid_X * grid_size * cos(fGM_Start_Ref_Theta) - nGM_Grid_Y * grid_size * sin(fGM_Start_Ref_Theta);
		fGM_Stage_Y = fGM_Start_Ref_Y + nGM_Grid_X * grid_size * sin(fGM_Start_Ref_Theta) + nGM_Grid_Y * grid_size * cos(fGM_Start_Ref_Theta);
		uGM_Step = MOVEMENT;
		nGM_Heading = HEADING_RIGHT;

		Debug_STR.Format("RIGHT_MOVE X:%d, Y:%d", nGM_Grid_X, nGM_Grid_Y);
		Debug_Print(Debug_STR);
		break;

	case MOVE_LEFT:
		nGM_Grid_X--;
		fGM_Stage_X = fGM_Start_Ref_X + nGM_Grid_X * grid_size * cos(fGM_Start_Ref_Theta) - nGM_Grid_Y * grid_size * sin(fGM_Start_Ref_Theta);
		fGM_Stage_Y = fGM_Start_Ref_Y + nGM_Grid_X * grid_size * sin(fGM_Start_Ref_Theta) + nGM_Grid_Y * grid_size * cos(fGM_Start_Ref_Theta);
		uGM_Step = MOVEMENT;
		nGM_Heading = HEADING_LEFT;

		Debug_STR.Format("LEFT_MOVE X:%d, Y:%d", nGM_Grid_X, nGM_Grid_Y);
		Debug_Print(Debug_STR);
		break;

	case MOVE_UPWARD:
		nGM_Grid_Y++;
		fGM_Stage_X = fGM_Start_Ref_X + nGM_Grid_X * grid_size * cos(fGM_Start_Ref_Theta) - nGM_Grid_Y * grid_size * sin(fGM_Start_Ref_Theta);
		fGM_Stage_Y = fGM_Start_Ref_Y + nGM_Grid_X * grid_size * sin(fGM_Start_Ref_Theta) + nGM_Grid_Y * grid_size * cos(fGM_Start_Ref_Theta);
		uGM_Step = MOVEMENT;
		nGM_Heading = HEADING_UP;

		Debug_STR.Format("UPWARD_MOVE X:%d, Y:%d", nGM_Grid_X, nGM_Grid_Y);
		Debug_Print(Debug_STR);
		break;

	case MOVE_DOWNWARD:
		nGM_Grid_Y--;
		fGM_Stage_X = fGM_Start_Ref_X + nGM_Grid_X * grid_size * cos(fGM_Start_Ref_Theta) - nGM_Grid_Y * grid_size * sin(fGM_Start_Ref_Theta);
		fGM_Stage_Y = fGM_Start_Ref_Y + nGM_Grid_X * grid_size * sin(fGM_Start_Ref_Theta) + nGM_Grid_Y * grid_size * cos(fGM_Start_Ref_Theta);
		uGM_Step = MOVEMENT;
		nGM_Heading = HEADING_DOWN;

		Debug_STR.Format("DOWNWARD_MOVE X:%d, Y:%d", nGM_Grid_X, nGM_Grid_Y);
		Debug_Print(Debug_STR);
		Debug_STR.Format("X: %.1f Y: %.1f / toX: %.1f toY: %.1f", X, Y, fGM_Stage_X, fGM_Stage_Y);
		Debug_Print(Debug_STR);
		break;

	case MOVE_FORWARD:
		if(nGM_Heading == HEADING_RIGHT)
			nGM_Grid_X++;
		else if(nGM_Heading == HEADING_DOWN)
			nGM_Grid_Y--;
		else if(nGM_Heading == HEADING_LEFT)
			nGM_Grid_X--;
		else if(nGM_Heading == HEADING_UP)
			nGM_Grid_Y++;
		fGM_Stage_X = fGM_Start_Ref_X + nGM_Grid_X * grid_size * cos(fGM_Start_Ref_Theta) - nGM_Grid_Y * grid_size * sin(fGM_Start_Ref_Theta);
		fGM_Stage_Y = fGM_Start_Ref_Y + nGM_Grid_X * grid_size * sin(fGM_Start_Ref_Theta) + nGM_Grid_Y * grid_size * cos(fGM_Start_Ref_Theta);
		uGM_Step = MOVEMENT;
		break;

	case MOVEMENT:
		if(toPoint(fGM_Stage_X, fGM_Stage_Y, 25))
		{
			Debug_STR.Format("MOVEMENT done");
			Debug_Print(Debug_STR);
			nGD_GRID[nGM_Grid_X + MAX_HALF_GRID_NUMBER][nGM_Grid_Y + MAX_HALF_GRID_NUMBER] = 1;
			uGM_Step = MANAGER;
		}
		break;

	case TURN_RIGHT:
		if(nGM_Heading == HEADING_RIGHT)
			nGM_Heading = HEADING_DOWN;
		else if(nGM_Heading == HEADING_DOWN)
			nGM_Heading = HEADING_LEFT;
		else if(nGM_Heading == HEADING_LEFT)
			nGM_Heading = HEADING_UP;
		else if(nGM_Heading == HEADING_UP)
			nGM_Heading = HEADING_RIGHT;
		uGM_Step = TURN;
		break;

	case TURN_LEFT:
		if(nGM_Heading == HEADING_RIGHT)
			nGM_Heading = HEADING_UP;
		else if(nGM_Heading == HEADING_DOWN)
			nGM_Heading = HEADING_RIGHT;
		else if(nGM_Heading == HEADING_LEFT)
			nGM_Heading = HEADING_DOWN;
		else if(nGM_Heading == HEADING_UP)
			nGM_Heading = HEADING_LEFT;
		uGM_Step = TURN;
		break;

	case TURN_BACKWARD:
		if(nGM_Heading == HEADING_RIGHT)
			nGM_Heading = HEADING_LEFT;
		else if(nGM_Heading == HEADING_DOWN)
			nGM_Heading = HEADING_UP;
		else if(nGM_Heading == HEADING_LEFT)
			nGM_Heading = HEADING_RIGHT;
		else if(nGM_Heading == HEADING_UP)
			nGM_Heading = HEADING_DOWN;
		uGM_Step = TURN;
		break;

	case TURN:
		if(nGM_Heading == HEADING_RIGHT)
		{
			fGM_Stage_Theta = Standarize_Rad(fGM_Start_Ref_Theta + 0, ZERO_to_2PI);
		}
		else if(nGM_Heading == HEADING_DOWN)
		{
			fGM_Stage_Theta = Standarize_Rad(fGM_Start_Ref_Theta - PI/2, ZERO_to_2PI);
		}
		else if(nGM_Heading == HEADING_LEFT)
		{
			fGM_Stage_Theta = Standarize_Rad(fGM_Start_Ref_Theta - PI, ZERO_to_2PI);
		}
		else if(nGM_Heading == HEADING_UP)
		{
			fGM_Stage_Theta = Standarize_Rad(fGM_Start_Ref_Theta + PI/2, ZERO_to_2PI);
		}


		if(TurnTo(fGM_Stage_Theta))
		{
			uGM_Step = MANAGER;
		}
		break;

	case END:
		DriveFastStop();
		break;

	default:

		break;
	}


	return False;
}
이렇게 하면 된다. 이동 함수는 다른 곳에서 정의되어 있다.
이러한 이동함수는 실제 로봇의 이동함수의 프로토콜과 같아서 포팅이 매우 쉽다.

'Enginius > Robotics' 카테고리의 다른 글

e-puck H/W  (0) 2011.08.09
Robot Simulator - BSA (Backtracking Spiral Algorithm)  (0) 2011.02.13
Robot Simulator - Non grid, Divie and Conquer, Spiral  (0) 2011.02.10
Robots / Boston Dynamics  (4) 2009.10.30
plen - humanoid robot  (0) 2009.10.30

'Enginius/Robotics' Related Articles

티스토리툴바