튜토리얼 1: 힘/토크 센서

CANbus 노드 및 연결

본 튜토리얼에서는 ROBOTOUS 힘/토크 센서로 CANbus 통신 시스템 인터페이스를 구축하는 내용을 다룹니다. 전체 시스템 연결도와 인터페이스를 구성할 CANbus 장치인 힘/토크 센서와 추가 장치 대한 정보는 아래 표와 같습니다.

종류	설명	수량
ROBOTOUS RFT80-6A01	6축 힘/힘/토크 센서 ∅80×22mm, 210g	1
W1502 케이블	-	1
RS485/CAN 커넥터	-	1

CANbus 시스템 구성 요소

먼저 힘/토크 센서의 입출력 PIN 중에서 CANbus 통신에 필요한 CAN HIGH, CAN LOW, GND, 5V PIN 만을 사용할 것이기 때문에 이를 분류할 수 있는 케이블을 연결해야 합니다. 아래 자료를 참고하여 힘/토크 센서에 E91337 케이블과 W1502 케이블을 연결합니다.

W1502_End Tool Connector.pdf

케이블 작업 후, 아래 그림을 참고하여 STEP 제어기의 CANbus 커넥터와 W1502 의 PIN 맵을 대조하여 요구되는 커넥터에 맞는 케이블을 연결합니다.

STEP CAN port

커넥터와 케이블을 모두 연결하는 것으로 CANbus 통신 시스템 인터페이스 구성을 완료합니다.

CANbus Bitrate 설정

PuTTY 와 같은 SSH 를 이용하여 STEP 에 접속하고, 아래 터미널 명령어를 통해 STEP 에 기본적으로 설치 된 CAN 유틸리티를 확인할 수 있습니다. 각각의 유틸리티에 대한 설명은 STEP 통신 연결하기의 CANbus 에 자세히 설명되어 있습니다.

