今回はDobotのReal-time feedback Portを利用しDobot CR5の現在状態を受信します。Client側はBeckhoff TwinCAT3とTF6310 TCP/IP Functionを使用します。よろしくお願いします。

Thanks!

この記事が出来上がるのはベッコフ日本法人ベッコフオートメーション株式会社さま・さまから機材を貸してくださったおかけです。誠にありがとうございます。

ベッコフ日本法人ベッコフオートメーション株式会社

IPC6920-005はベッコフ日本法人ベッコフオートメーション株式会社さまが貸してくださったものです。Beckhoff Automationは1980 年会社設立、PCベースの制御技術をベースにしたオープンオートメーションシステム導入の先頭に立つドイツ企業です。

ベッコフ日本法人ベッコフオートメーション株式会社は、2011年に横浜に本社、2017年に名古屋オフィスを設立しました。

こちらはベッコフ日本法人ベッコフオートメーション株式会社様のホームページです。

どうぞよろしくお願いします。

Address

横浜オフィス（本社）

ベッコフオートメーション株式会社

〒231-0062

神奈川県横浜市中区桜木町1-1-8

日石横浜ビル18階

HP

https://www.beckhoff.com/ja-jp/

株式会社フレアオリジナル

この記事ができあがったのは株式会社フレアオリジナルがDOBOT貸してくださったおかけです。本当にありがとうございます。フレアオリジナルは長野県にあるエンジニアリング会社で、協働ロボットDOBOTとDH-ROBOTICSの正規代理店です。

株式会社フレアオリジナルさまは様々な分野で事業を展開しております。

産業用・協働ロボットシステムの設計・製造・構想、設計、制作・導入・運用保守
ロボット特別教育
ROBOTICS LAB（XR開発)
VRのご提案

フレアオリジナルさまが自社の高い技術力を使って仮想空間と現実空間を繋がり、私達人間の限界を突破し、ロボティクス活動を続けていくでしょう。

Address

〒389-0601

長野県埴科郡坂城町坂城9439-5

株式会社フレアオリジナル

HP

https://www.flareoriginal.com/

https://roboticslabo.com/

Reference Link

Beckhoff#TwinCAT TF6310_Build a TCP Client

Dobot#Play with Dobot CR5 Part1_Your First Step!

Dobot#Part2_CR5のIO端子

Dobot#Part3_ツールを使ってみよう

Dobot Side

Real-time Feedback Port

DobotのControllerではReal Time Feedback Portがありまして、他のデバイスからDobotの状態を周期で受け取ることができます。

使用できるPortは以下です：

30004=8ms周期で接続先にデータを送信する(1440Bytes)
30005=200ms周期で接続先にデータを送信する(1440Bytes)
30006=Defaultが50msで接続先にデータを送信する(1440Bytes)

Check the Connections

実際TwinCATをDobot ControllerのReal-Time feedback Portと接続する前に、まずDootStudio Proを立ち上げ>Help>Debugging Toolを起動します。

Net Toolというツールがあり、Dobot Controllerから直接送受信できます。

TCP Client Tabを選び、Server IP AddressとServer Portを入力します。

今回はLANで接続しているので、IPは192.168.5.1で、Portは30005を使用します。

ConnectをクリックしDobot Controllerと接続します。

Dobot Controllerから返答が来ましたが、データがみえませんね。

一回DisconnectボタンをクリックしDobot ControllerのConnectionと切断します。

hexadecimal ReceiveのCheckboxをいれます。

もう一回接続すると、名前データが帰ってきました。これで接続OKって確認できました。

Robot Mode

Real-time feedback Portから返したデータの中にRobotの現在の状態を示す変数があります。

Mode	Description
1	ROBOT_MODE_INIT
2	ROBOT_MODE_BRAKE_OPEN
3
4	ROBOT_MODE_DISABLED
5	ROBOT_MODE_ENABLE
6	ROBOT_MODE_BACKDRIVE
7	ROBOT_MODE_RUNNING
8	ROBOT_MODE_RECORDING
9	ROBOT_MODE_ERROR
10	ROBOT_MODE_PAUSE
11	ROBOT_MODE_JOG

Brake Status

Real-time feedback Portから返したデータの中にBrakeの現在の状態を示す変数があります。

0x01=6軸目のBrakeがリリースされた
0x02=5軸目のBrakeがリリースされた
0x03=6軸目と5軸目のBrakeがリリースされた

