傾斜計の試作機の製作

※なにか気になる点がありましたらコメント欄にご記入ください。また、工作や回路を製作する場合には、細かい作業などに対して、細心の注意を払われるようお願いいたします。

１．はじめに
２．傾斜角度を測定する原理
３．傾斜計のハードウェア
４．傾斜計のソフトウェア
５．傾斜角度の表示（動画）
６．まとめ

※関数math.atan2(x,y)について詳しく調べた結果、コードの一部を変更し記事を訂正しました。（2022/06/07）

１．はじめに

　去年の７月に倒立振子なるものの組立キットを製作しました。（記事：【確率・統計ロボティクスキットMZIP-01】自作キットを組み立てて動かしてみた）そのとき、出典であるトランジスタ技術を購入し、いつか同等なものを自分でも作ってみたいと思いました。
　記事の中では、倒立振子を作る前段階として「傾斜計」と呼ばれるものにも触れられていました。倒立振子の車体の角度を測る部分のみを抜き出して構築したもののようです。
　今回、「傾斜計の試作機」に当たるものを作ってみたので、記事としてまとめておきます。

主な仕様

マイコン：ラズベリーパイPico
センサー：３軸加速度、角速度センサ（MPU6050）
使用言語：MicroPython

※ラズパイPicoをC/C++で動かすことも考えており、マイコンではセンサーからの値などをモニターする必要があるので、UART（今回はFT231X（秋月）を使用）での通信の処理もつけ足しました。

出典：トランジスタ技術　２０１９年７月号　第１章第２節（P.３９～）のあたり

※出典ではカルマンフィルタを大々的に取り上げていますが、今回の記事では一切触れていません。
　また、今回の記事の内容は、センサーからの値を読み取る部分と、UARTによるシリアル通信の部分の動作確認の色合いが強いです。傾斜角を正確に測るためには、以下の内容では不十分だと考えられます。
　予めご了承ください。

２．傾斜角度を測定する原理

　図1のように座標系を取ると、車体の重心にかかる重力ｍｇ[Ｎ]を、ℤ軸とＹ軸の２つの成分に分けることができます。

　このとき、車体の傾斜角をθ［radian］とすると

$\tan\theta = \dfrac{Y軸成分}{Z軸成分}$

　となるので

$\theta = \tan^{-1} \dfrac{Y軸成分}{Z軸成分}$

　よりもとめることができます。（ただし、タンジェントの逆関数は多価関数なので、計算結果がどこからどこに対応しているかということに注意する必要があります）
　ただし、今回の段階では加速度センサーからの値には、オフセット値（定常時の一定の誤差）およびノイズが含まれています。カルマンフィルタを用いるとこれらの値が除去できるようです。今後の課題として後々取り組みたいと思います。
　また、今後、倒立振子を組み立てることを考えると、倒立振子が水平に移動しているときや、傾斜角度が変化する場合、この式では正しく傾斜角度を求めることはできません。その際は傾斜角度の角速度も計算式に入れないとならないようです。これも今後の課題にしたいと思います。

３．傾斜計のハードウェア

　図２に全体の構成を示します。

　
　センサーからの値を読み込み、ラズパイPicoにて処理し、PCを経由してラズパイPico用のエディタであるThonny により、計算結果（車体の角度）を表示しています。
　実体配線図は図３に示す通りです。３軸加速度・角速度センサーであるMPU6050からI2C により値を読み込んでいます。
　ただし、USBコネクタとの接続を省略しています。ラズパイPicoからとFT231Xからの２本のUSBケーブルがＰＣ本体と繋がっています。回路の電源はラズパイPicoの＃３５ピンから３．３Vが供給されています。またノイズ対策のため、電源ラインに積層セラミックコンデンサと電解コンデンサを挿入しています。

４．傾斜計のソフトウェア

　コードをリスト１に示します。手順は３章で説明したとおりです。
　センサーからの値の読み込みや、ＵＡＲＴによるシリアル通信に関しては、ライブラリmachine の中にＩ２ＣとＵＡＲＴのモジュールが含まれているので、処理は割と楽です。
　計算結果はIDEであるThonny に表示されます。UARTでの処理も付け加えています。UARTによりPCに送られてくるデータは、Terataermにより表示しています。

リスト１

# File Name: UartAccelSensor_04.py
# Date     : May 22 (Sun) 2022
# Author   : @Tanuki_Bayashin
# Content  : mesure Accelaration, Calculate data,
#          : put out result by usart etc.
#
# UartAccelSensor_03.py との違い
# calc_Data(dataIn_Hex)の中で
# sgn_fabs(x)という関数を呼んでいる。
# 元のままだとxが-1e-10 のとき
# 計算結果が０になってしまい、エラーが生じる
#

from machine import UART, Pin, I2C
import utime, math

uart1 = UART(1, baudrate=9600, tx=Pin(4), rx=Pin(5))
uart1.write('Hello! Program is started!!!\r\n')  # write 5 bytes

#get instance of AccelSensor
# i2cport = 0, sda = #12 GP, scl = #13 pin GP,
# freq = 400KHz
i2cport = 0
real_sda = 12
real_scl = 13
real_freq = 400000
i2c = I2C(i2cport, sda = Pin(real_sda), scl = Pin(real_scl), \
          freq = real_freq)

# 0x68:mpu6050の i2c アドレス
# 0x6B へ　0 を書き込む　→　動作オン
i2c.writeto(0x68, b'\x6B\x00')

