STM32 ] 델타로봇과 MFC 연동 + 컨베이어 벨트 추가 (4)

 

 

 

 

 

 

컨베이어 벨트 추가

 

컨베이어 벨트에 달린 모터는 델타 로봇에 쓰인 것과 동일한 모델인 AX-12A 모터이다. Daisy Chain으로 연결하고 ID를 3으로 지정하고, Baudrate를 115200으로 맞춰줬다.

 

처음엔 모터 혼이 저 큰 구멍에 위치하게 배치되어 있었는데 테스트 구동을 해보니 장력 부족으로 벨트가 잘 돌아가지 않는 것 같아 뒤로 당겨서 배치해주었다.

 

 

#define AX_CONVEYOR_ID				3

 

 

dxl_control.c

void setEndless(uint8_t ID, uint8_t State)
{
	if ( State )
	{
		uint8_t length = 9;
		uint8_t packet[length];

	    packet[0] = AX_HEADER;
	    packet[1] = AX_HEADER;
	    packet[2] = ID;
	    packet[3] = length-4;
	    packet[4] = AX_WRITE;
	    packet[5] = ADDR_CCW_ANGLE_LIMIT_L;
	    packet[6] = 0; 						// full rotation
	    packet[7] = 0;						// full rotation
	    uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
	    packet[8] = Checksum;

	    sendInstPacket(packet, length);
	}
	else	// use torque off instead
	{
		turn(ID,0,0);

		uint8_t length = 9;
		uint8_t packet[length];

	    packet[0] = AX_HEADER;
	    packet[1] = AX_HEADER;
	    packet[2] = ID;
	    packet[3] = length-4;
	    packet[4] = AX_WRITE;
	    packet[5] = ADDR_CCW_ANGLE_LIMIT_L;
	    packet[6] = 255;					// 1023 low
	    packet[7] = 3;						// 1023 high
	    uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
	    packet[8] = Checksum;

	    sendInstPacket(packet, length);
	}
}

void turn(uint8_t ID, uint8_t SIDE, uint16_t Speed)
{
		if (SIDE == LEFT)
		{

		    uint8_t Speed_L = Speed;
		    uint8_t Speed_H = Speed >> 8;		// 16 bits - 2 x 8 bits variables

			uint8_t length = 9;
			uint8_t packet[length];

		    packet[0] = AX_HEADER;
		    packet[1] = AX_HEADER;
		    packet[2] = ID;
		    packet[3] = length-4;
		    packet[4] = AX_WRITE;
		    packet[5] = ADDR_GOAL_SPEED_L;
		    packet[6] = Speed_L;
		    packet[7] = Speed_H;
		    uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
		    packet[8] = Checksum;

		    sendInstPacket(packet, length);
		}

		else
		{

		    uint8_t Speed_L = Speed;
		    uint8_t Speed_H = (Speed >> 8) + 4;		// 16 bits - 2 x 8 bits variables

			uint8_t length = 9;
			uint8_t packet[length];

		    packet[0] = AX_HEADER;
		    packet[1] = AX_HEADER;
		    packet[2] = ID;
		    packet[3] = length-4;
		    packet[4] = AX_WRITE;
		    packet[5] = ADDR_GOAL_SPEED_L;
		    packet[6] = Speed_L;
		    packet[7] = Speed_H;
		    uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7])) & 0xFF;
		    packet[8] = Checksum;

		    sendInstPacket(packet, length);
		}
}

 

라이브러리를 참고해 setEndless 함수와 turn 함수를 가져왔다. 먼저 setEndless 함수로 해당 모터를 바퀴 모드로 전환하고 turn 함수에서 Moving Speed를 설정해 움직인다.

 

 

예를 들어, 512로 설정된 경우 CW 방향으로 회전하며 최대 출력 대비 약 50%로 돌아가게 할 수 있다.

 

 

 

 

void syncWriteTorqueOnOff(uint8_t State){

    uint8_t length = 14;
    uint8_t packet[length];

    packet[0] = AX_HEADER;
    packet[1] = AX_HEADER;
    packet[2] = AX_BROADCAST_ID;
    packet[3] = length-4;
    packet[4] = AX_SYNC_WRITE;
    packet[5] = ADDR_TORQUE_ENABLE;
    packet[6] = 0x01;		// length of data to access

    packet[7] = 0;			// ID 0
    packet[8] = State;

    packet[9] = 1;			// ID 1
    packet[10] = State;

    packet[11] = 2;			// ID 2
    packet[12] = State;
    uint8_t Checksum = (~(packet[2] + packet[3] + packet[4] + packet[5] + packet[6] + packet[7]
						+ packet[8] + packet[9] + packet[10] + packet[11] + packet[12])) & 0xFF;
    packet[13] = Checksum;

    sendInstPacket(packet, length);
}

 

그리고 그동안 Torque On/Off 기능을 사용할 때 브로드캐스트 ID를 사용했었는데, 델타로봇 Torque를 On/Off할 때 컨베이어 벨트의 Torque 도 같이 On/Off 되면 곤란하니 따로 SyncWrite 명령을 쓰는 버전 함수를 만들어 줬다.

 

 

 

freertos.c

	  if(ConveyorRightOnSemHandle != NULL)
	  {
		  if(osSemaphoreWait(ConveyorRightOnSemHandle, 0) == osOK)
		  {
			  setEndless(AX_CONVEYOR_ID, ON);
			  turn(AX_CONVEYOR_ID, RIGHT, 600);
		  }
	  }
	  if(ConveyorLeftOnSemHandle != NULL)
	  {
		  if(osSemaphoreWait(ConveyorLeftOnSemHandle, 0) == osOK)
		  {
			  setEndless(AX_CONVEYOR_ID, ON);
			  turn(AX_CONVEYOR_ID, LEFT, 600);
		  }
	  }
	  if(ConveyorOffSemHandle != NULL)
	  {
		  if(osSemaphoreWait(ConveyorOffSemHandle, 0) == osOK)
		  {
			  onOffTorque(AX_CONVEYOR_ID, OFF);
			  //setEndless(AX_CONVEYOR_ID, OFF);
		  }
	  }

 

	  if(PumpOnSemHandle != NULL)
	  {
		  if(osSemaphoreWait(PumpOnSemHandle, 0) == osOK)
		  {
			  HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 0);
		  }
	  }
	  if(PumpOffSemHandle != NULL)
	  {
		  if(osSemaphoreWait(PumpOffSemHandle, 0) == osOK)
		  {
			  HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 1);
		  }
	  }

 

더 이상 기능을 추가하기 전에 구조를 뜯어고치고 싶기도 하고 취업으로 인해 시간이 없어서 다음글은 늦게 올라올 수도 있을 것 같다.