로봇은 관절(Joint)에 붙어있는 모터로 조인트 각도를 제어하여 움직인다. 그리고 실제 그리퍼가 붙어 주어진 작업을 수행하는 말단 장치(End Effector)의 위치는 조인트 각도로부터 결정된다. 조인트 각도와 엔드 이펙터의 위치, 이 둘 사이를 변환하는 계산 과정을 기구학 이라고 한다.
이 중 정기구학(Forward Kinematics)은 일련의 조인트 각도가 주어졌을 때 엔드 이펙터의 직교좌표상 위치를 구하는 과정이고 역기구학(Inverse Kinematics)은 엔드 이펙터의 직교좌표상 위치에 대응하는 조인트 각도를 구하는 과정이다.
https://www.marginallyclever.com/other/samples/fk-ik-test.html
Delta Robot Forward/Inverse Kinematics Calculations
www.marginallyclever.com
위 홈페이지에 들어가서 Base radius (f), Bicep length (rf), Forearm length (re), End Effector radius (e) 등 의 설정 값을 입력하면 아래에서 정기구학과 역기구학의 결과를 간단하게 확인할 수 있다.
위 그림에서 base의 좌표를 0,0,0 으로 본다. 역기구학 계산 결과 End Effector의 좌표가 0,0,-256.984 일 때 모터 1, 2, 3의 θ 값이 전부 0이 된다. 모터의 각도 θ는 Base와 Bicep이 평행할 때 0이고, Base 기준으로 Bicep이 위로 들리면 negative 값, 아래로 내려오면 positive 값을 갖는다.
'd' or 'a' : x축 제어
'w' or 's' : y축 제어
't' or 'b' : z 축 제어
테스트용으로 한 번 구현해 보았고 지금은 코드에서 삭제해놨다.
진공펌프 제어를 추가한 버전. 사실 내가 기대했던 건 물건을 흡착해서 옆으로 던져버리는 그림이었는데 속도가 빠르지 않고 가동범위가 크지 앉아서 그런지 원하던 그림이 안나왔다. 이건 테스트를 거쳐서 수정할 예정이다.
진공펌프
아두이노 ] 릴레이 모듈 + 흡입펌프 다루기
릴레이 모듈 Relay module 릴레이 모듈이란 낮은 전압/전류로 더 높은 전압/전류를 스위칭 할 수있게끔 해주는 부품이다. 릴레이의 원리 : 모듈 내부에 전류가 흐르면 자기장이 형성되는 전자석(철
eteo.tistory.com
예전에 진공펌프 다루는 글을 써둔게 있어서 여기서 따로 정리하지는 않겠다.
다만 펌프의 GND를 보드와 같은 GND로 묶으면 자꾸 보드를 리부팅시켜서 전원을 파워서플라이를 사용해 따뤄줬다. 그리고 STM32보드는 GPIO핀 출력이 3.3V라서 릴레이 모듈의 신호부 VCC도 3.3V에 연결했다.
소스코드
main.c
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart->Instance==USART3){
if(rx3_data=='1'){
printf("\r\n");
osSemaphoreRelease(UPSemHandle);
}else if(rx3_data=='2'){
printf("\r\n");
osSemaphoreRelease(DOWMSemHandle);
}else if(rx3_data=='3'){
printf("\r\n");
osSemaphoreRelease(ReadPosSemHandle);
}else if(rx3_data=='4'){
printf("\r\n");
osSemaphoreRelease(TorqueOffSemHandle);
}else if(rx3_data=='5'){
printf("\r\n");
osSemaphoreRelease(TorqueOnSemHandle);
}else if(rx3_data=='6'){
printf("\r\n");
osSemaphoreRelease(ThrowSemHandle);
}
HAL_UART_Receive_IT(&huart3, &rx3_data, 1);
}
}
PC로부터 UART를 통해 수신된 문자가 위에 해당하는 경우 인터럽트 내에서 세마포어 시그널을 쏘는 부분이다. 원래 FreeRTOS API를 쓰면 ISR 내부가 아닌 경우 xSemaphoreGive() 함수를 쓰고 ISR 안에서는 xSemaphoreGiveFromISR() 를 쓰고 portEND_SWITCHING_ISR(pdFALSE)를 해줘야 하는데 CMSIS-RTOS API인 osSemaphoreRelease 함수는 내부에 아래처럼 정의되어 있어서 그냥 쓰면 된다.
dxl_control.c
void syncWriteGoalPosition(uint16_t P0, uint16_t S0, uint16_t P1, uint16_t S1, uint16_t P2, uint16_t S2){
uint8_t P0_L = P0;
uint8_t P0_H = P0 >> 8;
uint8_t P1_L = P1;
uint8_t P1_H = P1 >> 8;
uint8_t P2_L = P2;
uint8_t P2_H = P2 >> 8;
uint8_t S0_L = S0;
uint8_t S0_H = S0 >> 8;
uint8_t S1_L = S1;
uint8_t S1_H = S1 >> 8;
uint8_t S2_L = S2;
uint8_t S2_H = S2 >> 8;
uint8_t length = 23;
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_GOAL_POSITION_L;
packet[6] = 0x04; // length of data to access
packet[7] = 0; // ID 0
packet[8] = P0_L;
packet[9] = P0_H;
packet[10] = S0_L;
packet[11] = S0_H;
packet[12] = 1; // ID 1
packet[13] = P1_L;
packet[14] = P1_H;
packet[15] = S1_L;
packet[16] = S1_H;
packet[17] = 2; // ID 1
packet[18] = P2_L;
packet[19] = P2_H;
packet[20] = S2_L;
packet[21] = S2_H;
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] + packet[13]
+ packet[14] + packet[15] + packet[16] + packet[17] + packet[18] + packet[19]
+ packet[20] + packet[21])) & 0xFF;
packet[22] = Checksum;
sendInstPacket(packet, length);
}
그리고 dxl_control.c 파일에 Sync Write 기능을 구현한 함수를 추가해 놨다. 매개변수로 3개의 모터의 포지션과 속도값을 전달 받아서 한번에 패킷을 보내 제어할 수 있다. 만든 이유는 모터의 이동 각도에 비례해서 각각 모터의 속도를 조절하기 위함이다.
참고로 access할 데이터의 길이는 4byte이고, 접근할 메모리의 시작 주소는 30(0x1E)이다.
델타로봇 기구학 라이브러리는 아래 출처에서 그대로 가져와 포팅했다.
출처 : https://github.com/tinkersprojects/Delta-Kinematics-Library
DeltaKinematics.c
void ServoConversion() {
GP[0] = (uint16_t)(((ThetaA + 147.9) / 0.29)+0.5);
GP[1] = (uint16_t)(((ThetaB + 147.9) / 0.29)+0.5);
GP[2] = (uint16_t)(((ThetaC + 147.9) / 0.29)+0.5);
}
void setCoordinates(double x, double y, double z) {
C.x = x;
C.y = y;
C.z = z;
}
void resetCoordinates(double x, double y, double z) {
C.x += x;
C.y += y;
C.z += z;
}
추가한건 별거 없다. 지금은 디버깅 편이를 위해 다 전역변수로 해놨는데 나중에 수정할 생각도 있다.
그리고 위에서 모터의 각도 θ는 Base와 Bicep이 평행할 때 0이라고 했는데, 이건 기구학 계산시의 얘기고 실제 AX-12A 모터의 포지션 값을 읽어보니 Base와 Bicep이 평행할 때 510이 나왔다.
아래 사진을 보면 모터는 관절모드로 움직일 때 0도에서 300도 사이를 움직일 수 있고 10비트의 분해능(Resolution)을 가진다. 따라서 포지션 값의 단위는 300/1024 = 0.29° 이고, 포지션 값 510은 147.9°를 뜻한다.
위의 ServoConverstion() 함수가 역기구학으로 계산된 모터의 각도 Theta를 실제 모터에 명령하기 위한 포지션값으로 변환하는 과정이다. 마지막에 +0.5 해준 것은 (uint16_t) 캐스팅시 내림이 되니까 반올림 해주기 위함이다.
freertos.c
typedef struct
{
float mX;
float mY;
float mZ;
int maxSpeed;
uint8_t timing;
}queueMessage;
메시지큐 구조체 정의한 부분. 원래 좌표를 double 형으로 선언해놨는데 스피드와 타이밍을 추가하려니 자꾸 hard fault 오류가 나서 좌표 변수를 float로 바꿔줬다. RTOS를 사용하려면 heap 과 stack 사이즈 설정을 신중히 해야 하는 것 같다.
/* USER CODE END Variables */
osThreadId defaultTaskHandle;
osThreadId calWritePosTaskHandle;
osMessageQId setQueueHandle;
osSemaphoreId ReadPosSemHandle;
osSemaphoreId TorqueOnSemHandle;
osSemaphoreId TorqueOffSemHandle;
osSemaphoreId UPSemHandle;
osSemaphoreId DOWMSemHandle;
osSemaphoreId ThrowSemHandle;
/* Private function prototypes -----------------------------------------------*/
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
deltaInit();
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* Create the semaphores(s) */
/* definition and creation of ReadPosSem */
osSemaphoreDef(ReadPosSem);
ReadPosSemHandle = osSemaphoreCreate(osSemaphore(ReadPosSem), 1);
/* definition and creation of TorqueOnSem */
osSemaphoreDef(TorqueOnSem);
TorqueOnSemHandle = osSemaphoreCreate(osSemaphore(TorqueOnSem), 1);
/* definition and creation of TorqueOffSem */
osSemaphoreDef(TorqueOffSem);
TorqueOffSemHandle = osSemaphoreCreate(osSemaphore(TorqueOffSem), 1);
/* definition and creation of UPSem */
osSemaphoreDef(UPSem);
UPSemHandle = osSemaphoreCreate(osSemaphore(UPSem), 10);
/* definition and creation of DOWMSem */
osSemaphoreDef(DOWMSem);
DOWMSemHandle = osSemaphoreCreate(osSemaphore(DOWMSem), 10);
/* definition and creation of ThrowSem */
osSemaphoreDef(ThrowSem);
ThrowSemHandle = osSemaphoreCreate(osSemaphore(ThrowSem), 10);
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
for(int i=0; i<10; i++){
osSemaphoreWait(UPSemHandle, 0);
osSemaphoreWait(DOWMSemHandle, 0);
osSemaphoreWait(ThrowSemHandle, 0);
}
osSemaphoreWait(ReadPosSemHandle, 0);
osSemaphoreWait(TorqueOnSemHandle, 0);
osSemaphoreWait(TorqueOffSemHandle, 0);
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* Create the queue(s) */
/* definition and creation of setQueue */
osMessageQDef(setQueue, 10, queueMessage);
setQueueHandle = osMessageCreate(osMessageQ(setQueue), NULL);
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 512);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* definition and creation of calWritePosTask */
osThreadDef(calWritePosTask, cal_Write_Pos_Task, osPriorityNormal, 0, 512);
calWritePosTaskHandle = osThreadCreate(osThread(calWritePosTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
}
중간에 osSemaphoreWait() 함수를 써준 것은 CMSIS-API를 쓰면 내 의도와는 다르게 세마포어 Initial Count가 Max Count와 동일하게 생성되기 때문이다. Free RTOS API를 쓰는 방법도 있겠지만 그냥 위의 방법으로 해결 했다.
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
/* USER CODE BEGIN StartDefaultTask */
/* Infinite loop */
for(;;)
{
if(UPSemHandle != NULL)
{
if(osSemaphoreWait(UPSemHandle, 0) == osOK)
{
queueMessage smsg;
smsg.mX=0;
smsg.mY=0;
smsg.mZ=-256.984;
smsg.maxSpeed=100;
smsg.timing=0;
osMessagePut(setQueueHandle, (uint32_t)&smsg, 100);
// upEndEffector();
}
}
if(DOWMSemHandle != NULL)
{
if(osSemaphoreWait(DOWMSemHandle, 0) == osOK)
{
queueMessage smsg;
smsg.mX=0;
smsg.mY=0;
//smsg.mZ=-380.724;
smsg.mZ=-437.724;
smsg.maxSpeed=100;
smsg.timing=1;
osMessagePut(setQueueHandle, (uint32_t)&smsg, 100);
// downEndEffector();
}
}
if(ReadPosSemHandle != NULL)
{
if(osSemaphoreWait(ReadPosSemHandle, 0) == osOK)
{
osThreadSetPriority(defaultTaskHandle, osPriorityAboveNormal);
uint8_t buf[30];
for(int i = 0; i < 3; i++){
memset(buf,0,sizeof(buf));
sprintf((char*)buf, "ID %d's Position : %d\r\n", i, getPresentPosition(i));
HAL_UART_Transmit(&huart3, buf, sizeof(buf), 1000);
}
osThreadSetPriority(defaultTaskHandle, osPriorityNormal);
}
}
if(TorqueOffSemHandle != NULL)
{
if(osSemaphoreWait(TorqueOffSemHandle, 0) == osOK)
{
onOffTorque(AX_BROADCAST_ID, OFF);
}
}
if(TorqueOnSemHandle != NULL)
{
if(osSemaphoreWait(TorqueOnSemHandle, 0) == osOK)
{
onOffTorque(AX_BROADCAST_ID, ON);
}
}
if(ThrowSemHandle != NULL)
{
if(osSemaphoreWait(ThrowSemHandle, 0) == osOK)
{
queueMessage smsg;
smsg.mX=0;
smsg.mY=-130;
smsg.mZ=-200;
smsg.maxSpeed=500;
smsg.timing=2;
osMessagePut(setQueueHandle, (uint32_t)&smsg, 100);
}
}
}
/* USER CODE END StartDefaultTask */
첫번째 Task에는 UART 커맨드를 받아서 변경할 좌표값을 큐에 넣는 부분, 현재 각도를 읽는 부분, Torque On/Off 하는 부분 등이 있다.
특히 각도를 읽는 부분은 Task를 나누기 시작하니까 자꾸 중간에 오류가 나서 각도를 읽는동안은 우선권을 높여줬다.
/* USER CODE END Header_cal_Write_Pos_Task */
void cal_Write_Pos_Task(void const * argument)
{
/* USER CODE BEGIN cal_Write_Pos_Task */
/* Infinite loop */
for(;;)
{
osEvent setEvent;
setEvent = osMessageGet(setQueueHandle, osWaitForever);
if(setEvent.status == osEventMessage)
{
queueMessage *msgp;
msgp = setEvent.value.p;
queueMessage msg = *(msgp);
printf("X : %lf \r\n",msg.mX);
printf("Y : %lf \r\n",msg.mY);
printf("Z : %lf \r\n",msg.mZ);
printf("Speed : %d \r\n",msg.maxSpeed);
setCoordinates((double)msg.mX,(double)msg.mY,(double)msg.mZ);
uint16_t GPBefore[3];
memcpy(GPBefore, GP, sizeof(uint16_t)*3);
inverse();
ServoConversion();
uint16_t diff[3];
diff[0]=abs(GP[0]-GPBefore[0]);
diff[1]=abs(GP[1]-GPBefore[1]);
diff[2]=abs(GP[2]-GPBefore[2]);
uint16_t max = (diff[0] > diff[1] && diff[0] > diff[2]) ? diff[0] : (diff[1] > diff[2]) ? diff[1] : diff[2];
double speed[3]={0,};
speed[0]=((double)diff[0]/max)*msg.maxSpeed;
speed[1]=((double)diff[1]/max)*msg.maxSpeed;
speed[2]=((double)diff[2]/max)*msg.maxSpeed;
syncWriteGoalPosition(GP[0],(uint16_t)speed[0],GP[1],(uint16_t)speed[1],GP[2],(uint16_t)speed[2]);
servoDelay(10);
if(msg.timing==2){
HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 0);
}
servoDelay(990);
if(msg.timing==1){
HAL_GPIO_WritePin(GPIOG, GPIO_PIN_0, 1);
}
}
}
/* USER CODE END cal_Write_Pos_Task */
}
두번 째 Task는 메시지를 수신하면 메시지 내용을 출력하고 setCoordinates() 함수로 전역변수인 좌표값 설정하고 GPBefore 배열에 모터의 이전 포지션 값을 저장해 놓는다.
inverse(); 와 ServoConversion(); 함수로 좌표에 따른 모터 각도 값을 구하고 그 값을 포지션 값으로 변환한다.
diff 배열에는 abs() 함수를 써서 모터가 움직여야할 절대 포지션 차이를 구한다.
다음 삼항연산자로 포지션 차이 중 제일 큰 값을 max 변수에 저장하고, 가장 많이 움직여야되는 모터가 max speed로 움직이고, 나머지는 비율대로 speed 설정을 해서 목표위치에 도달하는 데 같은 시간이 걸리도록 하려고 했다.
소수점 계산을 위해 (double) 로 캐스팅하고 뒤에 (uint16_t)로 변환해 사용하였다.
다만, 이부분은 모터의 speed가 0으로 설정되었을 때 속도 제어를 전혀 하지 않고 모터의 최대 rpm을 사용한다는 점을 고려할 때 좀 수정해야 할 것 같다. 나중에 이부분은 함수화해서 깔끔하게 작성할거다.
Pin G0은 진공펌프랑 연결된 릴레이모듈을 제어하는 부분이다.
이후 구현하고자 하는 부분은 Torque Off하고 manual로 엔드이펙터를 옮겼을 때 정기구학을 사용해서 현재 각도 뿐만아니라 좌표도 동시에 읽는것. 그리고 MFC를 사용해 윈도우 어플리케이션으로 제어하는 프로그램을 만드는 것. 나중에 OpenCV랑 합쳐 객체 검출하고 자동 분류하기 위해 프로토콜 및 설계 구조를 좀 효율적으로 바꿔놓는 것 등을 생각하고 있다.
'DSP, MCU > STM32 (ARM Cortex-M)' 카테고리의 다른 글
| STM32 ] 델타로봇과 MFC 연동 + 컨베이어 벨트 추가 (4) (0) | 2022.08.28 |
|---|---|
| STM32 ] USB CDC (Virtual Port Com) 사용하기 (0) | 2022.08.26 |
| STM32 ] UART DMA, IDLE Line Interrupt 를 통한 데이터 수신 (Ring Buffer) (0) | 2022.08.18 |
| STM32 ] Dynamixel AX-12 사용 - 델타로봇 제어하기 (2) (0) | 2022.08.11 |
| STM32 ] Dynamixel AX-12A 모터 제어하기 (1) (7) | 2022.08.11 |
