Raspberry Pi で USB 加速度・角速度センサを使ってみる

[メカトラックス](https://mechatrax.com/)の[maeda01](https://qiita.com/maeda01)です。

本記事は Code Mercenaries 社の USB 加速度・角速度6軸センサである JoyWarrior56FR1-WP を使用して加速度を測定する例となります。


# 今回使用するもの
* Raspberry Pi 3 Model B
* micro SD カード (32 GB)
* JoyWarrior56FR1-WP : 6軸加速度・角速度センサ
* slee-Pi3 : Raspberry Pi 用電源管理/死活監視モジュール (詳細は [こちら](https://mechatrax.com/products/slee-pi/))
    * slee-Pi3 は必須ではありませんが、Raspberry Pi への安定した電源供給のために使用を推奨します。
    * slee-Pi3 のセットアップについては[弊社 GitHub ページ](https://github.com/mechatrax/slee-pi3/wiki/%E3%82%BD%E3%83%95%E3%83%88%E3%82%A6%E3%82%A7%E3%82%A2#1-%E3%82%BB%E3%83%83%E3%83%88%E3%82%A2%E3%83%83%E3%83%97) をご参照ください。


## JoyWarrior56FR1-WP

このセンサは以下のような特徴を持っています。
* ３軸方向の加速度計、および、3軸方向の角速度計（ジャイロセンサ）として使用できるセンサです。接続方式は USB で簡単に接続できます。

* 加速度については ±2g, ±4g, ±8g, ±16g のいずれかのレンジで測定が可能です。16ビットの分解能を持っているため、±2g レンジでは、最小単位61μgで測定できます。
* 今回は使用しませんが、角速度についても 125 dps, 245 dps, 500 dps, 1000 dps, 2000 dps のレンジで測定が可能です。

* 測定周期 (サンプリングレート) は Normal mode では 833 Hz、High speed mode では 6,444 Hz となっています。

* PCB モジュールがプラスチック製のケースに入っているので多少水がかかるようなところでも使用可能です。

* センサ本体にねじ止め用の穴があります。また、内部に磁石が入っており、鉄製の製品にワンタッチで取り付けられます。

* -10 °C から +85°C の環境で使用できます。

![joywarrior.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/1567be18-8264-068d-5b39-619b3b07cbef.jpeg)

製品ページ

https://www.codemercs.com/en/sensors/joywarrior56fr1-wp


データシート

https://www.codemercs.com/downloads/joywarrior/JW56FR1_Datasheet.pdf





# OSイメージをSDカードに書き込む
Raspberry Pi Imager を使用して OS を micro SD カードに書き込んでいきます。
本記事では Raspberry Pi OS Lite (32bit) を使用します。

![OSLiteをやく.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/fbde3c91-e690-e134-fade-371c25174867.png)





# センサの接続
JoyWarrior56FR1-WP の接続方法は非常に簡単です。Raspberry Pi の USB ポートに挿しこむだけです。

![センサの接続.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/26c3ecbd-e860-f62c-f41e-5008cb06a28d.jpeg)


Raspberry Pi の USB からセンサに +5V が供給されるのでセンサへ別途電源供給は不要です。

# プログラムの作成
JoyWarrior56FR1-WP から加速度を取得するプログラムを作成します。


USB デバイスを扱うためのモジュール ([PyUSB](https://pyusb.github.io/pyusb/)) をインストールします。

```
$ sudo pip install pyusb
```

以下は JoyWarrior56FR1-WP から加速度を取得し、加速度データをターミナルに表示 & csv ファイルに記録するサンプルコードになります。プログラム名は JoyWarrior56FR1.py としています。


```JoyWarrior56FR1.py
#!/usr/bin/python3

import csv
import datetime
import struct
import sys

import usb.core
import usb.util

VID = 0x07c0
PID = 0x111a
ARNG = 0x00
AFILT = 0xC3
MODE = 0x00
RANGE_ACC = 2

def cal_acceleration(n):
    return round((n - 32768) / 32768 * RANGE_ACC * 9.80665, 3)

if __name__ == "__main__":
    dev = usb.core.find(idVendor=VID, idProduct=PID)

    if dev is None:
        raise ValueError("Device not found")

    try:
        dev.detach_kernel_driver(0)
    except Exception:
        pass

    dev.write(2, [0x04, MODE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x00, ARNG, AFILT, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])

    cfg = dev.get_active_configuration()
    intf = cfg[(0, 0)]
    if dev.is_kernel_driver_active(intf.bInterfaceNumber):
        dev.detach_kernel_driver(intf.bInterfaceNumber)

    endpoint = usb.util.find_descriptor(
        intf,
        custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress)
        == usb.util.ENDPOINT_IN
    )

    if endpoint is None:
        raise ValueError('Endpoint not found')

    with open("./JoyWarrior56FR1_test.csv", "w", encoding="utf_8_sig") as f:
        writer = csv.writer(f)
        writer.writerow(["日時", "加速度x", "加速度y", "加速度z"])
        while True:
            output_bytes = endpoint.read(endpoint.wMaxPacketSize).tobytes()
            acc_bytes = struct.unpack("<HHH", output_bytes[:6])
            accx, accy, accz = list(map(cal_acceleration, acc_bytes))
            timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S.%f")
            print(f"加速度x[m/s^2]: {accx} 加速度y[m/s^2]: {accy} 加速度z[m/s^2]: {accz}")
            writer.writerow([timestamp, accx, accy, accz])
            f.flush()

```

このコードを実行すると3軸加速度の値がターミナルに表示されます。また、`JoyWarrior56FR1_test.csv` という名前の csv ファイルが生成されます。

```
$ ls
JoyWarrior56FR1.py
JoyWarrior56FR1_test.csv
```

以下、プログラムに関しての補足です。
* 測定モード (Normal mode / High speed mode) の設定を変数 `MODE` で行います。Normal mode の場合は `0x00`を、High speed mode の場合は `0x01` を入れてください。

* 加速度の測定レンジとフィルタ処理の設定をそれぞれ変数 `ARNG` と変数 `AFILT` で行います。上のサンプルコードでは加速度の測定レンジは ±2g、フィルタは 416.5 Hz のローパスフィルタ、2.08 Hz のハイパスフィルタを設定しています。
ここを変更する場合は[データシート](https://www.codemercs.com/downloads/joywarrior/JW56FR1_Datasheet.pdf)を参考に値を入力してください。また、加速度の測定レンジの設定を変更した場合は変数 `RANGE_ACC`の値を使用するレンジの値 (2, 4, 8, 16) に変更してください。

* ここで出力される3軸加速度の値は JoyWarrior56FR1-WP の座標系での値となっています。JoyWarrior56FR1-WP の座標系については[こちらのサイト](https://www.codemercs.com/downloads/joywarrior/JW56FR1WP_Instruction.pdf) をご参照ください。センサ本体にも x座標と y座標が書かれたシールが貼られています。


![joywarriorのシール.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/e2835b45-8370-a95d-ea6e-a6f6158d9a90.jpeg)


# 測定してみる

簡単なテストとして、JoyWarrior56FR1-WP を手の上にのせてゆらゆらさせてみます。

![手でゆらゆら.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/8527c146-d759-a7ca-1551-892e4850c9ac.jpeg)

作成したプログラムを動かして加速度の値を取得してみます。

```
$ sudo python3 JoyWarrior56FR1.py
加速度x[m/s^2]: 0.737 加速度y[m/s^2]: 2.076 加速度z[m/s^2]: -0.047
加速度x[m/s^2]: 0.725 加速度y[m/s^2]: 2.168 加速度z[m/s^2]: 0.023
加速度x[m/s^2]: 0.721 加速度y[m/s^2]: 2.321 加速度z[m/s^2]: -0.062
加速度x[m/s^2]: 0.635 加速度y[m/s^2]: 2.518 加速度z[m/s^2]: 0.109
︙
```
というように加速度の値が出力されます。
生成された csv ファイルから3軸加速度のグラフを作って波形を確認してみました。ハイパスフィルタにより、重力加速度によるオフセットがなくなった状態で加速度の測定ができました。

![加速度のデータ.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3104552/77ba7001-8c64-6a07-73d9-2d1c6a6e8cd5.png)



# まとめ
本記事では USB 接続の加速度・角速度センサであるJoyWarrior56FR1-WP で加速度を取得する方法について紹介しました。精度や速度をそれほど必要とせず、すぐに加速度のデータ収集をしたいという場合に使えそうですので、参考にしていただけると幸いです。