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;  // アーム1の辺の長さ
  L3 := 146.74; // アーム2の辺の長さ(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);
    // アーム1、2の底辺の長さ
    ld := sqrt(l * l + (z - L1) * (z - L1));
    result := (ld > 0) and (ld < (L2 + L3));
    if result then begin
      // アーム基点から先端への仰角
      // アーム1、アーム2底辺の三角形の基点側の内角
      phi := arctan2((z - L1), l);
      // アーム1の角度
      th2 := phi + arccos((ld * ld + L2 * L2 - L3 * L3) / (2.0 * ld * L2));
      // アーム2のアーム1からの角度
      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;
      // アーム1の角度(垂直で 0 度、前に傾斜で+方向)
      // Dobot の場合 -5.0 ~ +84.0 まで
      J2 := 90.0 - A2;
      // アーム2の角度(水平で 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;  // アーム1の辺の長さ
  L3 := 146.74; // アーム2の辺の長さ(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);
    // アーム1、2の底辺の長さ
    ld := sqrt(l * l + (z - L1) * (z - L1));
    result := (ld > 0) and (ld < (L2 + L3));
    if result then begin
      // アーム基点から先端への仰角
      // アーム1、アーム2底辺の三角形の基点側の内角
      phi := arctan2((z - L1), l);
      // アーム1の角度
      th2 := phi + arccos((ld * ld + L2 * L2 - L3 * L3) / (2.0 * ld * L2));
      // アーム2のアーム1からの角度
      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;
      // アーム1の角度(垂直で 0 度、前に傾斜で+方向)
      // Dobot の場合 -5.0 ~ +84.0 まで
      J2 := 90.0 - A2;
      // アーム2の角度(水平で 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.