# 加速度センサーから値をとる関数
def get_AccelSensor(i2c):
    # センサーからの値を入手
    data = i2c.readfrom_mem(0x68, 0x3B, 14)    # adrress 0x3B より 14bytes 入手
    Accel_X = data[0] * 256 + data[1]
    Accel_Y = data[2] * 256 + data[3]
    Accel_Z = data[4] * 256 + data[5]
    Temp    = data[6] * 256 + data[7]
    Gyro_X  = data[8] * 256 + data[9]
    Gyro_Y  = data[10] * 256 + data[11]
    Gyro_Z  = data[12] * 256 + data[13]

    return Accel_X, Accel_Y, Accel_Z, Temp, Gyro_X, Gyro_Y, Gyro_Z

# i2c adress を求める関数
def scan(i2c):
    a = i2c.scan()
    print('i2c address: ', a)

#
#
# この部分は都合により削除しました
#  (2022/06/07)
#
#
#
#
#
#

# 整数dataIn_Hexの値が負の数であるときの処理
def calc_Data(dataIn_Hex):
    data = float(dataIn_Hex)
    if data >= 32768:
        data = data - 65536
    return data            # この行は変更しました(2022/06/07)

# i2c adress を表示する
scan(i2c)

while(True):
    Accel_X, Accel_Y, Accel_Z, Temp, Gyro_X, Gyro_Y, Gyro_Z\
             = get_AccelSensor(i2c)
    
    Accel_X = calc_Data(Accel_X)
    Accel_Y = calc_Data(Accel_Y)
    Accel_Z = calc_Data(Accel_Z)
    Temp    = (calc_Data(Temp)+ 12421) / 340.0
    Temp    = calc_Data(Temp)
    Gyro_X  = calc_Data(Gyro_X)
    Gyro_Y  = calc_Data(Gyro_Y)
    Gyro_Z  = calc_Data(Gyro_Z)

    angle = math.atan2(Accel_Y, Accel_Z) * 180.0 / math.pi

    print('Accel_Y, Accel_Z, angle:', Accel_Y, Accel_Z, angle)
    uart1.write('angle:' + str(angle) + '\r\n')  # write angle of balancing car

    utime.sleep(0.5)

解説
14、15行目：　必要なライブラリをインポートしています。
17行目　　：　ＵＡＲＴ（シリアル通信）のインスタンスを取得しています
　　　　　　（UART1を選択,,ボーレート9600、TX=GPIO4（6番ピン）、RX=GPIO5（7番ピン））
18行目　　：　ＵＡＲＴにより文字列を出力しています（動作確認しています）

20~28行目：　I2Cのインスタンスを取得しています
　　　　　　（I2C0を選択、SDA=GPIO12（16番ピン）、SCL=GPIO13（17番ピン）、400KHz）
30~32行目　：　加速度センサーMPU6050の動作をオンにしています
34~46行目　：　加速度センサーMPU6050から値を読み取る関数です。
　　　　　　　31行目にてMPU6050内のアドレス0x3Bより値を読み取っています。
　　　　　　　32行目~38行目にて、各データを計算しています。

48~51行目　：　I2Cアドレスを取得して表示する関数です。
53~62行目　：　新たに加えた関数。絶対値を取り、1e-10を加え、符号を元に戻している
　　　　　　　　以前のままだと、ｘが -1e-10 のとき、０になってしまう
上の記述は必要なくなりました。関数math.atan2(x,y)のyが０のときは、π/２が求められるからです。 (2022/06/07)

64~69行目　：　取得したデータが32768以上のとき、マイナスになるように処理する関数です。
　　　　最後に関数sgn_fabs(data) の処理を行っており、ゼロで割る事がないようにしています。
72行目　　：　I2Cアドレスを１度だけ表示しています。（なくてもいい）

74行目　　：　ここから無限ループです
75~76行目　：　センサーの値を読み取っています
78~84行目　：　値がマイナスであるときの処理です。

87行目　　：　傾斜角をＹ軸方向の加速度とＺ軸方向の加速度から計算しています。
　　　　　　　戻り値はラジアンであるため、1回転が360度になるよう180／πを掛けています
89、90目　：　計算結果をprint文とUARTにて出力しています。
92行目　　　：　0.5秒待機しています。（無限ループ内の処理の最後）

解説は以上です。

５．傾斜角度の表示（動画）

　傾斜角度を表示している場面の動画です。

www.youtube.com
　画面中、中央上側の黒い画面はTerataermの画面で、UART（シリアル通信）によるデータを表示しています。画面下側の数字も傾斜角度を表示しており、ブレッドボードを傾けると、画面中の数値が角度に合わせて変化しているのが分かると思います。

６．まとめ

　今回、トランジスタ技術７月号（2019年）の記事をもとに、ラズベリーパイPicoにて、加速度センサーの値を読み取りました。そして、その値をもとにブレッドボードの傾斜角度を簡易的に求め、その結果をThonnyとTerataermに表示しました。
　次回はラズベリーパイPicoのソフトの部分をC/C++にて記述し、動作を確認したいと思います。

　最後まで読んでくださり、ありがとうございました。

追記： sgn_fabs()という関数を付け加えました。1e-10を加えるだけだと、元の値が-1e-10のとき、計算結果が０になってしまうためです。関数calc_Data()の中で、return文のところで使用しています。（計算結果は０になってもエラーにならないことが分かりました。2022/08/06）

Tanuki_Bayashin’s diary

電子工作を趣味としています。最近はラズベリーパイPicoというマイコンを使って楽しんでいます