[AmoroRobot2] 서보모터 이용한 로봇팔 실험

[AmoroRobot2] 서보모터 이용한 로봇팔 실험

-물체를 들기 위해 로봇팔을 내리는 모습

-물체를 들어 올리는 모습

소스코드

invalid-file

//=============================================================================

// Test Program - DX Motor Robot Arm Test
//
// 
// [arm.c]
//=============================================================================

/*
 - DX-Motor를 이용하여 로봇팔 제어 예제
 - Ez-RoboMaster의 AI Motor포트 DX-Motor(2번째 라인)에 DX-모터를 연결한다.
 - Dx-Motor은 3개를 이용고 ID가 1~3까지 사용한다.
 - 시리얼 포트로 'u', 'd', 's'를 전송하면 로봇팔을 올리거나 내리는 동작을 한다.
 - 시리얼 포트로 'g'를 전송하면 현제 모터의 위치값을 표시한다.
 - 표시된 모터위치 값을 이용하여 다른 동작을 추가할 수 있다.
*/

//Ez-RoboMaster API 헤드파일
#include "c:\winavr\ezrobomaster\ezrmlib.h"
#include "dx_arm.h"

unsigned int m_PosUp[3] = {394, 2, 859};
unsigned int m_PosScan = {400, 544, 340};
unsigned int m_PosDown[3] = {561, 428, 254};
unsigned int m_PosCurrent[3];

//Arm의 상태를 설정하는 함수
void ArmStatus(unsigned char Status)
{
	switch(Status)
	{
		case ARM_STATUS_SCAN:
			//Arm을 물체 검출 위치로 이동
			amr_MovePos(400, 544, 340);
			break;
					
		case ARM_STATUS_UP:
			//물체를 들어 올릴 위치로 이동
			arm_Move(m_PosUp);
			break;
			
		case ARM_STATUS_DOWN:
			//물체를 집거나 내려 놓는 위치로 이동
			arm_Move(m_PosScan);
			Delay(1500);
			arm_Move(m_PosDown);
			break;
			
		default :
			return ;
	}						
}

void DisPlayArmPos(void)
{
	unsigned int pos[3];
	
	//모터3개의 위치를 얻어와서
	arm_GetArmPos(pos);
	
	printf("M1=%d, M2=%d, M3=%d\r\n", pos[0], pos[1], pos[2]);
		
}
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
//Main 프로그램
int main(void)
{
	unsigned char data = 0;
	
	//Ez-RoboMaster초기화
	SysInit();

	InitArm();
	
	LcdClr(LINE_ALL);
	LcdDspStr(0,0, "ArmTest ");
			
	while(1)
	{
		//시이얼 포트로 데이터가 전송되면
		if(SrlKbhit())
		{
			data = SrlRcvByte();
			printf("\r\n");
			switch(data)
			{
				case 'u':
					printf("Arm Up\r\n");
					ArmStatus(ARM_STATUS_UP);
					break;
					
				case 'd':
					printf("Arm Down\r\n");
					ArmStatus(ARM_STATUS_UP);
					break;
					
				case 's':
					printf("Arm Scan\r\n");
					ArmStatus(ARM_STATUS_SCAN);
					break;					
					
				case 'g':
					DisPlayArmPos();
					break;
			}
			
			printf("Press [u], [d], [g], key>");
			Delay(100);
		}		

	}
	
	return 1;
}
//-----------------------------------------------------------------------------