Dobot Bluetooth UART 通信 Delphi 2020/11/13 ～ 2020/11/17
・2020/11/16 Android のスクリーンショットを追加
・2020/11/17 Android のコードを最後に追加
Dobot 純正の Bluetooth アダプタを使わずに、市販の Bluetooth モジュールを使っています。
Windows、Android 端末からワイヤレスで Dobot をコントロールできます。
※ iPhone (iOS) は Classic Bluetooth には対応していないため、使用できません。
送受信しているバイト配列のデータは、UART、Wi-Fi（UDP) でも同じ内容です。
Bluetooth モジュール RBT-001、レベル変換基板 80FG990 はマイクロテクニカで購入しました。
端末から見て受信側が安定しないので、バッファ IC 74HC4050 を追加しています。
接続図はこちらです。
ペアリング前にユーティリティソフトウェア（Simply Blue Commander）で、下記を設定します。
- RBT-001 Hardware Commands
・Set Event Filter: No events reported, no UART break (full cable replacement)
・Set Local Name:RBT-001 ※デフォルトは、"EasyBT"
・Change UART speed: 19200
下記のサンプルコードのターゲットは、Windows(VCL)です。
Android の場合は、少し遅い感じ。
画面の更新が Windows とは異なるようで、そのままのコードではHoming 中のリアルタイムの座標は表示されませんでした。
Android 用のコードを、Windows の次（最後）に追加しました。 2020/11/17
{
Classic Bluetooth で Dobot Magician を使う (Windows)
Dobot 背面のインターフェースコネクタ 10P のシリアル(TX, RX, GND)に、
Bluetooth モジュール RBT-001 を接続する
使用コネクタは「ケーブル圧接型ソケット10P FC-10P」、
コネクタ付きケーブルの場合は「フラットリボンケーブル FC/FCコネクター 10ピン」で検索する
レベル変換基板と Dobot との接続
TX - TX, RX - RX, GND - GND
TX, RX とも間にバッファ IC (74HC4050 等) を入れると、受信側が安定する
Windows、Android 端末 (iOS(iPhone)では使用不可）でペアリング前に、
ユーティリティソフトウェア（Simply Blue Commander）で、下記を設定
- RBT-001 Hardware Commands
・Set Event Filter: No events reported, no UART break (full cable replacement)
・Set Local Name:RBT-001
・Change UART speed: 19200
2020/11/12 f.izawa
URL: http://www.izawa-web.com/
e-mail : f.izawa@dream.com
参照元：Dobot Magician Communication Protocol
}
unit Unit8;
interface
uses
Winapi.Windows, Winapi.Messages, System.SysUtils, System.Variants, System.Classes, Vcl.Graphics,
Vcl.Controls, Vcl.Forms, Vcl.Dialogs, System.Bluetooth, Vcl.StdCtrls,
System.Bluetooth.Components, Vcl.Buttons, Vcl.ExtCtrls, math;
type
TPTPMode = (
JUMP_XYZ,
MOVJ_XYZ,
MOVL_XYZ,
JUMP_ANGLE,
MOVJ_ANGLE,
MOVL_ANGLE,
MOVJ_INC,
MOVL_INC
);
TJOGMode = (
IDEL, // Invalid status
AP_DOWN, // X+ / Joint1+
AN_DOWN, // X- / Joint1-
BP_DOWN, // Y+ / Joint2+
BN_DOWN, // Y- / Joint2-
CP_DOWN, // Z+ / Joint3+
CN_DOWN, // Z- / Joint3-
DP_DOWN, // R+ / Joint4+
DN_DOWN, // R- / Joint4-
LP_DOWN, // L+. Only when the parameter isJoint=1, the LP_DOWN is available
LN_DOWN // L-. Only when the parameter isJoint=1, the LN_DOWN is available
);
TJOGModel = (
COORDINATE_MODEL,
JOINT_MODEL
);
TIOFunction = (
IOFunctionDummy, // Do not config function
IOFunctionPWM, // PWM Output
IOFunctionDO, // IO Output
IOFunctionDI, // IO Intput
IOFunctionADC // AD Input
);
type
TForm8 = class(TForm)
Bluetooth1: TBluetooth;
Button1: TButton;
Memo1: TMemo;
Button2: TButton;
Button3: TButton;
Button4: TButton;
Button5: TButton;
Button6: TButton;
Button7: TButton;
SpeedButton1: TSpeedButton;
SpeedButton2: TSpeedButton;
SpeedButton3: TSpeedButton;
SpeedButton4: TSpeedButton;
SpeedButton5: TSpeedButton;
SpeedButton6: TSpeedButton;
SpeedButton7: TSpeedButton;
SpeedButton8: TSpeedButton;
Button8: TButton;
Button9: TButton;
Button10: TButton;
Edit1: TEdit;
Edit2: TEdit;
Label1: TLabel;
Label2: TLabel;
Label3: TLabel;
Edit3: TEdit;
Label4: TLabel;
Edit4: TEdit;
Button11: TButton;
Label5: TLabel;
Label6: TLabel;
Edit5: TEdit;
Edit6: TEdit;
Button12: TButton;
Button13: TButton;
Timer1: TTimer;
Edit7: TEdit;
Edit8: TEdit;
Edit9: TEdit;
Edit10: TEdit;
Edit11: TEdit;
Edit12: TEdit;
Edit13: TEdit;
Edit14: TEdit;
Edit15: TEdit;
Edit16: TEdit;
Edit17: TEdit;
Edit18: TEdit;
Edit19: TEdit;
Edit20: TEdit;
Edit21: TEdit;
Button14: TButton;
Edit22: TEdit;
Button15: TButton;
Button16: TButton;
procedure Button1Click(Sender: TObject);
procedure Button2Click(Sender: TObject);
procedure Button3Click(Sender: TObject);
procedure Button4Click(Sender: TObject);
procedure Button5Click(Sender: TObject);
procedure Button6Click(Sender: TObject);
procedure SpeedButton1MouseDown(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Integer);
procedure SpeedButton1MouseUp(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Integer);
procedure Button7Click(Sender: TObject);
procedure Button8Click(Sender: TObject);
procedure Button9Click(Sender: TObject);
procedure Button10Click(Sender: TObject);
procedure Button11Click(Sender: TObject);
procedure FormCreate(Sender: TObject);
procedure Button12Click(Sender: TObject);
procedure Button13Click(Sender: TObject);
procedure Timer1Timer(Sender: TObject);
procedure Button14Click(Sender: TObject);
procedure Button15Click(Sender: TObject);
procedure Button16Click(Sender: TObject);
private
{ Private 宣言 }
public
{ Public 宣言 }
end;
var
Form8: TForm8;
ADevice : TBluetoothDevice;
ASocket : TBluetoothSocket;
AlarmCode : array of string = [
'$00', 'ERR_COMMON_RESET',
'$10', 'ERR_PLAN_INV_SINGULARITY',
'$11', 'ERR_PLAN_INV_CALC',
'$12', 'ERR_PLAN_INV_LIMIT',
'$13', 'ERR_PLAN_PUSH_DATA_REPEAT',
'$14', 'ERR_PLAN_ARC_INPUT_PARAM',
'$15', 'ERR_PLAN_JUMP_PARAM',
'$20', 'ERR_MOVE_INV_SINGULARITY',
'$21', 'ERR_MOVE_INV_CALC',
'$22', 'ERR_MOVE_INV_LIMIT',
'$30', 'ERR_OVERSPEED_AXIS1',
'$31', 'ERR_OVERSPEED_AXIS2',
'$32', 'ERR_OVERSPEED_AXIS3',
'$33', 'ERR_OVERSPEED_AXIS4',
'$40', 'ERR_LIMIT_AXIS1_POS',
'$41', 'ERR_LIMIT_AXIS1_NEG',
'$42', 'ERR_LIMIT_AXIS2_POS',
'$43', 'ERR_LIMIT_AXIS2_NEG',
'$44', 'ERR_LIMIT_AXIS3_POS',
'$45', 'ERR_LIMIT_AXIS3_NEG',
'$46', 'ERR_LIMIT_AXIS4_POS',
'$47', 'ERR_LIMIT_AXIS4_NEG',
'$48', 'ERR_LIMIT_AXIS23_POS',
'$49', 'ERR_LIMIT_AXIS23_NEG',
'$50', 'ERR_LOSE_STEP_AXIS1',
'$51', 'ERR_LOSE_STEP_AXIS2',
'$52', 'ERR_LOSE_STEP_AXIS3',
'$53', 'ERR_LOSE_STEP_AXIS4' ];
runFlag : boolean; // 非常停止通知、検出用
tmBusy : boolean; // インターバルタイマーで処理中
const
// SPP(Serial Port Profile) による通信のUUID
ServiceUUID = '{00001101-0000-1000-8000-00805F9B34FB}';
// 検索する BT デバイス名
// デフォルト 'EasyBT' を 'RBT-001' に変更している
BTDeviceHead = 'RBT-001';
// プロトタイプ
function RBTReceiveData(ASocket: TBluetoothSocket; var readData: TBytes; ATimeout: Cardinal): integer;
function GetPose(ASocket: TBluetoothSocket; var x, y, z, r: single; var a0, a1, a2, a3: single): boolean;
implementation
{$R *.dfm}
function SetIOMultiplexing(ASocket: TBluetoothSocket; ioAddress: Byte; ioFunction: Byte): boolean;
// 多重入出力を設定
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 130; // id;
B[4] := 1; // ctrl r/w = W, isQueued = False;
B[5] := ioAddress; // EIO Num(1..20)
B[6] := ioFunction; // TIOFunction,
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 130);
end;
end;
function SetPTPCommonParams(ASocket: TBluetoothSocket;
velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 8; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 83; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[5 + i] := velocityRatio.Bytes[i];
for i := 0 to 3 do B[9 + i] := accelerationRatio.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 83);
end;
end;
function GetIODI(ASocket: TBluetoothSocket; ioAddress: Byte; var ioValue: Byte):boolean;
// GetIODI 入力状態を取得
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 133; // id;
B[4] := 0; // ctrl r/w = r, isQueued = False;
B[5] := ioAddress;
B[6] := 0;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 133) and (B[5] = ioAddress);
if result then
ioValue := B[6];
end;
end;
function GetIODO(ASocket: TBluetoothSocket; ioAddress: Byte; var ioLevel: Byte): boolean;
// 出力状態を取得
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, 8);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 131; // id;
B[4] := 0; // ctrl r/w = r, isQueued = False;
B[5] := ioAddress;
B[6] := 0;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 131) and (B[5] = ioAddress);
if result then
ioLevel := B[6];
end;
end;
function SetIODO(ASocket: TBluetoothSocket; ioAddress: Byte; ioLevel: Byte): boolean;
// 出力
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 131; // id;
B[4] := 1; // ctrl r/w = W, isQueued = False;
B[5] := ioAddress; // EIO Num(1..20)
B[6] := ioLevel; // Level output 0-Low level 1-High level
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 131);
end;
end;
function GetPTPCommonParams(ASocket: TBluetoothSocket;
var velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数取得
var
B : TBytes;
len, i : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 83; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $AD;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 83);
if result then begin
for i := 0 to 3 do begin
velocityRatio.Bytes[i] := B[5 + i];
accelerationRatio.Bytes[i] := B[9 + i];
end;
end;
end;
end;
function SetJOGCommonParams(ASocket: TBluetoothSocket;
velocityRatio, accelerationRatio: single): boolean;
// JOG 移動 速度係数、加速度係数設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 8; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 72; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := velocityRatio.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := accelerationRatio.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 72);
end;
end;
function GetJOGCommonParams(ASocket: TBluetoothSocket;
var velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数取得
var
B : TBytes;
len, i : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 72; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $B8;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 72);
if result then begin
for i := 0 to 3 do begin
velocityRatio.Bytes[i] := B[5 + i];
accelerationRatio.Bytes[i] := B[9 + i];
end;
end;
end;
end;
function SetSetPTPCoordinateParams(ASocket: TBluetoothSocket;
xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single): boolean;
// PTP LINE 移動 速度、加速度設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 16; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 81; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := xyzVelocity.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := rVelocity.Bytes[i];
for i := 0 to 3 do B[13 + i] := xyzAcceleration.Bytes[i];
for i := 0 to 3 do B[17 + i] := rAcceleration.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 81);
end;
end;
function GetPTPCoordinateParams(ASocket: TBluetoothSocket;
var xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single): boolean;
var
i, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 81; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $AF;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 81);
if result then begin
for i := 0 to 3 do begin
xyzVelocity.Bytes[i] := B[ 5 + i];
rVelocity.Bytes[i] := B[ 9 + i];
xyzAcceleration.Bytes[i] := B[13 + i];
rAcceleration.Bytes[i] := B[17 + i];
end;
end;
end;
end;
function SetJOGCoordinateParams(ASocket: TBluetoothSocket;
velocity, acceleration: array of single): boolean;
// Jog移動 X, Y, Z R 軸の速度と加速度
var
B : TBytes;
len, i, j : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 32; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 71; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for j := 0 to 3 do begin
for i := 0 to 3 do begin
B[ 5 + j * 4 + i] := velocity[j].Bytes[i]; // X,Y, Z, R axis
B[21 + j * 4 + i] := acceleration[j].Bytes[i];
end;
end;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 71);
end;
end;
function GetJOGCoordinateParams(ASocket: TBluetoothSocket;
var velocity: array of single; var acceleration: array of single): boolean;
var
i, j, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 71; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $B9;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 71);
if result then begin
for j := 0 to 3 do begin
for i := 0 to 3 do begin
velocity[j].Bytes[i] := B[ 5 + j * 4 + i];
acceleration[j].Bytes[i] := B[21 + j * 4 + i];
end;
end;
end;
end;
end;
function SetQueuedCmdClear(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 245; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0A;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 245);
end;
end;
function SetQueuedCmdStartExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len +4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 240; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0F;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 240);
end;
end;
function SetDeviceName(ASocket: TBluetoothSocket; const deviceName: string): boolean;
// デバイス名をセット
var
B, temp : TBytes;
len, i, n : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
temp := TEncoding.ANSI.GetBytes(deviceName);
n := Length(temp);
len := 2 + n; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 1; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to n - 1 do B[5 + i] := temp[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 1);
end;
end;
function GetDeviceName(ASocket: TBluetoothSocket; var deviceName: string): boolean;
var
B : TBytes;
len : integer;
i : Integer;
begin
result := false;
deviceName := '';
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 1; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $FF;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 1);
if result then begin
for i := 5 to len - 2 do
deviceName := deviceName + Char(B[i]);
end;
end;
end;
function GetDeviceSN(ASocket: TBluetoothSocket; var deviceSN: string): boolean;
var
B : TBytes;
len : integer;
i : Integer;
begin
result := false;
deviceSN := '';
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 0; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $00;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 0);
if result then begin
for i := 5 to len - 2 do
deviceSN := deviceSN + Char(B[i]);
end;
end;
end;
function GetAlarmsState(ASocket: TBluetoothSocket;
var isAlarm: boolean; var res: string): boolean;
// アラーム取得
var
alarmsState: array [0..15] of Byte;
B : TBytes;
len : integer;
i, j, idx, k : Integer;
begin
result := false;
isAlarm := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 20; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $EC;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 20);
if result then begin
for i := 0 to 15 do
alarmsState[i] := B[5 + i];
for i := 1 to 15 do begin // alarmsState[0] は無視
if alarmsState[i] > 0 then begin
isAlarm := true;
break;
end;
end;
end;
end;
// アラーム内容（文字列）を作成
if isAlarm then begin
for i := 1 to 15 do begin // alarmsState[0] は無視
for j := 0 to 7 do begin
if (alarmsState[i] and (1 shl j)) > 0 then begin
idx := i * 8 + j;
res := '';
for k := 0 to Length(AlarmCode) div 2 - 1 do begin
if idx = StrToInt(AlarmCode[k * 2]) then
res := res + AlarmCode[k * 2 + 1] + #09; // TAB を追加
end;
res := Trim(res); // 最後の #09 を削除
end;
end;
end;
end;
end;
function ClearAllAlarmsState(ASocket: TBluetoothSocket): boolean;
// アラームクリア
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 20; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $EB;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 20);
end;
end;
function SetQueuedCmdStopExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 241; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0E;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 241);
end;
end;
function SetQueuedCmdForceStopExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 242; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0D;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 242);
end;
end;
function GetQueuedCmdCurrentIndex(ASocket: TBluetoothSocket;
var queuedCmdCurrentIndex: Uint64): boolean;
// コマンドキューの現在の位置
var
B : TBytes;
len, i : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 246; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $0A;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 2000);
result := (len = B[2] + 4) and (B[3] = 246);
if result then begin
queuedCmdCurrentIndex := 0;
for i := 0 to 7 do
queuedCmdCurrentIndex :=
queuedCmdCurrentIndex or (Uint64(B[5 + i]) shl (i * 8));
end;
end;
end;
function SetHomeParams(ASocket: TBluetoothSocket; x, y, z, r: single): boolean;
// HOME 位置設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 16; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 30; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := x.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := y.Bytes[i];
for i := 0 to 3 do B[13 + i] := z.Bytes[i];
for i := 0 to 3 do B[17 + i] := r.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 30);
end;
end;
function GetHomeParams(ASocket: TBluetoothSocket; var x, y, z, r: single): boolean;
// ホーム座標を取得
var
B : TBytes;
len, i : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 30; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $E2;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 30);
if result then begin
for i := 0 to 3 do begin
x.Bytes[i] := B[ 5 + i];
y.Bytes[i] := B[ 9 + i];
z.Bytes[i] := B[13 + i];
r.Bytes[i] := B[17 + i];
end;
end;
end;
end;
function SetHomeCmd(ASocket: TBluetoothSocket): boolean;
// HomeCmd ホーミング
var
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := false;
if runFlag and (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 4; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 31; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
// ダミー(4バイト)
for i := 0 to 3 do B[5 + i] := 0;
// チェックサム
B[9] := $DE;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 31);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
Application.ProcessMessages;
if runFlag then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('X = %.3f, Y = %.3f, Z = %.3f', [x, y, z]));
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex);
end
else begin
result := false;
break;
end;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('x = %.3f, y = %.3f, z = %.3f', [x, y, z]));
end;
end;
end;
end;
function SetPTPCmd(ASocket: TBluetoothSocket; ptpMode: Byte; toX, toY, toZ, toR: single): boolean;
// PTPCmd
var
checkSum : Byte;
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 17; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 84; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
B[5] := ptpMode;// PTPMode 2= MOVL_XYZ
for i := 0 to 3 do B[ 6 + i] := toX.Bytes[i]; // X
for i := 0 to 3 do B[10 + i] := toY.Bytes[i]; // Y
for i := 0 to 3 do B[14 + i] := toZ.Bytes[i]; // Z
for i := 0 to 3 do B[18 + i] := toR.Bytes[i]; // R
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 84);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
Application.ProcessMessages;
if runFlag then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('x = %.3f, Y = %.3f, Z = %.3f', [x, y, z]));
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex);
end
else begin
result := false;
break;
end;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('x = %.3f, Y = %.3f, Z = %.3f', [x, y, z]));
end;
end;
end;
end;
function SetJOGCmd(ASocket: TBluetoothSocket; jogMode: Byte; jogCommand: Byte): boolean;
// JogCmd
// jogMode 0 で X, Y, Z 方向に動く。方向は、 jogCommand で指定
// jog 開始は、方向を指定。停止するまで動き続ける
// Jog 停止は、jogCommand に 0 を指定。
// スピードが速すぎると、使いにくい
var
checkSum : Byte;
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 73; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
B[5] := jogMode; // Jog mode 0-coordinate jog 1-Joint jog
B[6] := jogCommand; // Jog command(Value range0..8)
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 73);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
Application.ProcessMessages;
if runFlag then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('X = %.3f, Y = %.3f, Z = %.3f', [x, y, z]));
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex);
end
else begin
result := false;
break;
end;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
Form8.Memo1.Lines.Add(Format('x = %.3f, y = %.3f, z = %.3f', [x, y, z]));
end;
end;
end;
end;
function GetEndEffectorSuctionCup(ASocket: TBluetoothSocket;
var isCtrlEnable: Byte; var isSucked: Byte): boolean;
// 吸引カップの状態を取得
var
B : TBytes;
len : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 62; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
B[5] := $C2; // checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 62);
if result then begin
isCtrlEnable := B[5];
isSucked := B[6];
end;
end;
end;
function SetEndEffectorSuctionCup(ASocket: TBluetoothSocket; ctrl, suck: Byte):boolean;
// 吸引カップ ON/OFF
var
B : TBytes;
len, i : integer;
checkSum : Byte;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 62; // id;
B[4] := 3; // ctrl r/w($01) = W, isQueued($02) = True;
B[5] := ctrl; // CtrlEnable
B[6] := suck; // Suck
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 62);
end;
end;
function GetEndEffectorGripper(ASocket: TBluetoothSocket;
var isCtrlEnable: Byte; var isGripped: Byte): boolean;
// グリッパーの状態を取得
var
B : TBytes;
len : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 63; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
B[5] := $C1; // checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 63);
if result then begin
isCtrlEnable := B[5];
isGripped := B[6];
end;
end;
end;
function SetEndEffectorGripper(ASocket: TBluetoothSocket; ctrl, grip: Byte):boolean;
// 吸引カップ ON/OFF
var
B : TBytes;
len, i : integer;
checkSum : Byte;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 63; // id;
B[4] := 3; // ctrl r/w($01) = W, isQueued($02) = True;
B[5] := ctrl; // CtrlEnabled
B[6] := grip; // Grip
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 63);
end;
end;
function GetPose(ASocket: TBluetoothSocket;
var x, y, z, r: single; var a0, a1, a2, a3: single): boolean;
// GetPose 現在値を取得
var
i, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 10; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $F6;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 1000);
result := (len = B[2] + 4) and (B[3] = 10);
if result then begin
for i := 0 to 3 do begin
x.Bytes[i] := B[ 5 + i];
y.Bytes[i] := B[ 9 + i];
z.Bytes[i] := B[13 + i];
r.Bytes[i] := B[17 + i];
a0.Bytes[i] := B[21 + i];
a1.Bytes[i] := B[25 + i];
a2.Bytes[i] := B[29 + i];
a3.Bytes[i] := B[33 + i];
end;
end;
end;
end;
// Dobot Magician アーム角度を取得
// x, y, z : エンドエフェクタの位置
// A1, A2, A3 : 主軸の回転角度、アーム 1 の傾斜角度, アーム 2 の角度（アーム 1 からの傾斜角度）
// J1, J2, J3 : 主軸の回転角度、アーム 1 の角度（垂直で 0）アーム 2 の傾斜角度（水平で 0）
function dobotArmAngle(x, y, z: double; var A1, A2, A3, J1, J2, J3 : double): boolean;
// uses ,,,, math;
var
th1, th2, th3 : double;
phi, l, ld : double;
L1, L2, L3, L4 : double;
begin
L1 := 0.0; // アーム基点の高さ(Dobot Magician の場合: 0)
L2 := 135.0; // アーム１の辺の長さ
L3 := 146.74; // アーム２の辺の長さ(147.0mm)
L4 := 60.0; // エンドエフェクタの突き出し部分（必ず水平である）
result := false;
// X-Y平面上のアーム部分の長さ
l := sqrt(x * x + y * y) - L4 ;
if (l > L4) then begin
// X-Y 平面上のアームの角度
// Dobot の場合、-125～ +125 度であること
th1 := arctan2(y, x);
// アーム１、２の底辺の長さ
ld := sqrt(l * l + (z - L1) * (z - L1));
result := (ld > 0) and (ld < (L2 + L3));
if result then begin
// アーム基点から先端への仰角
// アーム１、アーム２底辺の三角形の基点側の内角
phi := arctan2((z - L1), l);
// アーム１の角度
th2 := phi + arccos((ld * ld + L2 * L2 - L3 * L3) / (2.0 * ld * L2));
// アーム２のアーム１からの角度
th3 := arcsin((ld * ld - L2 * L2 - L3 * L3) / (2.0 * L2 * L3)) + PI / 2.0;
// ラジアン->デグリ換算
A1 := th1 * 180.0 / PI;
A2 := th2 * 180.0 / PI ;
A3 := th3 * 180.0 / PI ;
// Dobot Arm の角度表示に変換
J1 := A1;
// アーム１の角度（垂直で 0 度、前に傾斜で＋方向）
// Dobot の場合 -5.0 ～ +84.0 まで
J2 := 90.0 - A2;
// アーム２の角度（水平で 0 度、前方に下がる方向が＋）
// Dobot の場合、 -15.0 ～ +75.0 まで
J3 := 180.0 - A2 - A3;
end;
end;
end;
// 可動範囲内にあるか
function dobotCheckXYZ(x, y, z : double): boolean;
var
A1, A2, A3, J1, J2, J3 : double;
dist1, dist2 : double;
begin
result := false;
if dobotArmAngle(x, y, z, A1, A2, A3, J1, J2, J3) then begin
dist1 := sqrt(x * x + y * y);
dist2 := sqrt((dist1 - 60.0) * (dist1 - 60.0) + z * z);
// (J2 < 89.00) : 大きくすると、下側に（初期値：83.99）
// (J3 < 75.00) : 大きくすると、中心方向に（初期値：75.00)
result := (J1 > -125.0 ) and (J1 < 125.0) and
(J2 > -4.99) and (J2 < 89.5) and
(J3 > -14.9 ) and (J3 < 89.5) and
(dist2 <= 268);
// 268 はアームが最大に伸びた時の長さ（268 ～ 272mm）一直線にはならないため、制限が必要
end;
end;
function dobotSetupIO(ASocket: TBluetoothSocket): boolean;
// IO セットアップ
begin
result := false;
if (ASocket = nil) or not ASocket.Connected then exit;
SetIOMultiplexing(ASocket, 18, Ord(TioFunction.IOFunctionDO));
SetIOMultiplexing(ASocket, 19, Ord(TioFunction.IOFunctionDI));
SetIOMultiplexing(ASocket, 20, Ord(TioFunction.IOFunctionDI));
result := true;
end;
function searchRBT(const deviceHead : string): boolean;
// Bluetooth デバイスを検索
var
ABluetoothManager : TBluetoothManager;
APairedDevices : TBluetoothDeviceList;
ADevice : TBluetoothDevice;
idx, i : integer;
begin
result := false;
if (ASocket = nil) or not ASocket.Connected then begin
try
ABluetoothManager := TBluetoothManager.Current;
if ABluetoothManager.ConnectionState = TBluetoothConnectionState.Connected then begin
// 過去にペアリングされたデバイスの一覧から、ターゲット を探す
APairedDevices := ABluetoothManager.GetPairedDevices;
if APairedDevices.Count > 0 then begin
idx := -1;
for i := 0 to APairedDevices.Count -1 do begin
if (BTDeviceHead = APairedDevices[i].DeviceName) then begin
idx := i;
break; // リストアップを終了
end;
end;
if idx >= 0 then begin
ADevice := APairedDevices[idx];
if ADevice <> nil then begin
ASocket := ADevice.CreateClientSocket(StringToGUID(ServiceUUID), False);
if ASocket <> nil then begin
// 接続
ASocket.Connect;
result := ASocket.Connected;
end;
end;
end;
end;
end;
except
on E : Exception do begin
ShowMessage(E.Message);
end;
end;
end;
end;
function RBTReceiveData(ASocket: TBluetoothSocket;
var readData: TBytes; ATimeout: Cardinal): integer;
// Dobot(Bluetooth) からの応答
var
AData : TBytes;
i : integer;
Ticks : Cardinal;
idx : integer;
loop : boolean;
cnt : integer;
begin
cnt := 0; idx := 0;
Setlength(readData, 64); // 大き目に用意
Ticks := TThread.GetTickCount;
loop := True;
while loop and (cnt < 500) do begin
//Sleep(1);
AData := ASocket.ReceiveData;
if Length(AData) > 0 then begin
for i := 0 to Length(AData) - 1 do begin
readData[idx] := AData[i];
// 受信バッファの長さを再セット
if idx = 2 then SetLength(readData, readData[2] + 4);
Inc(idx);
if idx >= Length(readData) then begin
loop := False;
break;
end;
end;
end;
Inc(cnt);
if loop then
loop := TThread.GetTickCount - Ticks < ATimeout;
end;
result := idx;
end;
procedure TForm8.Button10Click(Sender: TObject);
// Jog 移動の速度、加速度をセット
var
velocity : array [0..3] of single;
acceleration : array [0..3] of single;
i : integer;
begin
for i:= 0 to 3 do begin
velocity[i] := StrToFloatDef(Edit1.Text, 100);
acceleration[i] := StrToFloatDef(Edit2.Text, 100);;
end;
if SetJOGCoordinateParams(ASocket, velocity, acceleration) then
Memo1.Lines.Add('OK');
end;
procedure TForm8.Button11Click(Sender: TObject);
// PTP LINE 移動、速度、加速度セット
var
xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single;
begin
xyzVelocity := StrToFloatDef(Edit3.Text, 100);
xyzAcceleration := StrToFloatDef(Edit4.Text, 100);
rVelocity := StrToFloatDef(Edit5.Text, 100);
rAcceleration := StrToFloatDef(Edit6.Text, 100);
if SetSetPTPCoordinateParams(
ASocket, xyzVelocity, rVelocity, xyzAcceleration, rAcceleration) then
Memo1.Lines.Add('Ok');
end;
procedure TForm8.Button12Click(Sender: TObject);
// PTP CommonParams
// 速度係数、加速度係数を取得
// 数値はパーセント
var
velocityRatio : single;
accelerationRatio : single;
begin
if GetPTPCommonParams(ASocket, velocityRatio, accelerationRatio) then
Memo1.Lines.Add(Format('velocityRatio = %.3f, accelerationRatio = %.3f',
[velocityRatio, accelerationRatio]));
end;
procedure TForm8.Button13Click(Sender: TObject);
// PTP CommonParams
// 速度係数、加速度係数をセット
// 数値はパーセント
var
velocityRatio : single;
accelerationRatio : single;
begin
velocityRatio := 120.0;
accelerationRatio := 80.0;
if SetPTPCommonParams(ASocket, velocityRatio, accelerationRatio) then
Memo1.Lines.Add('Ok');
end;
procedure TForm8.Button14Click(Sender: TObject);
// HOME 位置セット
begin
if SetHomeParams(ASocket, 200, 0, 100, 0) then
Memo1.Lines.Add('Ok');
end;
procedure TForm8.Button15Click(Sender: TObject);
// Get ... テスト
var
x, y, z, r : single;
velocityRatio, accelerationRatio: single;
velocity, acceleration: array [0..3] of single;
xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single;
i : integer;
begin
if GetHomeParams(ASocket, x, y, z, r) then
Memo1.Lines.Add(Format('X = %.3f, Y = %.3f, Z = %.3f, R = %.3f' , [ x, y, z, r]));
if GetJOGCommonParams(ASocket, velocityRatio, accelerationRatio) then
Memo1.Lines.Add(Format('velocityRatio = %.3f, accelerationRatio = %.3f',
[velocityRatio, accelerationRatio]));
if SetJOGCommonParams(ASocket, velocityRatio, accelerationRatio) then
Memo1.Lines.Add('Ok');
if GetJOGCoordinateParams(ASocket, velocity, acceleration) then begin
for i := 0 to 3 do
Memo1.Lines.Add(Format('velocity[%d] = %.3f', [i, velocity[i]]));
for i := 0 to 3 do
Memo1.Lines.Add(Format('acceleration[%d] = %.3f', [i, acceleration[i]]));
end;
if GetPTPCoordinateParams(
ASocket, xyzVelocity, rVelocity, xyzAcceleration, rAcceleration) then begin
Memo1.Lines.Add(Format('xyzVelocity = %.3f, rVelocity = %.3f' ,
[xyzVelocity, rVelocity]));
Memo1.Lines.Add(Format('xyzAcceleration = %.3f, rAcceleration = %.3f' ,
[xyzAcceleration, rAcceleration]));
end;
end;
procedure TForm8.Button16Click(Sender: TObject);
// デバイス名セット
var
deviceName : string;
begin
deviceName := 'Dobot No.2';
if SetDeviceName(ASocket, deviceName) then
Memo1.Lines.Add('Ok');
end;
procedure TForm8.Button1Click(Sender: TObject);
// Bluetooth 検索、接続
begin
if searchRBT(BTDeviceHead) then begin
SetQueuedCmdClear(ASocket);
SetQueuedCmdStartExec(ASocket);
Memo1.Lines.Add('CONNECT');
dobotSetupIO(ASocket);
end;
end;
procedure TForm8.Button2Click(Sender: TObject);
// 吸引カップ 反転
// グリッパーの場合は、ctrl = 0 で OFF になる
// ctrl = 1 にし、suck で 開閉をコントロール可能
var
ctrl, suck: Byte;
begin
if GetEndEffectorSuctionCup(ASocket, ctrl, suck) then begin
SetEndEffectorSuctionCup(ASocket, 1, not suck and 1); // 反転
end;
end;
procedure TForm8.Button3Click(Sender: TObject);
// ホーミング
begin
runFlag := True; // 非常停止に対応するため
if SetHomeCmd(ASocket) then
Memo1.Lines.Add('HOME END');
end;
procedure TForm8.Button4Click(Sender: TObject);
// PTP 移動
var
pose, adeg : array [0..3] of single;
begin
// MOVL_XYZ = 直線移動
// 直線になるようにアームの角度を細かく補正しているので、MOVJ より遅くなる
// X,Y 位置を変えずに垂直移動する場合に使用
// MOVJ_XYZ = ジャンプ移動
// 直線補正をせずに最小限のアーム移動をする
// このため、Z 値のみの移動でも円弧動作になる
// MOVL よりかなり速い
// 通常の移動はこれを使用
// JUMP_XYZ = ピック＆プレース移動
// 予め高さ制限を設定しておくと、始点－終点の指定のみで、
// ピック＆プレース動作になる
// 速さは 同じ動作を MOVJ, MOVL で行った場合より若干速い
if GetPose(ASocket, pose[0], pose[1], pose[2], pose[3], adeg[0], adeg[1], adeg[2], adeg[3]) then begin
runFlag := True;
// 直線移動
SetPTPCmd(ASocket, Ord(TPTPMode.MOVL_XYZ), pose[0] + 20, pose[1] + 20, pose[2] + 20, pose[3]);
// ジャンプ移動
SetPTPCmd(ASocket, Ord(TPTPMode.MOVJ_XYZ), pose[0], pose[1], pose[2] , pose[3]);
end;
end;
procedure TForm8.Button5Click(Sender: TObject);
// アラームクリア
begin
ClearAllAlarmsState(ASocket);
end;
procedure TForm8.Button6Click(Sender: TObject);
// アラーム取得
var
isAlarm : boolean;
res : string;
begin
// アラームがある場合、res にその内容が TAB 区切りで列挙される
GetAlarmsState(ASocket, isAlarm, res);
if isAlarm then
// TAB をコンマに変換
Memo1.Lines.Add(StringReplace(res, #09, ', ', [rfReplaceAll]));
end;
procedure TForm8.Button7Click(Sender: TObject);
// デバイスシリアルと名前を取得
var
dobotSN, dobotName : string;
begin
if GetDeviceSN(ASocket, dobotSN) then
Memo1.Lines.Add('SN = ' + dobotSN);
if GetDeviceName(ASocket, dobotName) then
Memo1.Lines.Add('Name = ' + dobotName);
end;
procedure TForm8.Button8Click(Sender: TObject);
// コマンド停止
// 以降継続不能となるので、切断→再接続を行う
// 使い方は不明・・・
begin
if SetQueuedCmdStopExec(ASocket) then begin
Memo1.Lines.Add('SetQueuedCmdStopExec!!');
// 切断
ASocket.Close;
ASocket := nil;
// 接続
Button1Click(self);
end;
end;
procedure TForm8.Button9Click(Sender: TObject);
// 非常停止
// 以降継続不能となるので、切断→再接続を行う
// これ以上スマートな使い方は不明
begin
// 非常停止を通知
// 現状の対象は HOMEcmd、PTPcmd、JOGcmd のみ
runFlag := false;
Application.ProcessMessages;
if SetQueuedCmdForceStopExec(ASocket) then begin
Memo1.Lines.Add('SetQueuedCmdForceStopExec!!');
ASocket.Close; // 切断
ASocket := nil;
Button1Click(self); // 再接続
end;
end;
procedure TForm8.FormCreate(Sender: TObject);
begin
Memo1.Lines.Clear;
end;
procedure TForm8.SpeedButton1MouseDown(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Integer);
// Jog コマンド開始
// SpeedButton1 ～ 8 で共用
// SpeedButton1 ～ 8 が、X+. X-, Y+, Y-, Z+, Z-, R+, R-に対応
var
jogMode, jogCommand : Byte;
begin
jogMode := Ord(TJOGModel.COORDINATE_MODEL); // =0
jogCommand := StrToIntDef(Copy((Sender as TSpeedButton).Name, 12), 0);
if jogCommand > 0 then begin
runFlag := True;
SetJogCmd(ASocket, jogMode, jogCommand);
end;
end;
procedure TForm8.SpeedButton1MouseUp(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Integer);
// Jog コマンド終了（停止）
// SpeedButton1 ～ 8 で共用
var
jogMode, jogCommand : Byte;
begin
jogMode := Ord(TJOGModel.COORDINATE_MODEL); // =0
jogCommand := Ord(TJOGMode.IDEL); // =0
SetJogCmd(ASocket, jogMode, jogCommand);
end;
procedure TForm8.Timer1Timer(Sender: TObject);
// インターバル = 250 msec
var
pos, ang : array [0..3] of single;
i : integer;
edt : TEdit;
isAlarm : boolean;
res : string;
Ticks : Cardinal;
//ioValue: Byte;
begin
if not tmBusy then begin
tmBusy := true;
if (ASocket <> nil) and ASocket.Connected then begin
Ticks := TThread.GetTickCount;
// リアルタイムポーズ
if GetPose(ASocket, pos[0], pos[1], pos[2], pos[3], ang[0], ang[1], ang[2], ang[3]) then begin
for i := 0 to 3 do begin
edt := FindComponent('Edit' + IntToStr(7 + i)) as TEdit;
if (edt <> nil) and (edt.Text <> Format('%.3f', [pos[i]])) then
edt.Text := Format('%.3f', [pos[i]]);
edt := FindComponent('Edit' + IntToStr(11 + i)) as TEdit;
if (edt <> nil) and (edt.Text <> Format('%.3f', [ang[i]])) then
edt.Text := Format('%.3f', [ang[i]]);
end;
// X, Y, Z 座標から アーム角度を逆算
{
dobotArmAngle(pos[0], pos[1], pos[2], A[0], A[1], A[2], J[0], J[1], J[2]);
for i := 0 to 2 do begin
edt := FindComponent('Edit' + IntToStr(15 + i)) as TEdit;
if (edt <> nil) and (edt.Text <> Format('%.3f', [J[i]])) then
edt.Text := Format('%.3f', [J[i]]);
// 計算誤差
edt := FindComponent('Edit' + IntToStr(18 + i)) as TEdit;
if (edt <> nil) and (edt.Text <> Format('%.3f', [ang[i]-J[i]])) then
edt.Text := Format('%.3f', [ang[i]-J[i]]);
end;
}
// 計算上の可動範囲の確認
// 可動範囲内か
if not dobotCheckXYZ(pos[0], pos[1], pos[2]) then begin
if Edit22.Text <> 'NG' then Edit22.Text := 'NG';
end
else begin
if Edit22.Text <> '' then Edit22.Text := '';
end;
end;
// アラーム取得
res := '';
if GetAlarmsState(ASocket, isAlarm, res) then begin
if isAlarm then
// TAB を変換
res := StringReplace(res, #09, ', ', [rfReplaceAll]);
end;
if Edit21.Text <> res then Edit21.Text := res;
// DIO テスト（結構時間がかかる + 100msec）
//GetIODI(ASocket, 19, ioValue);
//Memo1.Lines.Add('DI19 = ' + ioValue.ToString);
//GetIODI(ASocket, 20, ioValue);
//Memo1.Lines.Add('DI20 = ' + ioValue.ToString);
//SetIODO(ASocket, 18, not ioValue and 1); // 反転
//GetIODO(ASocket, 18, ioValue);
//Memo1.Lines.Add('DO18 = ' + ioValue.ToString);
// 処理時間を表示(30～110 msec)
Caption := (TThread.GetTickCount - Ticks).ToString + ' msec';
end;
tmBusy := false;
end;
end;
end.
// 2020/11/17 Android でテスト済
// 通信の部分は、Windows 同じ
unit Unit8;
interface
uses
System.SysUtils, System.Types, System.UITypes, System.Classes, System.Variants,
FMX.Types, FMX.Controls, FMX.Forms, FMX.Graphics, FMX.Dialogs,
System.Bluetooth, System.Bluetooth.Components, FMX.StdCtrls,
FMX.Controls.Presentation, FMX.Edit, FMX.Layouts, FMX.ScrollBox, FMX.Memo,
FMX.Objects, math, FMX.Ani, FMX.ListBox, System.IniFiles, System.IOUtils;
type
TPTPMode = (
JUMP_XYZ,
MOVJ_XYZ,
MOVL_XYZ,
JUMP_ANGLE,
MOVJ_ANGLE,
MOVL_ANGLE,
MOVJ_INC,
MOVL_INC
);
TJOGMode = (
IDEL, // Invalid status
AP_DOWN, // X+ / Joint1+
AN_DOWN, // X- / Joint1-
BP_DOWN, // Y+ / Joint2+
BN_DOWN, // Y- / Joint2-
CP_DOWN, // Z+ / Joint3+
CN_DOWN, // Z- / Joint3-
DP_DOWN, // R+ / Joint4+
DN_DOWN, // R- / Joint4-
LP_DOWN, // L+. Only when the parameter isJoint=1, the LP_DOWN is available
LN_DOWN // L-. Only when the parameter isJoint=1, the LN_DOWN is available
);
TJOGModel = (
COORDINATE_MODEL,
JOINT_MODEL
);
TIOFunction = (
IOFunctionDummy, // Do not config function
IOFunctionPWM, // PWM Output
IOFunctionDO, // IO Output
IOFunctionDI, // IO Intput
IOFunctionADC // AD Input
);
type
TForm8 = class(TForm)
ScaledLayout1: TScaledLayout;
Edit1: TEdit;
Button1: TButton;
Bluetooth1: TBluetooth;
Rectangle1: TRectangle;
Label1: TLabel;
Rectangle2: TRectangle;
Label2: TLabel;
Rectangle3: TRectangle;
Label3: TLabel;
Rectangle4: TRectangle;
Label4: TLabel;
Rectangle5: TRectangle;
Label5: TLabel;
Label6: TLabel;
Rectangle6: TRectangle;
Rectangle7: TRectangle;
Rectangle8: TRectangle;
Label7: TLabel;
Label8: TLabel;
Rectangle9: TRectangle;
Rectangle10: TRectangle;
Label10: TLabel;
Rectangle11: TRectangle;
Label11: TLabel;
Rectangle12: TRectangle;
Label12: TLabel;
Rectangle13: TRectangle;
Label13: TLabel;
Button2: TButton;
Rectangle14: TRectangle;
Button3: TButton;
Rectangle15: TRectangle;
Button4: TButton;
Rectangle16: TRectangle;
Rectangle17: TRectangle;
Rectangle18: TRectangle;
Rectangle19: TRectangle;
Rectangle20: TRectangle;
Rectangle21: TRectangle;
Rectangle22: TRectangle;
Rectangle23: TRectangle;
Rectangle24: TRectangle;
Rectangle25: TRectangle;
Rectangle26: TRectangle;
Rectangle27: TRectangle;
Rectangle28: TRectangle;
Rectangle29: TRectangle;
Button5: TButton;
Button6: TButton;
Button7: TButton;
Button8: TButton;
Button9: TButton;
Button10: TButton;
Button11: TButton;
Button12: TButton;
ComboBox1: TComboBox;
Button13: TButton;
Button14: TButton;
Button15: TButton;
Label9: TLabel;
Rectangle30: TRectangle;
Rectangle31: TRectangle;
Label14: TLabel;
Rectangle32: TRectangle;
Label15: TLabel;
Rectangle33: TRectangle;
Label16: TLabel;
Rectangle35: TRectangle;
Label18: TLabel;
Rectangle36: TRectangle;
Label19: TLabel;
Rectangle37: TRectangle;
Label20: TLabel;
Rectangle34: TRectangle;
Label17: TLabel;
Rectangle38: TRectangle;
Label21: TLabel;
Timer2: TTimer;
procedure Button1Click(Sender: TObject);
procedure Button2Click(Sender: TObject);
procedure Button4Click(Sender: TObject);
procedure Button5MouseDown(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Single);
procedure Button5MouseUp(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Single);
procedure Button13Click(Sender: TObject);
procedure Button14Click(Sender: TObject);
procedure Button15Click(Sender: TObject);
procedure ComboBox1Change(Sender: TObject);
procedure Button3Click(Sender: TObject);
procedure FormCreate(Sender: TObject);
procedure Timer2Timer(Sender: TObject);
private
{ private 宣言 }
public
{ public 宣言 }
end;
var
Form8: TForm8;
ADevice : TBluetoothDevice;
ASocket : TBluetoothSocket;
AlarmCode : array of string = [
'$00', 'ERR_COMMON_RESET',
'$10', 'ERR_PLAN_INV_SINGULARITY',
'$11', 'ERR_PLAN_INV_CALC',
'$12', 'ERR_PLAN_INV_LIMIT',
'$13', 'ERR_PLAN_PUSH_DATA_REPEAT',
'$14', 'ERR_PLAN_ARC_INPUT_PARAM',
'$15', 'ERR_PLAN_JUMP_PARAM',
'$20', 'ERR_MOVE_INV_SINGULARITY',
'$21', 'ERR_MOVE_INV_CALC',
'$22', 'ERR_MOVE_INV_LIMIT',
'$30', 'ERR_OVERSPEED_AXIS1',
'$31', 'ERR_OVERSPEED_AXIS2',
'$32', 'ERR_OVERSPEED_AXIS3',
'$33', 'ERR_OVERSPEED_AXIS4',
'$40', 'ERR_LIMIT_AXIS1_POS',
'$41', 'ERR_LIMIT_AXIS1_NEG',
'$42', 'ERR_LIMIT_AXIS2_POS',
'$43', 'ERR_LIMIT_AXIS2_NEG',
'$44', 'ERR_LIMIT_AXIS3_POS',
'$45', 'ERR_LIMIT_AXIS3_NEG',
'$46', 'ERR_LIMIT_AXIS4_POS',
'$47', 'ERR_LIMIT_AXIS4_NEG',
'$48', 'ERR_LIMIT_AXIS23_POS',
'$49', 'ERR_LIMIT_AXIS23_NEG',
'$50', 'ERR_LOSE_STEP_AXIS1',
'$51', 'ERR_LOSE_STEP_AXIS2',
'$52', 'ERR_LOSE_STEP_AXIS3',
'$53', 'ERR_LOSE_STEP_AXIS4' ];
//runFlag : boolean; // 非常停止通知、検出用
//tmBusy : boolean; // インターバルタイマーで処理中
CMDMODE : integer;
GB_queuedCmdIndex : UInt64;
BTDeviceHead : string;
const
// SPP(Serial Port Profile) による通信のUUID
ServiceUUID = '{00001101-0000-1000-8000-00805F9B34FB}';
// 検索する BT デバイス名
// デフォルト 'EasyBT' を 'RBT-001' に変更している
//BTDeviceHead = 'RBT-001';
// プロトタイプ
function RBTReceiveData(ASocket: TBluetoothSocket; var readData: TBytes; ATimeout: Cardinal): integer;
function GetPose(ASocket: TBluetoothSocket; var x, y, z, r: single; var a0, a1, a2, a3: single): boolean;
implementation
{$R *.fmx}
function SetIOMultiplexing(ASocket: TBluetoothSocket; ioAddress: Byte; ioFunction: Byte): boolean;
// 多重入出力を設定
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 130; // id;
B[4] := 1; // ctrl r/w = W, isQueued = False;
B[5] := ioAddress; // EIO Num(1..20)
B[6] := ioFunction; // TIOFunction,
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 130);
end;
end;
function SetPTPCommonParams(ASocket: TBluetoothSocket;
velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 8; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 83; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[5 + i] := velocityRatio.Bytes[i];
for i := 0 to 3 do B[9 + i] := accelerationRatio.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 83);
end;
end;
function GetIODI(ASocket: TBluetoothSocket; ioAddress: Byte; var ioValue: Byte):boolean;
// GetIODI 入力状態を取得
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 133; // id;
B[4] := 0; // ctrl r/w = r, isQueued = False;
B[5] := ioAddress;
B[6] := 0;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 133) and (B[5] = ioAddress);
if result then
ioValue := B[6];
end;
end;
function GetIODO(ASocket: TBluetoothSocket; ioAddress: Byte; var ioLevel: Byte): boolean;
// 出力状態を取得
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, 8);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 131; // id;
B[4] := 0; // ctrl r/w = r, isQueued = False;
B[5] := ioAddress;
B[6] := 0;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 131) and (B[5] = ioAddress);
if result then
ioLevel := B[6];
end;
end;
function SetIODO(ASocket: TBluetoothSocket; ioAddress: Byte; ioLevel: Byte): boolean;
// 出力
var
checkSum : Byte;
i, len : integer;
B : TBytes;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len= 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len= 2 + Payload Len;
B[3] := 131; // id;
B[4] := 1; // ctrl r/w = W, isQueued = False;
B[5] := ioAddress; // EIO Num(1..20)
B[6] := ioLevel; // Level output 0-Low level 1-High level
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 131);
end;
end;
function GetPTPCommonParams(ASocket: TBluetoothSocket;
var velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数取得
var
B : TBytes;
len, i : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 83; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $AD;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 83);
if result then begin
for i := 0 to 3 do begin
velocityRatio.Bytes[i] := B[5 + i];
accelerationRatio.Bytes[i] := B[9 + i];
end;
end;
end;
end;
function SetJOGCommonParams(ASocket: TBluetoothSocket;
velocityRatio, accelerationRatio: single): boolean;
// JOG 移動 速度係数、加速度係数設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 8; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 72; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := velocityRatio.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := accelerationRatio.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 72);
end;
end;
function GetJOGCommonParams(ASocket: TBluetoothSocket;
var velocityRatio, accelerationRatio: single): boolean;
// PTP 移動 速度係数、加速度係数取得
var
B : TBytes;
len, i : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 72; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $B8;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 72);
if result then begin
for i := 0 to 3 do begin
velocityRatio.Bytes[i] := B[5 + i];
accelerationRatio.Bytes[i] := B[9 + i];
end;
end;
end;
end;
function SetSetPTPCoordinateParams(ASocket: TBluetoothSocket;
xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single): boolean;
// PTP LINE 移動 速度、加速度設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 16; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 81; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := xyzVelocity.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := rVelocity.Bytes[i];
for i := 0 to 3 do B[13 + i] := xyzAcceleration.Bytes[i];
for i := 0 to 3 do B[17 + i] := rAcceleration.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 81);
end;
end;
function GetPTPCoordinateParams(ASocket: TBluetoothSocket;
var xyzVelocity, rVelocity, xyzAcceleration, rAcceleration: single): boolean;
var
i, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 81; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $AF;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 81);
if result then begin
for i := 0 to 3 do begin
xyzVelocity.Bytes[i] := B[ 5 + i];
rVelocity.Bytes[i] := B[ 9 + i];
xyzAcceleration.Bytes[i] := B[13 + i];
rAcceleration.Bytes[i] := B[17 + i];
end;
end;
end;
end;
function SetJOGCoordinateParams(ASocket: TBluetoothSocket;
velocity, acceleration: array of single): boolean;
// Jog移動 X, Y, Z R 軸の速度と加速度
var
B : TBytes;
len, i, j : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 32; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 71; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for j := 0 to 3 do begin
for i := 0 to 3 do begin
B[ 5 + j * 4 + i] := velocity[j].Bytes[i]; // X,Y, Z, R axis
B[21 + j * 4 + i] := acceleration[j].Bytes[i];
end;
end;
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 71);
end;
end;
function GetJOGCoordinateParams(ASocket: TBluetoothSocket;
var velocity: array of single; var acceleration: array of single): boolean;
var
i, j, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 71; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $B9;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 71);
if result then begin
for j := 0 to 3 do begin
for i := 0 to 3 do begin
velocity[j].Bytes[i] := B[ 5 + j * 4 + i];
acceleration[j].Bytes[i] := B[21 + j * 4 + i];
end;
end;
end;
end;
end;
function SetQueuedCmdClear(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 245; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0A;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 245);
end;
end;
function SetQueuedCmdStartExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len +4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 240; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0F;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 240);
end;
end;
function SetDeviceName(ASocket: TBluetoothSocket; const deviceName: string): boolean;
// デバイス名をセット
var
B, temp : TBytes;
len, i, n : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
temp := TEncoding.ANSI.GetBytes(deviceName);
n := Length(temp);
len := 2 + n; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 1; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to n - 1 do B[5 + i] := temp[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
// チェックサム
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 1);
end;
end;
function GetDeviceName(ASocket: TBluetoothSocket; var deviceName: string): boolean;
var
B : TBytes;
len : integer;
i : Integer;
begin
result := false;
deviceName := '';
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 1; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $FF;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 1);
if result then begin
for i := 5 to len - 2 do
deviceName := deviceName + Char(B[i]);
end;
end;
end;
function GetDeviceSN(ASocket: TBluetoothSocket; var deviceSN: string): boolean;
var
B : TBytes;
len : integer;
i : Integer;
begin
result := false;
deviceSN := '';
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 0; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $00;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 0);
if result then begin
for i := 5 to len - 2 do
deviceSN := deviceSN + Char(B[i]);
end;
end;
end;
function GetAlarmsState(ASocket: TBluetoothSocket;
var isAlarm: boolean; var res: string): boolean;
// アラーム取得
var
alarmsState: array [0..15] of Byte;
B : TBytes;
len : integer;
i, j, idx, k : Integer;
begin
result := false;
isAlarm := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 20; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
// チェックサム
B[5] := $EC;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 20);
if result then begin
for i := 0 to 15 do
alarmsState[i] := B[5 + i];
for i := 1 to 15 do begin // alarmsState[0] は無視
if alarmsState[i] > 0 then begin
isAlarm := true;
break;
end;
end;
end;
end;
// アラーム内容（文字列）を作成
if isAlarm then begin
for i := 1 to 15 do begin // alarmsState[0] は無視
for j := 0 to 7 do begin
if (alarmsState[i] and (1 shl j)) > 0 then begin
idx := i * 8 + j;
res := '';
for k := 0 to Length(AlarmCode) div 2 - 1 do begin
if idx = StrToInt(AlarmCode[k * 2]) then
res := res + AlarmCode[k * 2 + 1] + #09; // TAB を追加
end;
res := Trim(res); // 最後の #09 を削除
end;
end;
end;
end;
end;
function ClearAllAlarmsState(ASocket: TBluetoothSocket): boolean;
// アラームクリア
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 20; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $EB;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 20);
end;
end;
function SetQueuedCmdStopExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 241; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0E;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 241);
end;
end;
function SetQueuedCmdForceStopExec(ASocket: TBluetoothSocket): boolean;
var
B : TBytes;
len : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 242; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
// チェックサム
B[5] := $0D;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 242);
end;
end;
function GetQueuedCmdCurrentIndex(ASocket: TBluetoothSocket;
var queuedCmdCurrentIndex: Uint64): boolean;
// コマンドキューの現在の位置
var
B : TBytes;
len, i : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 246; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $0A;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 2000);
result := (len = B[2] + 4) and (B[3] = 246);
if result then begin
queuedCmdCurrentIndex := 0;
for i := 0 to 7 do
queuedCmdCurrentIndex :=
queuedCmdCurrentIndex or (Uint64(B[5 + i]) shl (i * 8));
end;
end;
end;
function SetHomeParams(ASocket: TBluetoothSocket; x, y, z, r: single): boolean;
// HOME 位置設定
var
B : TBytes;
len, i : integer;
checkSum : integer;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 16; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 30; // id;
B[4] := 1; // ctrl r/w($01) = W, isQueued($02) = false;
for i := 0 to 3 do B[ 5 + i] := x.Bytes[i];
for i := 0 to 3 do B[ 9 + i] := y.Bytes[i];
for i := 0 to 3 do B[13 + i] := z.Bytes[i];
for i := 0 to 3 do B[17 + i] := r.Bytes[i];
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 30);
end;
end;
function GetHomeParams(ASocket: TBluetoothSocket; var x, y, z, r: single): boolean;
// ホーム座標を取得
var
B : TBytes;
len, i : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 30; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $E2;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 30);
if result then begin
for i := 0 to 3 do begin
x.Bytes[i] := B[ 5 + i];
y.Bytes[i] := B[ 9 + i];
z.Bytes[i] := B[13 + i];
r.Bytes[i] := B[17 + i];
end;
end;
end;
end;
function SetHomeCmdEx(ASocket: TBluetoothSocket): boolean;
// HomeCmd ホーミング
var
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 4; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 31; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
// ダミー(4バイト)
for i := 0 to 3 do B[5 + i] := 0;
// チェックサム
B[9] := $DE;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 31);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex)
else
break;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
end;
end;
end;
end;
function SetHomeCmd(ASocket: TBluetoothSocket;var queuedCmdIndex : Uint64): boolean;
// HomeCmd ホーミング
var
i, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 4; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 31; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
// ダミー(4バイト)
for i := 0 to 3 do B[5 + i] := 0;
// チェックサム
B[9] := $DE;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 31);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
end;
end;
end;
function SetPTPCmd(ASocket: TBluetoothSocket; ptpMode: Byte; toX, toY, toZ, toR: single): boolean;
// PTPCmd
var
checkSum : Byte;
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 17; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 84; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
B[5] := ptpMode;// PTPMode 2= MOVL_XYZ
for i := 0 to 3 do B[ 6 + i] := toX.Bytes[i]; // X
for i := 0 to 3 do B[10 + i] := toY.Bytes[i]; // Y
for i := 0 to 3 do B[14 + i] := toZ.Bytes[i]; // Z
for i := 0 to 3 do B[18 + i] := toR.Bytes[i]; // R
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 84);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex)
else
break;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
end;
end;
end;
end;
function SetJOGCmd(ASocket: TBluetoothSocket; jogMode: Byte; jogCommand: Byte): boolean;
// JogCmd
// jogMode 0 で X, Y, Z 方向に動く。方向は、 jogCommand で指定
// jog 開始は、方向を指定。停止するまで動き続ける
// Jog 停止は、jogCommand に 0 を指定。
// スピードが速すぎると、使いにくい
var
checkSum : Byte;
i, len : integer;
B : TBytes;
queuedCmdIndex : Uint64;
executedCmdIndex : Uint64;
x, y, z, r, a0, a1, a2, a3 : single;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 73; // id;
B[4] := 3; // ctrl r/w = W, isQueued = True;
B[5] := jogMode; // Jog mode 0-coordinate jog 1-Joint jog
B[6] := jogCommand; // Jog command(Value range0..8)
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 73);
if result then begin
queuedCmdIndex := 0;
for i := 0 to 7 do
queuedCmdIndex := queuedCmdIndex or (Uint64(B[5 + i]) shl (i * 8));
executedCmdIndex := 0;
while result and (executedCmdIndex < queuedCmdIndex) do begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
if result then
result := GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex)
else
break;
end;
if result then begin
result := GetPose(ASocket, x, y, z, r, a0, a1, a2, a3);
end;
end;
end;
end;
function GetEndEffectorSuctionCup(ASocket: TBluetoothSocket;
var isCtrlEnable: Byte; var isSucked: Byte): boolean;
// 吸引カップの状態を取得
var
B : TBytes;
len : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 62; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
B[5] := $C2; // checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 62);
if result then begin
isCtrlEnable := B[5];
isSucked := B[6];
end;
end;
end;
function SetEndEffectorSuctionCup(ASocket: TBluetoothSocket; ctrl, suck: Byte):boolean;
// 吸引カップ ON/OFF
var
B : TBytes;
len, i : integer;
checkSum : Byte;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 62; // id;
B[4] := 3; // ctrl r/w($01) = W, isQueued($02) = True;
B[5] := ctrl; // CtrlEnable
B[6] := suck; // Suck
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 62);
end;
end;
function GetEndEffectorGripper(ASocket: TBluetoothSocket;
var isCtrlEnable: Byte; var isGripped: Byte): boolean;
// グリッパーの状態を取得
var
B : TBytes;
len : integer;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 63; // id;
B[4] := 0; // ctrl r/w($01) = R, isQueued($02) = false;
B[5] := $C1; // checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 63);
if result then begin
isCtrlEnable := B[5];
isGripped := B[6];
end;
end;
end;
function SetEndEffectorGripper(ASocket: TBluetoothSocket; ctrl, grip: Byte):boolean;
// 吸引カップ ON/OFF
var
B : TBytes;
len, i : integer;
checkSum : Byte;
begin
result := False;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 2; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 63; // id;
B[4] := 3; // ctrl r/w($01) = W, isQueued($02) = True;
B[5] := ctrl; // CtrlEnabled
B[6] := grip; // Grip
// チェックサム
checkSum := 0;
for i := 3 to len + 2 do checkSum := checkSum + B[i];
checkSum := $100 - checkSum;
B[len + 3] := checkSum;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 500);
result := (len = B[2] + 4) and (B[3] = 63);
end;
end;
function GetPose(ASocket: TBluetoothSocket;
var x, y, z, r: single; var a0, a1, a2, a3: single): boolean;
// GetPose 現在値を取得
var
i, len : integer;
B : TBytes;
begin
result := false;
if (ASocket <> nil) and ASocket.Connected then begin
len := 2 + 0; // len = 2 + Payload Len;
SetLength(B, len + 4);
B[0] := $AA;
B[1] := $AA;
B[2] := len; // len = 2 + Payload Len;
B[3] := 10; // id;
B[4] := 0; // ctrl r/w = R, isQueued = False;
// チェックサム
B[5] := $F6;
// 送信
ASocket.SendData(B);
// 受信
len := RBTReceiveData(ASocket, B, 1000);
result := (len = B[2] + 4) and (B[3] = 10);
if result then begin
for i := 0 to 3 do begin
x.Bytes[i] := B[ 5 + i];
y.Bytes[i] := B[ 9 + i];
z.Bytes[i] := B[13 + i];
r.Bytes[i] := B[17 + i];
a0.Bytes[i] := B[21 + i];
a1.Bytes[i] := B[25 + i];
a2.Bytes[i] := B[29 + i];
a3.Bytes[i] := B[33 + i];
end;
end;
end;
end;
// Dobot Magician アーム角度を取得
// x, y, z : エンドエフェクタの位置
// A1, A2, A3 : 主軸の回転角度、アーム 1 の傾斜角度, アーム 2 の角度（アーム 1 からの傾斜角度）
// J1, J2, J3 : 主軸の回転角度、アーム 1 の角度（垂直で 0）アーム 2 の傾斜角度（水平で 0）
function dobotArmAngle(x, y, z: double; var A1, A2, A3, J1, J2, J3 : double): boolean;
// uses ,,,, math;
var
th1, th2, th3 : double;
phi, l, ld : double;
L1, L2, L3, L4 : double;
begin
L1 := 0.0; // アーム基点の高さ(Dobot Magician の場合: 0)
L2 := 135.0; // アーム１の辺の長さ
L3 := 146.74; // アーム２の辺の長さ(147.0mm)
L4 := 60.0; // エンドエフェクタの突き出し部分（必ず水平である）
result := false;
// X-Y平面上のアーム部分の長さ
l := sqrt(x * x + y * y) - L4 ;
if (l > L4) then begin
// X-Y 平面上のアームの角度
// Dobot の場合、-125～ +125 度であること
th1 := arctan2(y, x);
// アーム１、２の底辺の長さ
ld := sqrt(l * l + (z - L1) * (z - L1));
result := (ld > 0) and (ld < (L2 + L3));
if result then begin
// アーム基点から先端への仰角
// アーム１、アーム２底辺の三角形の基点側の内角
phi := arctan2((z - L1), l);
// アーム１の角度
th2 := phi + arccos((ld * ld + L2 * L2 - L3 * L3) / (2.0 * ld * L2));
// アーム２のアーム１からの角度
th3 := arcsin((ld * ld - L2 * L2 - L3 * L3) / (2.0 * L2 * L3)) + PI / 2.0;
// ラジアン->デグリ換算
A1 := th1 * 180.0 / PI;
A2 := th2 * 180.0 / PI ;
A3 := th3 * 180.0 / PI ;
// Dobot Arm の角度表示に変換
J1 := A1;
// アーム１の角度（垂直で 0 度、前に傾斜で＋方向）
// Dobot の場合 -5.0 ～ +84.0 まで
J2 := 90.0 - A2;
// アーム２の角度（水平で 0 度、前方に下がる方向が＋）
// Dobot の場合、 -15.0 ～ +75.0 まで
J3 := 180.0 - A2 - A3;
end;
end;
end;
// 可動範囲内にあるか
function dobotCheckXYZ(x, y, z : double): boolean;
var
A1, A2, A3, J1, J2, J3 : double;
dist1, dist2 : double;
begin
result := false;
if dobotArmAngle(x, y, z, A1, A2, A3, J1, J2, J3) then begin
dist1 := sqrt(x * x + y * y);
dist2 := sqrt((dist1 - 60.0) * (dist1 - 60.0) + z * z);
// (J2 < 89.00) : 大きくすると、下側に（初期値：83.99）
// (J3 < 75.00) : 大きくすると、中心方向に（初期値：75.00)
result := (J1 > -125.0 ) and (J1 < 125.0) and
(J2 > -4.99) and (J2 < 89.9) and
(J3 > -14.9) and (J3 < 89.9) and
(dist2 <= 268.5);
// 268 はアームが最大に伸びた時の長さ（268 ～ 272mm）一直線にはならないため、制限が必要
end;
end;
function dobotSetupIO(ASocket: TBluetoothSocket): boolean;
// IO セットアップ
begin
result := false;
if (ASocket = nil) or not ASocket.Connected then exit;
SetIOMultiplexing(ASocket, 18, Ord(TioFunction.IOFunctionDO));
SetIOMultiplexing(ASocket, 19, Ord(TioFunction.IOFunctionDI));
SetIOMultiplexing(ASocket, 20, Ord(TioFunction.IOFunctionDI));
result := true;
end;
function RBTReceiveData(ASocket: TBluetoothSocket;
var readData: TBytes; ATimeout: Cardinal): integer;
// Dobot(Bluetooth) からの応答
var
AData : TBytes;
i : integer;
Ticks : Cardinal;
idx : integer;
loop : boolean;
cnt : integer;
begin
cnt := 0; idx := 0;
Setlength(readData, 64); // 大き目に用意
Ticks := TThread.GetTickCount;
loop := True;
while loop and (cnt < 500) do begin
//Sleep(1);
AData := ASocket.ReceiveData;
if Length(AData) > 0 then begin
for i := 0 to Length(AData) - 1 do begin
readData[idx] := AData[i];
// 受信バッファの長さを再セット
if idx = 2 then SetLength(readData, readData[2] + 4);
Inc(idx);
if idx >= Length(readData) then begin
loop := False;
break;
end;
end;
end;
Inc(cnt);
if loop then
loop := TThread.GetTickCount - Ticks < ATimeout;
end;
result := idx;
end;
function searchRBT(const deviceHead : string): boolean;
// Bluetooth デバイスを検索
var
ABluetoothManager : TBluetoothManager;
APairedDevices : TBluetoothDeviceList;
ADevice : TBluetoothDevice;
idx, i : integer;
begin
result := false;
if (ASocket = nil) or not ASocket.Connected then begin
try
ABluetoothManager := TBluetoothManager.Current;
if ABluetoothManager.ConnectionState = TBluetoothConnectionState.Connected then begin
// 過去にペアリングされたデバイスの一覧から、ターゲット を探す
APairedDevices := ABluetoothManager.GetPairedDevices;
if APairedDevices.Count > 0 then begin
idx := -1;
for i := 0 to APairedDevices.Count -1 do begin
if (BTDeviceHead = APairedDevices[i].DeviceName) then begin
idx := i;
break; // リストアップを終了
end;
end;
if idx >= 0 then begin
ADevice := APairedDevices[idx];
if ADevice <> nil then begin
ASocket := ADevice.CreateClientSocket(StringToGUID(ServiceUUID), False);
if ASocket <> nil then begin
// 接続
ASocket.Connect;
result := ASocket.Connected;
end;
end;
end;
end;
end;
except
on E : Exception do begin
ShowMessage(E.Message);
end;
end;
end;
end;
procedure TForm8.Button13Click(Sender: TObject);
// 吸引カップ 吸着・破壊反転
var
ctrl, suck: Byte;
begin
if GetEndEffectorSuctionCup(ASocket, ctrl, suck) then begin
SetEndEffectorSuctionCup(ASocket, 1, not suck and 1); // 反転
end;
end;
procedure TForm8.Button14Click(Sender: TObject);
// グリッパー開・閉反転
var
ctrl, grip: Byte;
begin
if GetEndEffectorGripper(ASocket, ctrl, grip) then begin
SetEndEffectorGripper(ASocket, 1, not grip and 1); // 反転
end;
end;
procedure TForm8.Button15Click(Sender: TObject);
// グリッパー、吸引カップ停止
begin
SetEndEffectorGripper(ASocket, 0, 0); // OFF
end;
procedure TForm8.Button1Click(Sender: TObject);
// 接続
var
dobotSN, dobotName : string;
velocity : array [0..3] of single;
acceleration : array [0..3] of single;
i : integer;
IniFile: TMemIniFile; // uses ,,, System.IniFiles;
begin
if Edit1.Text = '' then begin
BTDeviceHead := 'RBT-001';
Edit1.Text:= BTDeviceHead;
end
else
BTDeviceHead := Edit1.Text;
IniFile := TMemIniFile.Create(System.IOUtils.TPath.Combine(
System.IOUtils.TPath.GetDocumentsPath, 'dobotBT.ini'), TEncoding.UTF8);
// uses ,,, System.IOUtils;
with IniFile do begin
try
WriteString('RBT-001', 'DeviceName', Edit1.Text);
UpdateFile;
finally
Free;
end;
end;
if searchRBT(BTDeviceHead) then begin
//Edit1.Text:= BTDeviceHead;
SetQueuedCmdClear(ASocket);
SetQueuedCmdStartExec(ASocket);
if GetDeviceSN(ASocket, dobotSN) then Label5.Text := dobotSN;
if GetDeviceName(ASocket, dobotName) then Label6.Text := dobotName;
for i := 0 to 3 do begin // JOG 初期速度
velocity[i] := 100;
acceleration[i] := 100;
end;
SetJOGCoordinateParams(ASocket, velocity, acceleration);
SetJOGCommonParams(ASocket, 100, 100);
CMDMODE := 202; // モニタモード
Timer2.Enabled := true;
end;
end;
procedure TForm8.Button2Click(Sender: TObject);
// アラームクリア
begin
ClearAllAlarmsState(ASocket);
end;
procedure TForm8.Button3Click(Sender: TObject);
// 非常停止
begin
CMDMODE := 0;
Timer2.Enabled := False;
if SetQueuedCmdForceStopExec(ASocket) then begin
ASocket.Close; // 切断
ASocket := nil;
Button1Click(self); // 再接続
end;
end;
procedure TForm8.Button4Click(Sender: TObject);
// Home
begin
CMDMODE := 101;
Timer2.Enabled := True;
end;
procedure TForm8.Button5MouseDown(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Single);
// JOG 開始
var
jogMode, jogCommand : Byte;
btn : TButton;
begin
jogMode := Ord(TJOGModel.COORDINATE_MODEL); // =0
btn := Sender as TButton;
if btn <> nil then begin
jogCommand := StrToIntDef(Copy(btn.Name, 7), 0) - 4;
if jogCommand > 0 then begin
SetJogCmd(ASocket, jogMode, jogCommand);
end;
end;
end;
procedure TForm8.Button5MouseUp(Sender: TObject; Button: TMouseButton;
Shift: TShiftState; X, Y: Single);
// JOG 停止
var
jogMode, jogCommand : Byte;
begin
jogMode := Ord(TJOGModel.COORDINATE_MODEL); // =0
jogCommand := Ord(TJOGMode.IDEL); // =0
SetJogCmd(ASocket, jogMode, jogCommand);
end;
procedure TForm8.ComboBox1Change(Sender: TObject);
// JOG の速度をセット
var
spd : single;
velocity : array [0..3] of single;
acceleration : array [0..3] of single;
i : integer;
begin
spd := 100;
with ComboBox1 do begin
if ItemIndex >= 0 then begin
spd := StrToFloatDef(Items[ItemIndex], 100);
end;
end;
// Jog 移動の速度、加速度をセット
for i:= 0 to 3 do begin
velocity[i] := spd;
acceleration[i] := spd;
end;
SetJOGCoordinateParams(ASocket, velocity, acceleration);
SetJOGCommonParams(ASocket, 100, 100);
end;
procedure TForm8.FormCreate(Sender: TObject);
// フォーム生成
var
btn : TButton;
lbl : TLabel;
i : integer;
//IniFile: TMemIniFile; // uses ,,, System.IniFiles;
begin
for i := 1 to 12 do begin
btn := FindComponent('Button' + IntToStr(i)) as TButton;
if btn <> nil then
btn.Align := TAlignLayout.Client;
end;
for i := 13 to 15 do begin
btn := FindComponent('Button' + IntToStr(i)) as TButton;
if btn <> nil then btn.Align := TAlignLayout.Client;
end;
for i := 1 to 4 do begin
lbl := FindComponent('Label' + IntToStr(i)) as TLabel;
if lbl <> nil then lbl.Align := TAlignLayout.Client;
lbl := FindComponent('Label' + IntToStr(i+9)) as TLabel;
if lbl <> nil then lbl.Align := TAlignLayout.Client;
end;
for i := 1 to 8 do begin
lbl := FindComponent('Label' + IntToStr(i)) as TLabel;
if lbl <> nil then lbl.text := '';
lbl := FindComponent('Label' + IntToStr(i+13)) as TLabel;
if lbl <> nil then begin
if i > 4 then lbl.text := '';
lbl.Align := TAlignLayout.Client;
end;
end;
// 縦画面に固定
Application.FormFactor.Orientations :=
[TFormOrientation.Portrait, TFormOrientation.InvertedPortrait];
IniFile := TMemIniFile.Create(System.IOUtils.TPath.Combine(
System.IOUtils.TPath.GetDocumentsPath, 'dobotBT.ini'), TEncoding.UTF8);
// uses ,,, System.IOUtils;
with IniFile do begin
try
Edit1.Text := ReadString('RBT-001', 'DeviceName', '');
finally
Free;
end;
end;
end;
procedure TForm8.Timer2Timer(Sender: TObject);
var
executedCmdIndex: UInt64;
pos, ang : array [0..3] of single;
i : integer;
lbl : TLabel;
isAlarm : boolean;
res : string;
Ticks : Cardinal;
dist1, dist2 : double;
begin
case CMDMODE of
// HOME
101:begin
Timer2.Enabled := False;
if SetHomeCmd(ASocket, GB_queuedCmdIndex) then begin
CMDMODE := 102; // GetPose...
end
else
CMDMODE := 202; // モニタを続ける
Timer2.Enabled := True;
end;
// GetPose
102,202:begin
Timer2.Enabled := False;
Ticks := TThread.GetTickCount;
if GetPose(ASocket, pos[0], pos[1], pos[2], pos[3], ang[0], ang[1], ang[2], ang[3]) then begin
dist1 := sqrt(pos[0] * pos[0] + pos[1] * pos[1]);
dist2 := sqrt((dist1 - 60.0) * (dist1 - 60.0) + pos[2] * pos[2]);
for i := 0 to 3 do begin
lbl := FindComponent('Label' + IntToStr(1 + i)) as TLabel;
if (lbl <> nil) then begin
if (lbl.Text <> Format('%.2f ', [pos[i]])) then
lbl.Text := Format('%.2f ', [pos[i]]);
end;
end;
// アーム角度
for i := 0 to 3 do begin
lbl := FindComponent('Label' + IntToStr(18 + i)) as TLabel;
if (lbl <> nil) then begin
case i of
0:begin
if (lbl.Text <> Format('%.2f ', [ang[i]])) then
lbl.Text := Format('%.2f ', [ang[i]]);
if (ang[i] > -125) and (ang[i] < 125) then
lbl.TextSettings.FontColor := TAlphaColors.Black
else
lbl.TextSettings.FontColor := TAlphaColors.Red;
end;
1:begin
if (lbl.Text <> Format('%.2f ', [ang[1]])) then
lbl.Text := Format('%.2f ', [ang[i]]);
if (ang[1] > -4.99) and (ang[1] < 89.9) then
lbl.TextSettings.FontColor := TAlphaColors.Black
else
lbl.TextSettings.FontColor := TAlphaColors.Red;
end;
2:begin
if (lbl.Text <> Format('%.2f ', [ang[2]])) then
lbl.Text := Format('%.2f ', [ang[2]]);
if (ang[2] > -14.9) and (ang[2] < 89.9) then
lbl.TextSettings.FontColor := TAlphaColors.Black
else
lbl.TextSettings.FontColor := TAlphaColors.Red;
end;
3: begin
if (lbl.Text <> Format('%.2f ', [dist2])) then
lbl.Text := Format('%.2f ', [dist2]);
if (dist2 > 268.5) then
lbl.TextSettings.FontColor := TAlphaColors.Red
else
lbl.TextSettings.FontColor := TAlphaColors.Black;
end;
end;
end;
end;
// アラーム取得
res := '';
if GetAlarmsState(ASocket, isAlarm, res) then begin
if isAlarm then
// TAB を変換
res := StringReplace(res, #09, ', ', [rfReplaceAll]);
end;
if Label7.Text <> res then Label7.Text := res;
// 計算上の可動範囲の確認
// 可動範囲内か
if not dobotCheckXYZ(pos[0], pos[1], pos[2]) then begin
if Rectangle30.Fill.Color <> TAlphaColorRec.Red then
Rectangle30.Fill.Color := TAlphaColorRec.Red;
end
else begin
if Rectangle30.Fill.Color <> TAlphaColorRec.Lime then
Rectangle30.Fill.Color := TAlphaColorRec.Lime;
end;
Label8.Text := (TThread.GetTickCount - Ticks).ToString + ' msec';
if CMDMODE = 102 then
CMDMODE := 103; // GetQueuedCmdCurrentIndex ...
end
else
CMDMODE := 202; // モニタを続ける
Timer2.Enabled := True;
end;
// GetQueuedCmdCurrentIndex
103:begin
Timer2.Enabled := False;
if GetQueuedCmdCurrentIndex(ASocket, executedCmdIndex) then begin
if executedCmdIndex < GB_queuedCmdIndex then
CMDMODE := 102 // GetPose
else
CMDMODE := 202; // モニタを続ける
end
else
CMDMODE := 202; // モニタを続ける
Timer2.Enabled := True;
end;
end;
end;
end.