のようなイメージで、6軸のBrake状態をBit単位で表現します。なので、

Bit0=Joint6
Bit1=Joint5
Bit2=Joint4
Bit3=Joint3
Bit4=Joint2
Bit5=Joint1

Joint Mode

Real-time feedback Portから返したデータの中にJoint Modeの現在の状態を示す変数があります。

8=Position Mode
10=Torque Mode

Robot Type

Real-time feedback Portから返したデータの中に現在のロボット種類を示す変数があります。

Mode	Description
1	MG400
2	M1Pro
3	CR3
31	CR3L
5	CR5
7	CR7
10	CR10
12	CR12
16	CR16

Beckhoff Side

Add Reference

今回使用するのはTF6310のTc2_TcpIpライブラリです。プロジェクトにそのライブラリを追加してください。

DUT

DUT_Dobot_CR_RealtimeFeedback

こちらはDobot TCP/IP通信仕様書に提供されたデータ構造に合わせて構造体を作り、合計1440Bytesです。

TYPE DUT_Dobot_CR_RealtimeFeedback :
STRUCT
MessageSize :UINT; //0-1,Total message length in bytes
Rev1 :ARRAY[1..3]OF UINT; //2-7
DigitalInputs :ULINT; //8-15,Current state of the digital inputs.
DigitalOutputs :ULINT; //16-23
RobotMode :ULINT; //24-31
TimeStamp :ULINT; //32-39,TimeStamp in ms
Rev2 :ULINT; //40-47
TestValue :ULINT; //48-55
Rev3 :LREAL; //56-63
SpeedSacling :LREAL; //64-71,Speed scaling of the trajectory CR\MG 0071 limiter
LinearMomentumNorm :LREAL; //72-79,Norm of Cartesian linear momentum(specific hardware version)
VMain :LREAL; //80-87,Masterboard: Main voltage
VRobot :LREAL; //88-95,Masterboard: Robot voltage (48V)
IRobot 　　　　　　　　　　:LREAL; //96-103,Masterboard: Robot current
Rev4 :ARRAY[1..2]OF LREAL; //104-119
ToolAcceleroMeter 　　　 :ARRAY[1..3]OF LREAL; //120-143,Tool x,y and z accelerometer values(specific hardware version)
ElbowPosition :ARRAY[1..3]OF LREAL; //144-167,Elbow position(specific CR 0167 hardware version)
ElbowVelocity :ARRAY[1..3]OF LREAL; //167-191,Elbow velocity(specific hardware version)
QTarget :ARRAY[1..6]OF LREAL; //192-239,Target joint positions
QDTarget :ARRAY[1..6]OF LREAL; //240-287,Target joint velocities
QDDTarget :ARRAY[1..6]OF LREAL; //288-335,Target joint accelerations
ITarget :ARRAY[1..6]OF LREAL; //336-383,Target joint currents
MTarget :ARRAY[1..6]OF LREAL; //384-431,Target joint moments (torques)
QActual :ARRAY[1..6]OF LREAL; //432-479,Actual joint positions
QDActual :ARRAY[1..6]OF LREAL; //480-527,Actual joint velocities
IActual :ARRAY[1..6]OF LREAL; //528-575,Actual joint currents
ActualTCPForce :ARRAY[1..6]OF LREAL; //576-623,CP sensor value (calculated by CR\MG 0623 six-axis force)
ToolVectorActual 　　　 :ARRAY[1..6]OF LREAL; //624-671,Actual Cartesian coordinates of the tool
TCPSpeedActual :ARRAY[1..6]OF LREAL; //672-719,Actual speed of the tool given in CR\MG 0719 Cartesian coordinates
TCPForce :ARRAY[1..6]OF LREAL; //720-767,TCP force value (calculated by CR\MG joint current)
ToolVectorTarget 　　　:ARRAY[1..6]OF LREAL; //768-815,Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz),
TCPSpeedTarget :ARRAY[1..6]OF LREAL; //816-863,Target speed of the tool given in CR\MG Cartesian coordinates
MotorTemperatures 　　　:ARRAY[1..6]OF LREAL; //864-911,Temperature of each joint in CR 0911 degrees celsius
JointModes :ARRAY[1..6]OF LREAL; //912-959,Joint control modes
VActual :ARRAY[1..6]OF LREAL; //960-1007,Actual joint voltages
HandType :ARRAY[1..4]OF BYTE; //1008-1011,Hand Type
User :BYTE; //1012,
Tool :BYTE; //1013,
RunQueuedCmd :BYTE; //1014,
PauseCmdFlag :BYTE; //1015,
VelocityRatio :BYTE; //1016,
AccelerationRatio 　　　 :BYTE; //1017,
JerkRatio :BYTE; //1018,
XYZVelocityRatio :BYTE; //1019,
RVeolcityRatio :BYTE; //1020,
XYZAccelerationRatio :BYTE; //1021,
RAccelerationRatio :BYTE; //1022,
XYZJerkRatio :BYTE; //1023,
RJerkRatio :BYTE; //1024,
BrakeStatus :BYTE; //1025,
EnableStatus :BYTE; //1026,
DragStatus :BYTE; //1027,
RunningStatus :BYTE; //1028,
ErrorStatus :BYTE; //1029,
JogStatusCR :BYTE; //1030,
CRRobotType :BYTE; //1031,
DragButtonSignal :BYTE; //1032,
EnableButtonSignal :BYTE; //1033,
RecordButtonSignal :BYTE; //1034,
ReappearButtonSignal :BYTE; //1035,
JawButtonSignal :BYTE; //1036,
SixForceOnline :BYTE; //1037,
Rev5 :ARRAY[1..82]OF BYTE; //1038-1119,
MActual :ARRAY[1..6]OF LREAL; //1120-1167,Actual torque
Load :LREAL; //1168-1175,Payload weight
CenterX :LREAL; //1176-1183,Eccentric distance in X direction
CenterY :LREAL; //1184-1191,Eccentric distance in Y direction
CenterZ :LREAL; //1192-1199,Eccentric distance in Z direction
UserCoordinate :ARRAY[1..6]OF LREAL; //1120-1247,User coordinate
ToolCoordinate :ARRAY[1..6]OF LREAL; //1248-1295,Tool coordinate
TraceIndex :LREAL; //1296-1303,Track playback running index
SixForceValue :ARRAY[1..6]OF LREAL; //1304-1351,Six – axis force original value
TargetQuaternion :ARRAY[1..4]OF LREAL; //1352-1383,Target quaternion [qw,qx,qy,qz]
ActualQuaternion :ARRAY[1..4]OF LREAL; //1384-1415,Actual quaternion[qw,qx,qy,qz] CR
Rev6 :ARRAY[1..24]OF BYTE; //1416-1440

