Skip to content

튜토리얼 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 케이블을 연결합니다.

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


STEP CAN port

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

CANbus Bitrate 설정

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

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

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

Item Default Values Remarks
CAN UART
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 로 바꿔주어야 합니다.

1
$ 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 통신을 수신하는 터미널로 동작하게 됩니다.

1
$ sudo SERCAN_dump sercan0

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

1
$ sudo SERCAN_send sercan0 064#0100000000000000

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

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

1
2
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 에서 힘/토크 센서로 전송합니다.

1
$ sudo SERCAN_send sercan0 064#0B00000000000000

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

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

1
2
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 의 패킷은 다음과 같습니다.

1
$ sudo SERCAN_send sercan0 064#0C00000000000000

Receiver ID Data Field
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 를 변형한 예제에 대해 설명합니다. 해당 예제는 다음과 같습니다.

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
#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 어플리케이션 실행 을 참고하십시오.