$ ls -l /usr/bin/*SERCAN*

그 다음, ROBOTOUS 힘/토크 센서를 사용하기 위해서 STEP 과 센서 사이에 연결된 CANbus 통신을 센서의 기본 설정에 맞게 조정해야 하며, 센서 기본 설정은 아래 표와 같습니다.

Item	Default Values		Remarks
Item	CAN	UART	Remarks
Filtering	Filter OFF	Filter OFF
Data Output Rate	200Hz	200Hz
Message ID	Receiver ID: 100(0x64) Transmitter ID #1: 1(0x01) Transmitter ID #2: 2(0x02)	N/A	CAN only
Communication Setting	CAN 2.0 A, B Compatible Identifier: Standard Identifier Bit Rate: 1Mbps Size of Data: 8 bytes	Baud Rate: 115,200bps 1 Stop Bit No Parity No Flow Control Data Length: 8 Bits

Default Setting of Communication
(ROBOTUS INC., 6 Axis Force Torque Sensor RFT Series Installation and Operation Manual)

위 표를 통해 알 수 있듯이, 힘/토크 센서 사용을 위해 아래 명령어로 비트레이트를 1Mbps 로 바꿔주어야 합니다.

$ sudo SERCAN_setBitrate 1M

정상적으로 비트레이트가 업데이트되면 Bit rate updating SUCCESSFULLY! 문구가 출력됩니다.

힘/토크 센서 제어

센서 설정을 완료하면 힘/토크 센서로 CANbus 메시지를 송수신하는 것이 가능합니다. 그러므로 SERCAN_send 명령어로 STEP 과 힘/토크 센서 사이에 CANbus 통신이 제대로 이루어지는지 확인해보도록 합니다. ROBOTOUS 힘/토크 센서의 CANbus 패킷 정의는 다음과 같습니다.

Command	Command ID	No. of Parameters	w/Response Packet	Remarks
Read Model Name	1(0x01)	0	Yes
Read Serial Number	2(0x02)	0	Yes
Read Firmware Version	3(0x03)	0	Yes
Set Communication ID	4(0x04)	3	Yes	CAN only
Read Communication ID	5(0x05)	0	Yes	CAN only
Set Baud-rate	6(0x06)	1	Yes	UART only
Read Baud-rate	7(0x07)	0	Yes
Set Filter	8(0x08)	2	Yes
Read Filter Setting	9(0x09)	0	Yes
Read F/T Data Output (once)	10(0x0A)	0	Yes
Start F/T Data Output	11(0x0B)	0	Yes
Stop F/T Data Output	12(0x0C)	0	No	Available even during data output
Reserved	13(0x0D)	N.A.	N.A.
Reserved	14(0x0E)	N.A.	N.A.
Set Data Output Rate	15(0x0F)	1	Yes
Read Data Output Rate	16(0x10)	0	Yes	Available even during data output
Set Bias	17(0x11)	1	No
Read Count of Overload Occurrence	18(0x12)	0	Yes

Summary of command packets
(ROBOTOUS INC., 6 Axis Force Torque Sensor RFT Series Installation and Operation Manual)

Read Model Name

CANbus 통신이 제대로 이루어지는지 확인하기 위해, 먼저 힘/토크 센서로부터 Model Name 을 받아오는 과정을 진행합니다. PuTTY 를 1개 더 실행하여 아래와 같은 SERCAN_dump 명령어를 입력합니다. 명령어 입력이 완료되면 해당하는 PuTTY 는 힘/토크 센서로부터 CANbus 통신을 수신하는 터미널로 동작하게 됩니다.

$ sudo SERCAN_dump sercan0

아래 표를 확인하여 힘/토크 센서로부터 Model Name 을 받아오는 command 의 패킷을 찾아 (XX 0x00, NULL) STEP 에서 힘/토크 센서로 전송합니다. 이 때, 2개의 PuTTY 터미널 중 수신용 터미널 외 다른 PuTTY 에서 아래와 같은 SERCAN_send 명령어를 사용합니다.

$ sudo SERCAN_send sercan0 064#0100000000000000

Receiver ID	Data Field
Receiver ID	D1	D2	D3	D4	D5	D6	D7	D8
064	01	XX	XX	XX	XX	XX	XX	XX

패킷을 전송하고 나면, 수신용 PuTTY 가 다음과 같은 CANbus 메시지를 출력하는 것을 확인할 수 있습니다.

sercan 0x1  [8] 01 52 46 54 38 30 2D 36
sercan 0x2  [8] 41 30 31 2D 41 00 00 00

ROBOTOUS 힘/토크 센서의 메뉴얼을 참고하면, 이렇게 도착한 패킷은 Model Name command 와 ASCII Code 로 된 Model Name 이 같이 쓰여진 것입니다. Model Name command 인 0x1 노드의 0Byte 자리의 hex 문자를 제외하고 나머지 hex 문자 52 46 54 38 30 2D 36 41 30 31 2D 41 을 ASCII 코드표를 확인해서 텍스트 문자로 변환하면 RFT80-6A01-A 같은 Model Name 이 나오는 것을 확인할 수 있습니다.

이때, CANbus 통신이 원활하게 이루어지지 않는 문제가 발생하면, STEP 의 CAN card 에 위치한 리셋 버튼을 눌러보거나, SERCAN 펌웨어 업데이트 를 참고하여 SERCAN 펌웨어를 업데이트 하십시오.

Start/Stop F/T Data Output

CANbus 통신이 정상적으로 확인되면, 다음으로 힘/토크 센서로부터 Torque 값과 Force 값을 받아오는 과정을 진행해보도록 합니다. 이 과정 역시 2개의 PuTTY 를 실행하여 진행합니다.

아래 표와 명령어를 확인하여 힘/토크 센서로부터 실시간으로 값을 받아오는 command 의 패킷을 찾아 STEP 에서 힘/토크 센서로 전송합니다.

$ sudo SERCAN_send sercan0 064#0B00000000000000

Receiver ID	Data Field
Receiver ID	D1	D2	D3	D4	D5	D6	D7	D8
064	0B	XX	XX	XX	XX	XX	XX	XX

패킷을 전송하고 나면, 수신용 PuTTY 에서 다음과 같은 형식의 CANbus 메시지를 실시간으로 수신받는 것을 확인할 수 있습니다.

sercan 0x1  [8] 0B F9 7F 07 B2 1E B9 FC
sercan 0x2  [8] D4 FB 88 05 53 00 00 00

ROBOTOUS 힘/토크 센서의 메뉴얼을 참고하면, 이렇게 도착한 패킷은 실시간 Sensing command 와 16진수로 표현된 Force 값과 Torque 값이 같이 쓰여진 것입니다. 실시간 Sensing command 인 0x1 노드의 0Byte 자리의 hex 문자를 제외하고, 나머지 16진수들 F9 7F 07 B2 1E B9 FC D4 FB 88 05 53을 10진수로 변환하면, X, Y, Z torque value 와 X, Y, Z force value 를 구할 수 있습니다 (자세한 내용은 아래의 16진수-10진수 변환 방법을 참고하십시오).

힘/토크 센서로부터 실시간으로 Torque 값과 Force 값을 받아오는 것을 멈추고 싶다면, 실행 명령어와 해당 command 의 패킷은 다음과 같습니다.

$ sudo SERCAN_send sercan0 064#0C00000000000000

Receiver ID	Data Field
Receiver ID	D1	D2	D3	D4	D5	D6	D7	D8
064	0C	XX	XX	XX	XX	XX	XX	XX

16진수-10진수 변환 방법

Data Field
D1	D2	D3	D4	D5	D6	D7	D8	D9	D10	D11	D12	D13	D14	D15	D16
ID	R1	R2	R3	R4	R5	R6	R7	R8	R9	R10	R11	R12	R13	XX	XX

힘/토크 센서로부터 받은 패킷은 위의 표와 같은 형식을 갖춥니다. 이 때, 각 데이터 필드는 다음과 같습니다.

R1 : Fx's upper byte, R2 : Fx's lower byte
R3 : Fy's upper byte, R4 : Fy's lower byte
R5 : Fz's upper byte, R6 : Fz's lower byte
R7 : Tx's upper byte, R8 : Tx's lower byte
R9 : Ty's upper byte, R10 : Ty's lower byte
R11 : Tz's upper byte, R12: Tz's lower byte

각각의 upper byte 와 lower byte 에 대해 다음의 연산을 실시하면 해당하는 값의 10진수 값을 얻을 수 있습니다.

이때, RFT80-6A01 모델의 스펙에 따라 DF 는 50, DT 는 1000으로 계산합니다.

CANbus 프로젝트 컴파일

STEP 통신 연결하기의 SERCAN 예제에서는 sercan_test 예제를 살펴봤다면, 여기에서는 힘/토크 센서를 제어할 수 있도록 rtsercan_test 를 변형한 예제에 대해 설명합니다. 해당 예제는 다음과 같습니다.

#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <sys/mman.h>
#include <stdlib.h>

#include <native/task.h>
#include <native/timer.h>
#include <rtdk.h>

#include "NRMKsercan_tp.h"
#include "NRMKhw_tp.h"

TP_PORT test_port=1;

int rtsercan_fd  = -1;

RT_TASK write_task;
RT_TASK read_task;
int working=1;

//task start value
double trigger = 0;

//CANbus data to physical data
unsigned char data_field[16]; // storage buffer for data field
//..... Received CAN data Save .....
// 8 byte data of can message id is #1 save in data_field [0] ~ [7]
// 8 byte data of can message id is #2 save in data_field [8] ~ [15] // data field processing
short raw_data[6] = { 0 };
unsigned short temp;
unsigned DF=50, DT=2000; // DF, DT depend on the model, refer to 3.6.11
double ft_array[6];

void cleanup_all(void)
{
    working=0;
    rt_task_delete(&write_task);
    rt_task_delete(&read_task);
}

void catch_signal(int sig)
//void cath_signal()
{
    cleanup_all();
    return;
}


void write_thread(void* arg)
{
    CAN_FRAME cfame;

    rt_task_set_periodic(NULL, TM_NOW, 1e9);

    while (working)
    {
        //initial setting
        //rt_printf("writing task initial setting completed...  \n");
        cfame.can_id=0x64;
        cfame.can_dlc=8;
        for (int i=0; i<8; ++i) cfame.data[i]=0x00;

        //start sensing
        if(trigger == 0)
        {
            //rt_printf("start sensing...   \n");
            for (int i=0; i<8; ++i){
            if(i==0)
                cfame.data[i]=0x0B;
            else
                cfame.data[i]=0x00;
        }
    }

        RTSERCAN_write(rtsercan_fd, cfame);
        rt_task_wait_period(NULL);
    }

}

void read_task_proc(void *arg)
{
    int res1, res2;
    CAN_FRAME RxFrame1;
    CAN_FRAME RxFrame2;
    rt_task_set_periodic(NULL, TM_NOW, 1e6);
    while (working)
    {
        //Get CANbus Torque value
        res1=RTSERCAN_read(rtsercan_fd, &RxFrame1);
    res2=RTSERCAN_read(rtsercan_fd, &RxFrame2);

        if (res1==SERCAN_ERR_FREE && res2==SERCAN_ERR_FREE)
        {/*
            rt_print_CANFrame(RxFrame1);
            rt_print_CANFrame(RxFrame2);
        */

        //CANbus data to Torque data
        for(int i=0; i<8; ++i)
        {
            data_field[i] = (unsigned char) RxFrame1.data[i];
            data_field[i+8] = (unsigned char) RxFrame2.data[i];
        }

        for (int idx = 0; idx < 6; idx++)
        {
            temp = data_field [2 * idx + 1] * 256;
            temp += data_field [2 * idx + 2];

            raw_data[idx] = (signed short)temp; // variable casting
        }
        // Conversion from signed short data to float data and data scaling

        // Set Force/Torque Original
        for (int n = 0; n < 3; n++)
        {
            ft_array[n] = (((float)raw_data[n] ) / DF); // refer to 3.6.11
            ft_array[n + 3] = (((float)raw_data[n + 3] ) / DT); // refer to 3.6.11
        }

        static unsigned int print_count = 0;
        if (print_count % 1000 == 0)
        {
//          rt_printf("\nX direction Force: %f   \n",   ft_array[0]);
//          rt_printf("Y direction Force: %f    \n",    ft_array[1]);
//          rt_printf("Z direction Force: %f    \n\n",  ft_array[2]);
//          rt_printf("X direction Torque: %f   \n",    ft_array[3]);
//          rt_printf("Y direction Torque: %f   \n",    ft_array[4]);
//          rt_printf("Z direction Torque: %f   \n\n",  ft_array[5]);
            printf("\nX direction Force: %f \n",    ft_array[0]);
            printf("Y direction Force: %f   \n",    ft_array[1]);
            printf("Z direction Force: %f   \n\n",  ft_array[2]);
            printf("X direction Torque: %f  \n",    ft_array[3]);
            printf("Y direction Torque: %   \n",    ft_array[4]);
                printf("Z direction Torque: %   \n\n",  ft_array[5]);
                //print_count = 0;
            }
            print_count++;

            trigger++;
        }
        /*
        if(trigger >= 10)
        {
            rt_printf("stop sensing...  \n");
            CAN_FRAME cfame;
            cfame.can_id=100;
            cfame.can_dlc=8;
            for (int i=0; i<8; ++i)
            {
                if(i==0)
                    cfame.data[i]=12;
                else
                    cfame.data[i]=0;
            }

            RTSERCAN_write(rtsercan_fd, cfame);
            break;
        }
        */
        rt_task_wait_period(NULL);
    }
}