END_STRUCT
END_TYPE

uDUT_Dobot_CR_RealtimeFeedback

次はUNIONを使用し、1440長さのByte配列と先ほど定義したDUT_Dobot_CR_RealtimeFeedbackを同じメモリOFFESTするように定義します。

TYPE uDUT_Dobot_CR_RealtimeFeedback :
UNION
_raw :ARRAY[0..1439]OF BYTE;
data :DUT_Dobot_CR_RealtimeFeedback;
END_UNION
END_TYPE

e_Dobot_CR_RealtimeFeedback_RobotMode

こちらはDobotのオペレーションモードです。

{attribute ‘qualified_only’}
{attribute ‘strict’}
TYPE e_Dobot_CR_RealtimeFeedback_RobotMode :
(
NOP := 0
,Init:=1
,BrakeOpen:=2
,Disable:=4
,Enable:=5
,Drag:=6
,Run:=7
,DragRecord:=8
,Alarm:=9
,Pause:=10
,Jogging:=11

);
END_TYPE

e_Dobot_CR_RealtimeFeedback_RobotType

次は接続しているロボットの型式です。

{attribute ‘qualified_only’}
{attribute ‘strict’}
TYPE e_Dobot_CR_RealtimeFeedback_RobotType :
(
Nop := 0
,CR3:=3
,CR3L:=31
,CR5:=5
,CR7:=7
,CR10:=10
,CR12:=12
,CR16:=16
,MG400:=1
,M1Pro:=2
);
END_TYPE

Functions

FC_Dobot_Get_RobotType

こちらのFunctionはリアルタイムデータからもらったロボットタイプ変数に該当するロボット型式を戻しします。

FUNCTION FC_Dobot_Get_RobotType : STRING
VAR_INPUT
RobotType :BYTE;
END_VAR
VAR
END_VAR

FC_Dobot_Get_RobotType:=”;

CASE BYTE_TO_INT(RobotType) OF

