UMEHOSHI ITA TOP PAGE

[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車

このページで示した[Raspberry Pi]のインストールと[UMEHOSHI ITA]を制御できた後のページです。
Raspberry Pi 3 Model A+]に接続した[UMEHOSHI ITA]基板を乗せるモータ付き台車の制作

[Raspberry Pi 3 Model A+]と[UMEHOSHI ITA]を乗せたモータ付き台車の 拡張ボードの回路結線図＆配置図


上記では、BNO055使用の下記の9軸センサーフュージョンモジュールキットを、さかさまに取り付けています。
上記の配置図内で示したように、さかさまに取り付けることによってX軸がロボット前方に向く方向になります。

一般にロボット工学系・ROS（Robot Operating System）系では、ロボット座標が次のように合わせます。
上記の配置図は、下位のロボット座標に方向が合うようにセンサーを配置した結果です。
方向意味推奨センサー軸
前方（前進方向）ロボットが進む方向+X軸
左方向ロボットの左側+Y軸
上方向地面から上（重力に逆らう方向）+Z軸

GPIO5, 6, 16, 17　をタクトスイッチに繋げる

GPIO5, 6, 16, 17にタクトスイッチSW1, SW2, SW3, SW4に接続しました。
（GPIO6, GPIO16, GPIO17はプルアップ付き入力に設定して使う予定です。）
GPIO5の接続しているSW1（ダイダイ）を次のように設定してシャットダウン用にしました。
SSHでログイン後、sudo nano /boot/config.txt　の操作で編集状態にします。
そして、このテキストの最後に「dtoverlay=gpio-shutdown,gpio_pin=5」の行を追加します。
nanoの保存、終了の操作後に、sudo rebootで再起動します。
以上で、GPIO5のSW1（ダイダイ）スイッチ操作でシャットダウンできるようしました。

上記の回路結線図で示しているように、40ピンのGPIO21にLEDを付けています。
これを出力ピンにして、Hiに設定すれば点灯します。
次のプログラム（swtest.py）は、これら検証用で起動時にLEDを点灯させ、 SW2、SW3、SW4 と順番に押して、LEDを消灯、点灯、消灯させます。
#!/usr/bin/python3
import RPi.GPIO as GPIO

GPIO.setmode(GPIO.BCM)
GPIO.setup(21, GPIO.OUT)  # GPIO21を出力に設定
GPIO.output(21, GPIO.HIGH)  # ON（3.3V）

for no in [6, 16, 17]:
    GPIO.setup(no, GPIO.IN, pull_up_down=GPIO.PUD_UP)  # プルアップ付き入力

while True:
    if GPIO.input(6) == GPIO.LOW: break # SW2スイッチが押されるのを待つ

GPIO.output(21, GPIO.LOW)   # OFF

while True:
    if GPIO.input(16) == GPIO.LOW: break # SW3スイッチが押されるのを待つ

GPIO.output(21, GPIO.HIGH)  # ON（3.3V）

while True:
    if GPIO.input(17) == GPIO.LOW: break # SW4スイッチが押されるのを待つ

GPIO.cleanup()              # ピンの初期化

BNO055センサーモジュールキット使用のフュージョンモードの動作検証プログラム

Raspbarry PIのGPIO2/SDAとGPIO3/SCLを使ってBNO055とI2Cの接続を行っています。
Raspbarry で、sudo raspi-configのコマンドで、次の操作で、I2Cを使えるようにします。
　→ 「Interface Options」 → 「I2C」 → 「Enable」 変更した後、再起動するとよいでしょう。
次の操作で、I2Cが動作しているか、検証できます。
suzuki@raspberrypi:~ $ i2cdetect -y 1
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:                         -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- 28 -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
これで、センサーのアドレスの0x28が見えればOKです。
取り付けた９軸センサーのBNO055は、内部MPUを使って各センサー情報を統合して簡単に高度な処理結果を得ることができるFusionモードと、 そうでない素データを取得するモードがある。
今回はFusionモードを次のプログラム（bno055_test.py）で試した。
#!/usr/bin/python3
import smbus # I?C通信をPythonから簡単に扱うためのモジュール
import time

BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ

BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス

bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)
# センサーをリセット（0x3FのSYS_TRIGGERレジスタのビット7をセット）
bus.write_byte_data(BNO055_ADDRESS,0x3F, 0x20)
time.sleep(0.7)  # リセット後は再起動まで時間がかかる
# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

# センサーフュージョンを有効にするNDOFモードに変更
bus.write_byte_data(BNO055_ADDRESS,BNO055_OPR_MODE, 0x0C) # NDOFモードへ
time.sleep(0.05)
print("BNO055をNDOF（Fusion）モードで初期化しました。（自動的にキャリブレーションが実行）")