int main(int argc, char* argv[])
{
    signal(SIGTERM, catch_signal);
    signal(SIGINT, catch_signal);

    /* no memory-swapping for this programm */
    mlockall(MCL_CURRENT | MCL_FUTURE);
    printf("preventing memory-swapping for this program...  \n");

    // Perform auto-init of rt_print buffers if the task doesn't do so
    rt_print_auto_init(1);

    // open rtsercan*******************
    rtsercan_fd=RTSERCAN_open();
    if (rtsercan_fd < 0) return 1;

    printf("rtsercan is opened! \n");
    //********************************

    //-------------------------------------------------------

    printf("creating RT_task... \n");

    rt_task_create(&write_task, "write_task", 0, 99, 0);
    rt_task_start(&write_task, &write_thread, NULL);

    printf("write task created... \n");

    rt_task_create(&read_task, "read_task", 0, 99, 0);
    rt_task_start(&read_task, &read_task_proc, NULL);

    printf("read task created... \n");

    pause();

return 0;

}

위와 같이, rtsercan_test 예제의 main.cpp 파일을 위의 코드로 된 main.cpp 파일로 교환하는 것만으로도 힘/토크 센서로부터 값을 받아오는 프로젝트를 완성할 수 있습니다. 이후, 실행을 위해 프로젝트 컴파일을 진행해야하며, 이 과정은 EtherCAT CORE 예제의 8. EtherCAT 프로젝트 컴파일 과정과 동일합니다.

CANbus 어플리케이션 실행

CANbus 어플리케이션을 실행은 EtherCAT 어플리케이션과 동일하게 WinSCP 프로그램을 통해 파일을 STEP 에 전송하여 진행합니다. 실행 방법에 대한 자세한 내용은 7. EtherCAT 어플리케이션 실행 을 참고하십시오.

Data Field
D1	D2	D3	D4	D5	D6	D7	D8	D9	D10	D11	D12	D13	D14	D15	D16
ID	R1	R2	R3	R4	R5	R6	R7	R8	R9	R10	R11	R12	R13	XX	XX

Data Field
D1	D2	D3	D4	D5	D6	D7	D8	D9	D10	D11	D12	D13	D14	D15	D16
ID	R1	R2	R3	R4	R5	R6	R7	R8	R9	R10	R11	R12	R13	XX	XX

Data Field
D1	D2	D3	D4	D5	D6	D7	D8	D9	D10	D11	D12	D13	D14	D15	D16
ID	R1	R2	R3	R4	R5	R6	R7	R8	R9	R10	R11	R12	R13	XX	XX