e_Dobot_CR_RealtimeFeedback_RobotType.CR10:
FC_Dobot_Get_RobotType:=’CR10′;
e_Dobot_CR_RealtimeFeedback_RobotType.CR12:
FC_Dobot_Get_RobotType:=’CR12′;
e_Dobot_CR_RealtimeFeedback_RobotType.CR16:
FC_Dobot_Get_RobotType:=’CR16′;
e_Dobot_CR_RealtimeFeedback_RobotType.CR3:
FC_Dobot_Get_RobotType:=’CR3′;
e_Dobot_CR_RealtimeFeedback_RobotType.CR3L:
FC_Dobot_Get_RobotType:=’CR3L’;
e_Dobot_CR_RealtimeFeedback_RobotType.CR5:
FC_Dobot_Get_RobotType:=’CR5′;
e_Dobot_CR_RealtimeFeedback_RobotType.CR7:
FC_Dobot_Get_RobotType:=’CR7′;
e_Dobot_CR_RealtimeFeedback_RobotType.M1Pro:
FC_Dobot_Get_RobotType:=’M1Pro’;
e_Dobot_CR_RealtimeFeedback_RobotType.MG400:
FC_Dobot_Get_RobotType:=’MG400′;

END_CASE

MAIN

DUTとテスト用のFunctionを用意したあとメインプログラムを作成します。

VAR

Socket通信用のFunction Block・受信データBuffer・受信CounterとエラーCounterなど、プログラムで使用される変数を定義します。

PROGRAM MAIN
VAR
SocketConnect :FB_SocketConnect;
SocketClose :FB_SocketClose;
SocketReceive :FB_SocketReceive;
hSocket :T_HSOCKET;
bConnect :BOOL;
bClose :BOOL;
RefleshingTimer :TON;
tRefleshTime :TIME:=T#200MS;
tTimeoutTimer,TRetryTimer:TON;
ReceiveByte :ARRAY[0..1439]OF BYTE;
receCounter:UINT:=0;
ErrorCounter:UINT:=0;
DobotCR5:uDUT_Dobot_CR_RealtimeFeedback;
iStep:UINT:=0;
RobotType :STRING;
Disabled :BOOL;
END_VAR

Program

Robot Controller に接続する>受信待ち>もし受信データサイズが1440Bytesなら受信Counterを加算する>もし時間内に1440Bytesのデータ受信できないなら>接続を切断>待ち>再接続のような流れです。

CASE iStep OF

0:
SocketConnect(
sRemoteHost:=’192.168.5.1′
,nRemotePort:=30005
,bExecute:=bConnect
,hSocket=>hSocket
);
IF bConnect AND 　　　　　　　　　(NOT SocketConnect.bBusy 　　　　　　　　　OR 　SocketConnect.bError)THEN
bConnect:=FALSE;
SocketConnect(bExecute:=FALSE);
iStep:=10;
END_IF

10:
RefleshingTimer(
IN:=NOT RefleshingTimer.Q
,PT:=tRefleshTime
);

SocketReceive(
hSocket:=hSocket
,cbLen:=1440
,pDest:=ADR(ReceiveByte)
,bExecute:=NOT RefleshingTimer.Q 　　　　　　　　　　　　　　　　AND NOT SocketReceive.bBusy
);

IF SocketReceive.nRecBytes=1440 THEN
DobotCR5._raw:=ReceiveByte;
receCounter:=receCounter+1;
RobotType:=　　　　　　　　　　　　　　FC_Dobot_Get_RobotType(　　　　　　　　　　　　　　RobotType:=DobotCR5.data.CRRobotType);
Disabled:=DobotCR5.data.RobotMode = e_Dobot_CR_RealtimeFeedback_RobotMode.Disable;
END_IF
tTimeoutTimer(IN:=SocketReceive.nRecBytes<>1440,PT:=T#400MS);
IF tTimeoutTimer.Q THEN
ErrorCounter:=ErrorCounter+1;
SocketReceive(bExecute:=FALSE);
RefleshingTimer(in:=FALSE);
tTimeoutTimer(IN:=FALSE);
iStep:=20;
END_IF

20:
TRetryTimer(IN:=TRUE,PT:=T#0.3S);
IF TRetryTimer.Q THEN
TRetryTimer(IN:=FALSE);
iStep:=25;
END_IF

25:
SocketClose(
hSocket:=hSocket
,bExecute:=bClose
);

IF bClose
AND NOT SocketClose.bBusy
THEN
bClose:=FALSE;
SocketClose(bExecute:=FALSE);
iStep:=0;
bConnect:=TRUE;
END_IF

END_CASE