def to_signed(val):
    """16ビット値を符号付き整数に変換"""
    if val >= 0x8000:
        val -= 0x10000
    return val

def read_euler():
    data = bus.read_i2c_block_data(BNO055_ADDRESS, BNO055_EULER_H_LSB, 6)
    # 各要素が 1 バイト（0?255）の整数を6個のリストで得られる。（1 LSB = 1/16 度）

    # データはリトルエンディアン形式（下位→上位の順）
    heading = (data[1] << 8) | data[0]  # 方位角（北基準のYAW）
    roll    = (data[3] << 8) | data[2]  # ロール角（左右の傾き）
    pitch   = (data[5] << 8) | data[4]  # ピッチ角（前後の傾き）

    # ロールとピッチは符号付き
    roll = to_signed(roll)
    pitch = to_signed(pitch)

    # スケーリング（1 LSB = 1/16 度）
    heading = heading / 16.0 # 
    roll    = roll / 16.0
    pitch   = pitch / 16.0
    return heading, roll, pitch

try:
    while True:
        h, r, p = read_euler()
        print(f"Heading: {h:7.2f}°, Roll: {r:7.2f}°, Pitch: {p:7.2f}°")
        time.sleep(0.2)

except KeyboardInterrupt:
    print("\n終了しました。")



Heading:  189.81°, Roll:   -1.38°, Pitch: -176.75°
Heading:  189.81°, Roll:   -1.38°, Pitch: -176.75°
この実行結果は、キャリブレーション操作後にロボットの前方を北に向けて、ロボットを水平に置いた状態のセンサー取得情報です。
（ロボットの前方はセンサーのX軸方向に一致するように取り付けています。）
キャリブレーションとは、測定機器やセンサーなどの誤差を修正し、正確な値を出力するように調整する作業で、その操作は後述します。
ロボットが右旋回すると Heading が増加し、左旋回すると Heading が減少する結果が得られた。 これからBNO055 の +X軸 はロボットの前方を正しく向いている。
その後下記表に示すように、各回転操作からセンサーの座標軸がロボット座標軸と在って正しい取り付けができたと判断している。
BNO055 の標準オイラー角定義とNDOFモードに於ける上記プログラムの出力値の意味
角度回転軸回転方向（＋）説明
Heading (Yaw)Z軸まわり反時計回り（上から見て右回転）ロボットが右旋回で値が増えるならOK（右が＋Z）
RollX軸まわり前方に向かって右手の親指を伸ばした向き左に傾けると＋、右に傾けるとーが正しい
PitchY軸まわりロボットの前方を下げる方向が＋前のめりで＋、上向きでー

さて、Heading (Yaw)、Roll、Pitchを得るレジスタと、目標となる出力範囲は、次の通りです。
項目レジスタ出力範囲
ヘディング (Heading)0x1A〜0x1B0〜360°（X軸を北に合わせる時に、0又は360度）
ロール (Roll)0x1C〜0x1D-90〜+90°（水平時は0度）
ピッチ (Pitch)0x1E〜0x1F-180〜+180°（水平時は0度）

対して、実験するとX軸を北に合わせた時に、Headingが189.81でした。
これが0になるようにオフセット調整します。次のように調整です。
    heading = heading - 189.81 # headingズレ補正
    heading = heading if heading >= 0 else heading + 360
同様に、水平時にPitchが -176.75°という実験結果なので、これが0になるように次のオフセット調整を行います。 
    pitch = pitch - -176.75 # pitchズレ補正
    if pitch > 180:
        pitch = -(360 - pitch)
    elif pitch < -180:
        pitch = -(-360 - pitch)
    return heading, roll, pitch
以上の調整で、次のようにほぼ希望の結果が得られました。 
Heading:    0.00°, Roll:   -1.38°, Pitch:    0.00°
Heading:    0.00°, Roll:   -1.38°, Pitch:    0.00°
前述のオフセット調整は、実験値で得られた調整値を埋め込みましたが、 将来的に初期起動には調整値を記録し、次回からそれを使って調整する仕組みにすべきと考えます。

前述で示したプログラムは、 BNO055をFusionモード（内部MPUを使って各センサー情報を統合して処理結果を得るモード）に属するNDOFモードで起動しています。
NDOFモードは、初期化時にOPR_MODE レジスタ (0x3D) に 0x0Cを設定することで行っていますが、
このモードでは、自動キャリブレーションを実行する状態になります。
（キャリブレーションとは、測定誤差を修正し、正しい測定できるように調整する校正の挙動です。）
「NDOFモードにしただけ」では自動で完了はしません。ユーザーが次の３項目でセンサーを動かすことにより完了します。 NDOFモードでは、自動キャリブレーションが行われます。そしてこのデータは電源投入後に常にリセットされます。
それで、すでに正しくキャリブレーション済みの値を保存しておき、起動時にそれを BNO055に再書き込みして使い回すことにします。
起動時のキャリブレーション実行で、それが完了したら その情報を保存する以下のプログラム（bno055_calib_write.py）を作りました。
（時々必要と思われる時に実行させる予定のコードです。）
#!/usr/bin/python3
import smbus # I?C通信をPythonから簡単に扱うためのモジュール
import time

BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ

BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス

bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)
# センサーをリセット（0x3FのSYS_TRIGGERレジスタのビット7をセット）
bus.write_byte_data(BNO055_ADDRESS,0x3F, 0x20)
time.sleep(0.7)  # リセット後は再起動まで時間がかかる
# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

# センサーフュージョンを有効にするNDOFモードに変更
bus.write_byte_data(BNO055_ADDRESS,BNO055_OPR_MODE, 0x0C) # NDOFモードへ
time.sleep(0.05)
print("BNO055をNDOF（Fusion）モードで初期化しました。（自動的にキャリブレーションが実行）")

while True: # NDOFモードで全てのセンサー（SYS, GYR, ACC, MAG）が 3 になるまで動かす。
    cal = bus.read_byte_data(BNO055_ADDRESS, 0x35)
    sys = (cal >> 6) & 0x03
    gyr = (cal >> 4) & 0x03
    acc = (cal >> 2) & 0x03
    mag = (cal >> 0) & 0x03
    print(f"SYS:{sys}, GYR:{gyr}, ACC:{acc}, MAG:{mag}")
    time.sleep(0.5)
    if sys == 3 and gyr == 3 and acc == 3 and mag == 3 : break # キャリブレーション完了？
    # 各値が 3 になれば完全キャリブレーション完了です

calib_data = bus.read_i2c_block_data(BNO055_ADDRESS, 0x55, 22)
with open("bno055_calib.bin", "wb") as f:
    f.write(bytearray(calib_data)) # オフセット値を読み出して保存
キャリブレーション情報は、RAM上のアドレス範囲：0x55 〜 0x6Aに保持されて、それを保存しています
実行例（ユーザーがMAG：磁気、ACC：加速度）、GYR：ジャイロの３項目の校正のためセンサーを動かす操作が必要で数十秒かかります。） 
suzuki@raspberrypi:~/py $ python bno055_calib_write.py
BNO055をNDOF（Fusion）モードで初期化しました。（自動的にキャリブレーションが実行）
SYS:0, GYR:0, ACC:0, MAG:0
・・・省略・・・
SYS:0, GYR:3, ACC:0, MAG:0
SYS:2, GYR:3, ACC:0, MAG:0
・・・省略・・・
SYS:2, GYR:3, ACC:0, MAG:0
・・・省略・・・
SYS:0, GYR:3, ACC:0, MAG:0
・・・省略・・・
SYS:0, GYR:3, ACC:0, MAG:0
SYS:2, GYR:3, ACC:0, MAG:0
SYS:3, GYR:3, ACC:0, MAG:0
SYS:2, GYR:3, ACC:0, MAG:0
・・・省略・・・
SYS:2, GYR:3, ACC:0, MAG:0
・・・省略・・・
SYS:0, GYR:3, ACC:0, MAG:1
SYS:2, GYR:3, ACC:0, MAG:1
SYS:1, GYR:3, ACC:0, MAG:1
SYS:2, GYR:3, ACC:0, MAG:2
SYS:3, GYR:3, ACC:0, MAG:2
SYS:3, GYR:3, ACC:0, MAG:3
・・・省略・・・
SYS:3, GYR:3, ACC:1, MAG:3
SYS:3, GYR:3, ACC:3, MAG:3
suzuki@raspberrypi:~/py $
上記実行で、各校正判定値が 3 になればキャリブレーションを完了したとして、 そのキャリブレーションデータを"bno055_calib.bin"に保存します。

次に、 キャリブレーションデータの"bno055_calib.bin"をBNO055に再書き込みして 補正する最終板のコード（bno055_M.py）を示します。
なお、このコードではX軸が北を向く時にHeadingが0、水平に置いた時にRollと Pitchが0になるオフセット調整のコードも付加しています。
この調整に使うデータは、"bno055_offset.txt"に調整無し時の各値を記憶しておいて、起動時にこれをロードして調整を行っています。
"bno055_offset.txt"の内容例：189.81,-1.38,-176.75
上記のデータは、ロボットを水平にしてはX軸が北を向く時のオフセット調整無し時のHeading,Roll,Pitchの値です。
"bno055_offset.txt"のファイルが存在しない時、このファイルを作るモードで起動する仕様にしました。
この"bno055_offset.txt"ファイル作成モード（make_offset_mode）では、起動時に20秒程度で終了し、終了直前の無調整のHeading,Roll,Pitchの値を保存します。
新たな調整データを設定したい場合は、"bno055_offset.txt"を削除して、起動時に20秒以内でロボットを水平でX軸が北を向くように置いて、 "bno055_offset.txt"が作られるプログラムの終了を待つ必要があります。
(一度"bno055_offset.txt"を作成後、再び作成が必要なる頻度は少ないと考えています。) 
#!/usr/bin/python3
import smbus # I?C通信をPythonから簡単に扱うためのモジュール
import time

