RaspberryPi + FE-URT-1でFEETECH社シリアルサーボを動かしてみる

## はじめに

Pythonを使って、FEETECH社シリアルサーボ,SMSシリーズ,SCSシリーズ用インターフェースボード FE-URT-1 からSTS3032サーボを動かしてみた。

## 環境

- [FEETECHサーボ用インターフェイスボード FE-URT-1](https://akizukidenshi.com/catalog/g/g116295/)
- [FEETECHサーボ STS3032](https://akizukidenshi.com/catalog/g/g116311/)
- RaspberryPi5

## STS3032動作モード
STS3032は、FEETECH社が製造する360度回転可能なシリアルバスサーボモーターで、以下4つの動作が可能である。以下4種類の動作をPC(Windows11)およびRaspberryPiから動かしてみる。

|モード　 |内容   |説明   |
|:------:|:-----|:-----|
|0  |サーボモード 　       |360°の範囲で位置制御が可能<br>位置は0～4095の値で指定  |
|1  |連続回転・速度制御モード  |回転速度を指定して制御<br>速度は10～9999の値で設定（負の値で逆回転）  |
|2  |連続回転・トルク制御モード  |トルクを指定して制御<br>Time値を50～1000で設定（負の値で逆回転）  |
|3  |ステップモード       |指定した回転量だけ回転<br>最大±30719ステップ（約7.5回転）まで設定可能  |

## FE-URT-1によるSTS3032動作確認(PC編 Windows11)
PC --[USB]--> FE-URT-1 --> STS3032
### ■ 接続確認
- STEP1．[FE-URT-1 マニュアル](https://akizukidenshi.com/goodsaffix/feetech_digital_servo_20220729.pdf)に従ってPC->FE-URT-1->STS3032を接続する

- STEP2．FD.exe起動後、Com/BaudRをマニュアルに従って設定し「Open」をクリックして通信を確立する
    - ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/905d0e78-d474-421c-a0d1-64b6d1b9537b.png)

- STEP3．「Search」をクリックし接続されたサーボを検索する
    - ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/a55c1731-81f3-4d25-b430-24fdd681708e.png)

### ■ 動作確認

- Operation Mode 0
    - STEP1．Programmingを表示し、WorkMode=0にする
    - STEP2．Debugに戻り「Write」を選択してスライダーを動かしてみる
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/a315ce55-3d77-4031-b8d8-311c2a2485a6.png)
    :::note info
    Mode=0では[Goal Position(Address=42)]で角度を指定している

- Operation Mode 1
    - STEP1．Programmingを表示し、WorkMode=1にする
    - STEP2．Debugに戻り「Speed」(±10～9999)に値を設定し、Setを押下する
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/7b9dcd88-8c3a-42bd-b11f-21a0867f193c.png)
    :::note info
    Mode=1では[Goal Velocity(Address=46)]で速度を指定している

- Operation Mode 2
    - STEP1．Programmingを表示し、WorkMode=2にする
    - STEP2．Debugに戻り「Time」(±50～1000)に値を設定し、Setを押下する
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/fbf45d73-a75b-4f84-9446-68938aac6418.png)
    :::note info
    ~~Mode=2では[Present PWM(Address=60)]でトルクを指定している~~
    ↑これ読み取り専用だった。。。ということでFE-URT-1から送信されているデータを確認してみる。
    Time = 500
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/6870a137-058b-4a55-86bd-2c1bbfca8ec1.png)
    Time = -500
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/69c4a575-b468-461a-8e7f-6f9207447267.png)
    ⇒[Goal Acceleration(Address=41)]でトルク指定していた



- Operation Mode 3
    - STEP1．Programmingを表示し、WorkMode=3、Min Position Limit/Max Position Limit=0にする
    - STEP2．Debugに戻り「Goal」に目標量「Speed」に速度を設定し、Setを押下する
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1899244/cbc0538c-3218-46b5-a3cd-df97eb748e31.png)
    :::note info
    Mode=3では[Goal Position(Address=42)]で目標移動量、[Goal Velocity(Address=46)]で速度を指定している


## FE-URT-1によるSTS3032動作確認(RaspberryPi編：USB接続)
RaspberryPi --[USB]--> FE-URT-1 --> STS3032

### ■ 接続確認
- STEP1．FE-URT-1 を Raspberry Pi の USB ポートに接続
- STEP2．ログメッセージ確認->「attached to ttyUSB0」と出ているためOK
    ```shell-session
    ~ $ dmesg | grep ttyUSB*
    usb 1-2: ch341-uart converter now attached to ttyUSB0
    ```

- STEP3．デバイスファイルの確認
    ```shell-session
    ~ $ ls /dev/ttyUSB*
    /dev/ttyUSB0
    ```

