はじめに
これまでカムロボを遠隔操作すべくハードウェアを組み立てていた．遠隔操作時にはカメラで進行方向を確認しながら操作することができるようになった．
しかしながらカメラによる視覚情報のみではカムロボの姿勢が十分に把握できず，段差でスタックしたり転倒したりする危険がある．
今回は加速度センサ MPU6050 を傾斜計とすることで現在の姿勢を把握できるようにする．
材料
カムロボの細かい構成部品はこの記事を確認してもらうとして，今回はこれに加速度センサ MPU6050 を追加する．
加速度センサ 2 個入りで購入時の価格は 796 円．探せばもうちょい安いものもあるかもしれない．
Bitly
amzn.to
傾斜計をつくる
配線はこんな感じ．Amazon に書いてある仕様によると 3V でも 5V でも動くらしいので，今回とりあえず電源は 5V に接続する．
自分でライブラリを実装するのは面倒なので，Adafruit のライブラリを拝借する．
adafruit-circuitpython-mpu6050
CircuitPython helper library for the MPU6050 6-DoF Accelerometer and Gyroscope
pypi.org
$ pip install adafruit-circuitpython-mpu6050サンプルコードが公開されているので，このまま動かせば傾斜計の出来上がり．
# SPDX-FileCopyrightText: 2021 ladyada for Adafruit Industries
# SPDX-License-Identifier: MIT
# Display inclination data five times per second
# See this page to learn the math and physics principals behind this example:
# https://learn.adafruit.com/how-tall-is-it/gravity-and-acceleration
import time
from math import atan2, degrees
import board
import adafruit_mpu6050
i2c = board.I2C() # uses board.SCL and board.SDA
# i2c = board.STEMMA_I2C() # For using the built-in STEMMA QT connector on a microcontroller
sensor = adafruit_mpu6050.MPU6050(i2c)
# Given a point (x, y) return the angle of that point relative to x axis.
# Returns: angle in degrees
def vector_2_degrees(x, y):
angle = degrees(atan2(y, x))
if angle < 0:
angle += 360
return angle
# Given an accelerometer sensor object return the inclination angles of X/Z and Y/Z
# Returns: tuple containing the two angles in degrees
def get_inclination(_sensor):
x, y, z = _sensor.acceleration
return vector_2_degrees(x, z), vector_2_degrees(y, z)
while True:
angle_xz, angle_yz = get_inclination(sensor)
print("XZ angle = {:6.2f}deg YZ angle = {:6.2f}deg".format(angle_xz, angle_yz))
time.sleep(0.2)理屈がわかると楽しさ N 倍ということで，ポイントだけかいつまんで実装内容を整理しておく．
① XYZ 軸の加速度を取得する
x, y, z = _sensor.acceleration② アークタンジェントで X 軸と重力加速度（のZ軸成分 ）の角度を計算する（Y軸も同様）
angle = degrees(atan2(y, x))適当に図示するとこんな感じ．
サンプルそのままだと芸がないのでちょっと手直（測定した傾斜角度をTCPでデスクトップPCに送信して，GUI上に表示）してみたのがこちら．転倒する直前の角度は黄，転倒する角度は赤で警告してくれる．
おわりに
今回は MPU6050 を使って傾斜を求める方法を整理した．
流石にそろそろ内部のスペース的にハードウェアによる機能追加は難しくなってきた．これ以上中身を詰め込むなら表面実装基盤を自作しないと無理そう．
コメント