STM32 ] Dynamixel AX-12A 모터 제어하기 (1)

 

다음 진행할 개인 프로젝트인 델타로봇의 제어를 위해 먼저 AX-12A 모터에 대해 파봤다.

 

다이나믹셀은 모터, 제어기, 드라이버, 센서, 감속기 밑 네트워크 기능을 하나의 모듈로 만든 로봇 전용 액추에이터로, 이전에 STM32보드로 모터를 다뤄본 적이 있지만 PWM 제어하던 그것과는 제어방식이 아주 많이 다르다.

 

 

스펙

 

 

 

 

핀맵

 

 

 

 

 

   

 

특이점은 여러 다이나믹셀 모터를 Daisy Chain 방식으로 연결해 하나의 버스에 패킷을 전송하면, 고유의 ID를 가지고 있는 각각의 모터들을 제어할 수 있다. 브로드캐스트 ID를 사용해 버스에 연결된 모터를 한번에 제어할 수도 있다.

 

 

 

 

 

 

 

그리고 위의 스펙을 보면 모터의 통신방식이 TTL레벨 Half Duplex 시리얼 통신인데 MCU의 RX, TX선과 연결하려면 아래와 같은 통신회로가 필요하다.

 

 

 

 

 

 

내가 사용하는 STM32F429ZI 보드에서는 Half Duxplex 기능을 소프트웨어적으로 지원해서 한번 사용해 보려고 했는데 잘 안되길래 그냥 IC 칩을 구매해 하드웨어 회로를 구성하였다.

 

사용된 칩은 74LS241 로 8개의 Tri-State Buffer 가 있다.

 

 

 

 

Function Table

 

 

다시 통신 회로를 보면 Enable(=Direction)핀이 LOW일 때는 read 상태이고 데이터를 보낼 때만 Enable핀을 HIIGH로 바꿔서 write 한다.

 

 

 

 

 

 

 

배선

74LS241N	STM32
19	Direction Pin 용도, GPIO Output Open Drain, PD7
18	USART2 RX, PA3
17	USART2 TX, PD5

 

 

 

주의할 것은 STM32는 GPIO핀 출력이 3.3V인데 IC칩의 Enable 신호를 주려면 5V 입력이 되야해서 open drain 으로 설정하고 외부에 5V 풀업회로를 꾸며줬다. 

 

 

나중엔 3셀 리포배터리 쓰고 배선도 만능기판에 납땜해서 깔끔하게 만들려고 한다.

 

 

 

아무튼 다음 모터를 구동하기 전에 한 일은 모터의 ID와 Baudrate를 확인해서 Baudrate를 일치시키고 ID가 겹치지 않도록 설정해주는 일이었다. 공장출고 값은 ID 는 1 Baudrate 는 1M인데 내가 사용한게 새 모터가 아니라서 확인이 필요했다. 이 작업은 OpenCM 9.04와 485 확장보드, 아두이노 IDE와 Dynamixel2Arduino 라이브러리의 scan_dynamixel 등 예제를 사용해서 간단하게 처리했다. 

 

총 3개의 AX-12A 를 쓸건데 ID는 0, 1, 2 로 했고 Baudrate는 1M으로 했더니 구동이 안되서 115200으로 내려서 설정했다.

 

 

 

 

 

 

 

다음 모터를 구동하기 위해서 다이나믹셀 1.0 프로토콜을 이용한다.

프로토콜 매뉴얼 : https://emanual.robotis.com/docs/kr/dxl/protocol1/#status-packetreturn-packet

 

 

패킷 종류에는 제어기가 다이나믹셀에게 보내는 명령 데이터인 Instruction Packet 과 다이나믹셀이 Controller로부터 받은 명령을 수행한 뒤 그 결과를 Return 하는 데이터인 Status Packet 이 있다.

 

 

 

 

 

 

 

 

 

 

다음 MCU가 다이나믹셀에게 Return이 있는 명령을 했을 때 다이나믹셀이 보내오는 Status Packet 이다.

 

 

 

 

 

 

 

 

 

Write 테스트

 

 

 

매개변수로 GoalPosition(목표각도) 을 받아서 상위바이트와 하위바이트를 구분하고 브로드캐스트 ID로 이동 명령을 내린다.

 

/* USER CODE BEGIN PFP */
void packet(uint16_t write)
{
	char write1=write&0xFF;
	char write2=(write>>8)&0xFF;

	uint8_t cheak_sum=(~(0xFE + 0x05 + 0x03 + 0x1E + write1 + write2)&0xff);
	uint8_t dynamixelpacket[10]={0xFF , 0xFF , 0xFE , 0x05 , 0x03 , 0x1E ,write1 , write2, cheak_sum};

	//HAL_HalfDuplex_EnableTransmitter(&huart2);
	HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, 1);
	HAL_UART_Transmit(&huart2,dynamixelpacket,9,1000);//packet
	HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, 0);
	//HAL_HalfDuplex_EnableReceiver(&huart2);
}

//...

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {  
	  packet(degrees);
	  HAL_Delay(100);

	  if(reverse == 0)
	  {
		  if(degrees < 681)
		  {
			  degrees += 10;
		  }
		  else
		  {
			  reverse = 1;
		  }
	  }
	  if(reverse == 1)
	  {
		  if(degrees > 512)
		  {
			  degrees -= 10;
		  }
		  else
		  {
			  reverse=0;
		  }
	  }

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }

 

 

 

나중에는 init 함수에서 Moving Speed 를 설정해 놓고 부드럽게 움직이도록 바꿀 예정이다.

 

 

 

 

 

 

Read 테스트

 

 

ID 2로부터 2바이트 길이인 Present Position(메모리주소 36=0x24) 값을 읽기위한 Instruction Packet을 보내면 Status Packet이 들어온다.

 

  uint32_t current_tick=0;
  uint32_t start_tick=0;

//...

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

	  current_tick = HAL_GetTick();
	  if(current_tick - start_tick >= 1000){

		  buf[0]= 0xFF;
		  buf[1]= 0xFF;
		  buf[2]= 0x01;
		  buf[3]= 0x04;
		  buf[4]= 0x02;
		  buf[5]= 0x24;
		  buf[6]= 0x02;
		  buf[7]= ~(buf[2]+buf[3]+buf[4]+buf[5]+buf[6]);

		  printf("\n\r");

		  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, 1);
		  HAL_UART_Transmit(&huart2,&buf[0],8,10);
		  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, 0);

		  start_tick = current_tick;
	  }

	  if(HAL_UART_Receive(&huart2, &rx2_data, 1, 10)==HAL_OK){
		  printf("rx : 0x%x ", rx2_data);
	  }

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }

 

 

 

출력되는 데이터 중 6, 7번째 값이 현재 각도이다.

 

구동중에 manual 로 모터를 돌리려면 먼저 torque off 해줘야 하는데 테스트용이라 그냥 선을 뽑아가면서 했다.

 

일단 Present Position 읽는 것 까진 확인했고 구조부터 다시 짤건데 기존 라이브러리 분석해서 포팅할 계획이다.