make_offset_mode=False # "bno055_offset.txt"オフセット調整ファイル作成モード
try:
    with open("bno055_offset.txt", "r") as fr:
        s = fr.readline() # 一行読み取り(改行を含めて)
    a = s.split(",")
    heading_offset = float(a[0]) # オフセット調整値取得
    roll_offset = float(a[1])
    pitch_offset = float(a[2])
    print(f"offset value heading:{heading_offset}, roll:{roll_offset}, pitch:{pitch_offset}")
except:
    make_offset_mode = True # 作成モード

# BNO055 の初期化
BNO055_ADDRESS = 0x28  # BNO055のI2Cアドレス(ADRピンがGNDなら0x28、VDDなら0x29になります)
BNO055_OPR_MODE = 0x3D # 動作モードを設定するためのレジスタ
BNO055_EULER_H_LSB = 0x1A # オイラー角（方位・ロール・ピッチ）のデータが始まるアドレス
bus = smbus.SMBus(1)# 引数の1でRaspberry PiのボードGPIO2: SDA、GPIO3: SCLを指定

bus.write_byte_data(BNO055_ADDRESS, BNO055_OPR_MODE, 0x00) # 設定変更(OPR_MODE)でCONFIGモードに切り替える
time.sleep(0.05)

with open("bno055_calib.bin", "rb") as f:
    calib_data = list(f.read(22))

# 設定モードへ
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x00)
time.sleep(0.025)

# キャリブレーションデータを書き込んで、調整情報を復元
bus.write_i2c_block_data(BNO055_ADDRESS, 0x55, calib_data)

# NDOFモードに戻す
bus.write_byte_data(BNO055_ADDRESS, 0x3D, 0x0C)
time.sleep(0.05)

# 出力単位(UNIT_SEL)を設定（0x00で「角度＝度（°）」単位）
bus.write_byte_data(BNO055_ADDRESS,0x3B, 0x00)

def to_signed(val):
    """16ビット値を符号付き整数に変換"""
    if val >= 0x8000:
        val -= 0x10000
    return val

def read_euler():
    data = bus.read_i2c_block_data(BNO055_ADDRESS, BNO055_EULER_H_LSB, 6)
    # 各要素が 1 バイト（0?255）の整数を6個のリストで得られる。（1 LSB = 1/16 度）

    # データはリトルエンディアン形式（下位→上位の順）
    heading = (data[1] << 8) | data[0]  # 方位角（北基準のYAW）
    roll    = (data[3] << 8) | data[2]  # ロール角（左右の傾き）
    pitch   = (data[5] << 8) | data[4]  # ピッチ角（前後の傾き）

    # ロールとピッチは符号付き
    roll = to_signed(roll)
    pitch = to_signed(pitch)

    # スケーリング（1 LSB = 1/16 度）
    heading = heading / 16.0 # 
    roll    = roll / 16.0
    pitch   = pitch / 16.0

    if make_offset_mode == False: 
        # X軸が北を向く時にHeadingが0、水平に置いた時にRollと Pitchが0になるオフセット調整
        heading = heading - heading_offset # headingズレ補正
        heading = heading if heading >= 0 else heading + 360
        roll = roll - roll_offset # rollズレ補正
        if roll > 90:
            roll = -(90 - roll)
        elif roll < -90:
            roll = -(-90 - roll)
        pitch = pitch - pitch_offset # pitchズレ補正
        if pitch > 180:
            pitch = -(360 - pitch)
        elif pitch < -180:
            pitch = -(-360 - pitch)
    #
    return heading, roll, pitch

try:
    count = 100
    while True:
        h, r, p = read_euler()
        print(f"Heading: {h:7.2f}°, Roll: {r:7.2f}°, Pitch: {p:7.2f}°")
        if make_offset_mode:
            with open("bno055_offset.txt", "w") as fw:
                s = fw.write(f"{h},{r},{p}\n") # オフセット調整データ書き込み
            count -= 1
            print(f"  {count}が0になるまでに、ロボットを水平にしてはX軸が北を向くように置いてください。")
            if count <= 0:
                print("\n終了しました。")
                break
        #
        time.sleep(0.2)

except KeyboardInterrupt:
    print("\n終了しました。")