- STEP4．通信テスト
    ```shell-session
    ~ $ minicom -b 115200 -D /dev/ttyUSB0
    ```
    ```shell-session
    minicom へようこそ 2.8
    
    オプション: I18n 
    ポート /dev/ttyUSB0, 07:11:23
    
    CTRL-A Z を押すと、説明画面になります。
    
    ```
    ※Ctrl+A -> X で終了

    ```shell-session
    ~ $ git clone https://github.com/AkariGroup/feetech_setup.git
    ~ $ python -m venv --system-site-packages .feetech
    ~ $ source .feetech/bin/activate
    ```

    ```shell-session
    (.feetech) ~ $ python feetech_setup/search_ping.py -p /dev/ttyUSB0
    シリアルポートを開きました。
    シリアルポートをbaudrate: 1000000 にセットしました。feetechサーボを探索します。
    ------------------------------------------------------------
    Feetechサーボが見つかりました。Baudrate: 1000000, ID:001, モデル名:521
    ------------------------------------------------------------
    シリアルポートをbaudrate: 500000 にセットしました。feetechサーボを探索します。
    シリアルポートをbaudrate: 250000 にセットしました。feetechサーボを探索します。
    シリアルポートをbaudrate: 128000 にセットしました。feetechサーボを探索します。
    シリアルポートをbaudrate: 115200 にセットしました。feetechサーボを探索します。
    シリアルポートがbaudrate: 76800に対応していません。スキップします。
    シリアルポートをbaudrate: 57600 にセットしました。feetechサーボを探索します。
    シリアルポートをbaudrate: 38400 にセットしました。feetechサーボを探索します。
    スキャンが完了しました。
    ```

### ■ 動作確認

```shell-session
(.feetech) ~ $ pip install feetech-servo-sdk
(.feetech) ~ $ python feetech_setup/move_test.py -p /dev/ttyUSB0
シリアルポートを開きました。
シリアルポートをbaudrate: 1000000 にセットしました。feetechサーボを探索します。
baudrate: 1000000 にfeetechサーボID: 1を発見しました。

ENTERキーを入力すると、サーボID1が2048に動きます。

ENTERキーを入力をすると、サーボID1が少し動きます。正しいサーボが動くか確認してください。

ENTERキーを入力をするとサーボOFFします。

id:1 サーボOFFしました。
------------------------
テスト完了！
------------------------
```
:::note info
move_test.pyのmain()内で--portオプションがPortHandlerへ渡せていないため、以下の通りコード修正が必要
```diff
- portHandler = PortHandler(DEVICENAME)
+ portHandler = PortHandler(args.port)
```
:::

- Operation Mode 0/1/2/3
    [Github](https://github.com/t-ohno215/feetech)


## FE-URT-1によるSTS3032動作確認(RaspberryPi編：USB接続)
RaspberryPi --[GPIO]--> FE-URT-1 --> STS3032

### ■ 接続確認
- STEP1．以下の通りRaspberryPiとFE-URT-1をGPIOで接続する

    |RPi5(GPIO)  |FE-URT11  |
    |:-----------|----------|
    |pin4 (5v)   |5v        |
    |pin6 (GND)  |GND       |
    |pin8 (Tx)   |TXD       |
    |pin10 (Rx)  |RXD       |
    |None        |DTR       |

<br>

- STEP2．シリアルポートの有効化
    ```shell-session
    ~ $ sudo raspi-config
    ```
    3 Interface Options > I6 Serial Port を選択する。
    - Would you like a login shell to be accessible over serial?
      →Noを選択
    - Would you like the serial port hardware to be enabled?
      →Yesを選択

    再起動する
    ```shell-session
    ~ $ sudo raspi-config
    ```
    - /boot/firmware/config.txtに設定を追記する
    ```
    enable_uart=1
    ```
    再起動する
    ```shell-session
    ~ $ sudo raspi-config
    ```
<br>

- STEP3．確認
    ```shell-session
    ~ $ ls -l /dev/serial0 
    lrwxrwxrwx 1 root root 8  5月 11 20:37 /dev/serial0 -> ttyAMA10
    ~ $ stty -F /dev/ttyAMA10 
    speed 9600 baud; line = 0;
    -brkint -imaxbel
    ```


以降はUSB接続時同様にpythonからデータ送信すればOK。

## 参考
### ドキュメント

- [FEETECH社の公式英語マニュアル](https://www.feetechrc.com/Data/feetechrc/upload/file/20201127/start%20%20tutorial201015.pdf)
- [FEETECH社デジタルサーボの使用方法](https://akizukidenshi.com/goodsaffix/feetech_digital_servo_20220729.pdf)

### サイト

https://github.com/AkariGroup/feetech_setup

https://homemadegarbage.com/sts3032