.. include:: /index.rst
:start-after: start_hello_message
:end-before: end_hello_message
.. _py_gravity_cube:
4.14 グラビティキューブ
=========================
**はじめに**
このレッスンでは、**IMU** によって駆動される、128×64 SSD1306 OLED 上に表示される **重力基準の 3D キューブ** を作成します。
このキューブは、加速度センサーのみを用いて算出したロールとピッチに基づき、重力に対する基板の向きに応じて傾きます。
主な特徴は次のとおりです。
- 正射影による 3D キューブ描画（透視歪みなし）
- 初期姿勢を基準とした、加速度センサー由来の姿勢推定
- 前後方向を把握しやすくするため、キューブの一面を塗りつぶして表示
- 実際の取り付け方向に合わせて X/Y の向きを反転できる 2 つの設定フラグ
基板を傾けると、キューブが滑らかに回転し、デバイスの向きを直感的に可視化できます。
----------------------------------------------
**必要なもの**
このプロジェクトで必要なコンポーネントは以下のとおりです。
.. list-table::
:widths: 30 20
:header-rows: 1
* - COMPONENT INTRODUCTION
- PURCHASE LINK
* - :ref:`cpn_wires`
- |link_wires_buy|
* - :ref:`cpn_10_axis_imu`
- \-
* - :ref:`cpn_oled`
- \-
* - :ref:`cpn_fusion_hat`
- \-
* - Raspberry Pi
- \-
.. ----------------------------------------------
.. **Circuit Diagram**
.. .. image:: img/fzz/4.14_gravity_cube_sch.png
.. :width: 80%
.. :align: center
----------------------------------------------
**配線図**
各コンポーネントの組み立ては、以下の配線図を参照してください。
.. image:: img/fzz/4.14_cube2_bb.png
:width: 80%
:align: center
----------------------------------------------
**セットアップ手順**
#. OLED 用ライブラリをインストールします。
.. raw:: html
.. code-block:: shell
sudo pip3 install adafruit-circuitpython-ssd1306 --break
#. IMU 用ライブラリをインストールします。
.. raw:: html
.. code-block:: shell
sudo pip install git+https://github.com/sunfounder/sunfounder-imu-python.git --break-system-packages
#. ``ai-lab-kit`` ディレクトリからサンプルを実行します。
.. raw:: html
.. code-block:: shell
cd ~/ai-lab-kit/python/
sudo python3 4.14_Cube.py
#. スクリプトを実行すると、次のように動作します。
* 加速度センサーが重力基準の X/Y/Z データを提供します。
* コードは、初期姿勢を基準（現在の向きが 0°, 0°）としてロールとピッチを計算します。
* **前面を塗りつぶした** ワイヤーフレームキューブが OLED に描画されます。
* 基板を傾けると、画面上のキューブが回転します。
* **Ctrl+C** を押すと終了し、OLED はクリアされます。
----------------------------------------------
**コード**
以下は、重力基準キューブ用の Python スクリプトです。
.. raw:: html
.. code-block:: python
import time
import math
from PIL import Image, ImageDraw, ImageFont
import adafruit_ssd1306
import board
from sunfounder_imu import IMU
# ========== User-configurable axis flip ==========
# Flip X/Y to match your physical mounting and perceived motion on the OLED.
# If motion looks reversed on a given axis, set that axis to True.
FLIP_X = False # True = invert roll direction on display; False = normal
FLIP_Y = False # True = invert pitch direction on display; False = normal
# ========== OLED setup ==========
WIDTH, HEIGHT = 128, 64
i2c = board.I2C()
oled = adafruit_ssd1306.SSD1306_I2C(WIDTH, HEIGHT, i2c, addr=0x3C)
oled.fill(0)
oled.show()
# Framebuffer
image = Image.new("1", (WIDTH, HEIGHT))
draw = ImageDraw.Draw(image)
font = ImageFont.load_default()
# ========== IMU initialization ==========
imu = IMU()
# ========== Cube model ==========
CUBE_SIZE = 9 # smaller cube for 128x64 OLED
VERTS = [
(-1, -1, -1), (+1, -1, -1), (+1, +1, -1), (-1, +1, -1),
(-1, -1, +1), (+1, -1, +1), (+1, +1, +1), (-1, +1, +1),
]
EDGES = [
(0,1),(1,2),(2,3),(3,0),
(4,5),(5,6),(6,7),(7,4),
(0,4),(1,5),(2,6),(3,7)
]
FRONT_FACE = [4,5,6,7] # +Z face gets filled
# ========== Projection (orthographic) ==========
def project_point(p, scale=CUBE_SIZE, cx=WIDTH//2, cy=HEIGHT//2):
"""
Orthographic projection. We flip the screen Y here so that positive 3D Y
appears upward on the OLED (more intuitive for tilt).
"""
x, y, _z = p
return int(cx + scale * x), int(cy - scale * y)
# ========== Math / orientation helpers ==========
def ema(prev, new, alpha):
"""Exponential smoothing to reduce jitter."""
return alpha * new + (1.0 - alpha) * prev
def rotate_point(p, roll, pitch, yaw=0.0):
"""Rotate p=(x,y,z) by Rx(roll)*Ry(pitch)*Rz(yaw). Yaw fixed to 0 for gravity-only."""
x, y, z = p
# Rx
cr, sr = math.cos(roll), math.sin(roll)
y, z = (y*cr - z*sr), (y*sr + z*cr)
# Ry
cp, sp = math.cos(pitch), math.sin(pitch)
x, z = (x*cp + z*sp), (-x*sp + z*cp)
# Rz (kept for completeness)
if yaw:
cz, sz = math.cos(yaw), math.sin(yaw)
x, y = (x*cz - y*sz), (x*sz + y*cz)
return (x, y, z)
def accel_to_rp(ax, ay, az):
"""
Convert accelerometer (m/s²) to roll/pitch in radians (gravity-referenced).
roll = rotation around X (right-hand rule)
pitch = rotation around Y
"""
# Convert from m/s² to g (9.80665 m/s² = 1g)
ax_g = ax / 9.80665
ay_g = ay / 9.80665
az_g = az / 9.80665
g = math.sqrt(ax_g*ax_g + ay_g*ay_g + az_g*az_g) + 1e-9
axn, ayn, azn = ax_g / g, ay_g / g, az_g / g
roll = math.atan2(ayn, azn)
pitch = math.atan2(-axn, math.sqrt(ayn*ayn + azn*azn))
return roll, pitch
def draw_cube(roll, pitch, yaw=0.0, annotate=True):
"""Render the cube with one filled face."""
draw.rectangle((0, 0, WIDTH, HEIGHT), outline=0, fill=0)
rverts = [rotate_point(v, roll, pitch, yaw) for v in VERTS]
pts = [project_point(v) for v in rverts]
# Filled front face
face_xy = [pts[i] for i in FRONT_FACE]
draw.polygon(face_xy, outline=255, fill=255)
# Wireframe edges
for a, b in EDGES:
x0, y0 = pts[a]
x1, y1 = pts[b]
draw.line((x0, y0, x1, y1), fill=255)
if annotate:
rdeg = math.degrees(roll)
pdeg = math.degrees(pitch)
draw.text((2, 2), f"R:{rdeg:+.0f} P:{pdeg:+.0f}", font=font, fill=255)
# ========== Baseline & smoothing ==========
baseline_set = False
roll0 = pitch0 = 0.0
ROLL_EMA = 0.20
PITCH_EMA = 0.20
roll_disp = pitch_disp = 0.0
try:
while True:
# Read IMU data
data = imu.read()
# Extract accelerometer data (in m/s²)
ax = data['accel_x']
ay = data['accel_y']
az = data['accel_z']
# Absolute roll/pitch from gravity
roll_abs, pitch_abs = accel_to_rp(ax, ay, az)
# First reading defines baseline (0°,0°)
if not baseline_set:
roll0, pitch0 = roll_abs, pitch_abs
baseline_set = True
# Relative orientation
roll_rel = roll_abs - roll0
pitch_rel = pitch_abs - pitch0
# Apply user flips to match perceived direction on OLED
if FLIP_X:
roll_rel = -roll_rel
if FLIP_Y:
pitch_rel = -pitch_rel
# Smooth
roll_disp = ema(roll_disp, roll_rel, ROLL_EMA)
pitch_disp = ema(pitch_disp, pitch_rel, PITCH_EMA)
# Render (yaw fixed to 0 in gravity-only mode)
draw_cube(roll_disp, pitch_disp, yaw=0.0, annotate=True)
# Show on OLED
oled.image(image)
oled.show()
time.sleep(0.02)
except KeyboardInterrupt:
oled.fill(0)
oled.show()
print("\nExited.")
----------------------------------------------
**コードの解説**
1. **IMU の初期化**
スクリプトは Fusion HAT+ モジュールから ``IMU`` インスタンスを生成します。
加速度センサーは、m/s² 単位の生の X、Y、Z 加速度値を提供します。
2. **重力基準の角度計算**
``accel_to_rp()`` は、加速度センサーの読み取り値を **roll** と **pitch** に変換します。
- Roll: **X** 軸まわりの回転
- Pitch: **Y** 軸まわりの回転
重力のみを用いるため、yaw は求められず、常に 0 に固定されます。
.. code-block:: python
def accel_to_rp(ax, ay, az):
"""
Convert accelerometer (m/s²) to roll/pitch in radians (gravity-referenced).
roll = rotation around X (right-hand rule)
pitch = rotation around Y
"""
# Convert from m/s² to g (9.80665 m/s² = 1g)
ax_g = ax / 9.80665
ay_g = ay / 9.80665
az_g = az / 9.80665
g = math.sqrt(ax_g*ax_g + ay_g*ay_g + az_g*az_g) + 1e-9
axn, ayn, azn = ax_g / g, ay_g / g, az_g / g
roll = math.atan2(ayn, azn)
pitch = math.atan2(-axn, math.sqrt(ayn*ayn + azn*azn))
return roll, pitch
3. **基準姿勢**
最初の読み取り時に、
- 現在の ``roll`` と ``pitch`` を基準値（ ``roll0`` 、 ``pitch0`` ）として保存します。
- 以後の姿勢は **相対角度** として表されます。
.. code-block:: python
roll_rel = roll_abs - roll0
pitch_rel = pitch_abs - pitch0
これにより、プログラム開始時のデバイスの向きが **0°, 0°** になります。
4. **ユーザー設定可能な軸反転**
``FLIP_X`` と ``FLIP_Y`` の 2 つのフラグを使うと、各軸の表示方向を反転できます。
- Roll の動きが逆に感じる場合は ``FLIP_X = True``
- Pitch の動きが逆に感じる場合は ``FLIP_Y = True``
IMU の取り付け方向や配線方向が異なる場合に便利です。
5. **平滑化（EMA）**
``ema()`` 関数は **指数移動平均** による平滑化を行います。
- センサーノイズによる揺れを低減
- キューブの動きをより自然に見せる
``ROLL_EMA`` と ``PITCH_EMA`` の値で、応答性と滑らかさのバランスを調整できます。
.. code-block:: python
def ema(prev, new, alpha):
"""Exponential smoothing to reduce jitter."""
return alpha * new + (1.0 - alpha) * prev
6. **3D キューブの回転**
``rotate_point()`` は、各キューブ頂点に回転行列を適用します。
.. code-block:: python
def rotate_point(p, roll, pitch, yaw=0.0):
"""Rotate p=(x,y,z) by Rx(roll)*Ry(pitch)*Rz(yaw). Yaw fixed to 0 for gravity-only."""
x, y, z = p
# Rx
cr, sr = math.cos(roll), math.sin(roll)
y, z = (y*cr - z*sr), (y*sr + z*cr)
# Ry
cp, sp = math.cos(pitch), math.sin(pitch)
x, z = (x*cp + z*sp), (-x*sp + z*cp)
# Rz (kept for completeness)
if yaw:
cz, sz = math.cos(yaw), math.sin(yaw)
x, y = (x*cz - y*sz), (x*sz + y*cz)
return (x, y, z)
- ``Rx(roll)`` の後に ``Ry(pitch)`` を適用します（yaw は 0 のまま）
- これにより回転後の 3D 座標が得られます
その後、 ``project_point()`` により、 **正射影** を用いて 3D 座標を 2D の OLED 座標へ変換します。
.. code-block:: python
def project_point(p, scale=CUBE_SIZE, cx=WIDTH//2, cy=HEIGHT//2):
"""
Orthographic projection. We flip the screen Y here so that positive 3D Y
appears upward on the OLED (more intuitive for tilt).
"""
x, y, _z = p
return int(cx + scale * x), int(cy - scale * y)
7. **キューブの描画**
``draw_cube()`` は次の処理を行います。
- 画面をクリアする
- 8 個すべての頂点を回転・投影する
- 全エッジをワイヤーフレームとして描画する
- 1 つの面（ ``FRONT_FACE`` ）を塗りつぶして、どの面が手前を向いているか分かりやすくする
- 必要に応じて左上に roll と pitch を度数で表示する
.. code-block:: python
def draw_cube(roll, pitch, yaw=0.0, annotate=True):
"""Render the cube with one filled face."""
draw.rectangle((0, 0, WIDTH, HEIGHT), outline=0, fill=0)
rverts = [rotate_point(v, roll, pitch, yaw) for v in VERTS]
pts = [project_point(v) for v in rverts]
# Filled front face
face_xy = [pts[i] for i in FRONT_FACE]
draw.polygon(face_xy, outline=255, fill=255)
# Wireframe edges
for a, b in EDGES:
x0, y0 = pts[a]
x1, y1 = pts[b]
draw.line((x0, y0, x1, y1), fill=255)
if annotate:
rdeg = math.degrees(roll)
pdeg = math.degrees(pitch)
draw.text((2, 2), f"R:{rdeg:+.0f} P:{pdeg:+.0f}", font=font, fill=255)
----------------------------------------------
**トラブルシューティング**
- **キューブが動かない**
- Raspberry Pi で I2C が有効になっていることを確認してください。
- 使用している IMU ドライバで正しい I2C アドレスが使われていることを確認してください。
- **動きが逆に見える**
- ``FLIP_X`` または ``FLIP_Y`` を切り替えて方向を反転してください。
- 基板を水平に置いた状態から、1 軸ずつゆっくり傾けて、どちらを反転すべきか確認してください。
- **キューブの動きが揺れすぎる**
- ``ROLL_EMA`` / ``PITCH_EMA`` を小さくして平滑化を強めてください（例: 0.1）。
- 基板がしっかり固定され、ケーブルに引っ張られていないことを確認してください。
- **平らに置いているのにキューブが傾いて見える**
- 基板を静止させた状態でスクリプトを再起動してください。最初の読み取り値が基準になります。
- プログラム開始時に基板が本当に水平であることを確認してください。
- **OLED に何も表示されない**
- OLED の I2C アドレス（一般的には ``0x3C``）を確認してください。
- 配線とコントラスト設定を確認してください。
- ``adafruit-circuitpython-ssd1306`` が正しくインストールされていることを確認してください。
----------------------------------------------
**試してみよう**
1. **ジャイロから Yaw を追加する**
ジャイロ積分と加速度センサーのデータを組み合わせたコンプリメンタリフィルタを使い、yaw 軸を追加して完全な 3D 回転を実現します。
2. **キューブのサイズや位置を変更する**
``CUBE_SIZE`` や投影中心を調整して、キューブの位置や大きさを変更できます。
- 拡大してより迫力のある表示にする
- 少し上へ移動して文字表示用のスペースを増やす
3. **ワイヤーフレームのグリッドを追加する**
キューブの背後に簡単な「床」グリッドを描いて、3D の動きをより分かりやすくします。
4. **自動再センタリング機能を追加する**
ボタンの長押しやキー入力で、実行中に基準姿勢をリセットできるようにします。
5. **複数の表示モード**
次のモードを切り替えられるようにします。
- ワイヤーフレームのみ
- 塗りつぶし面のみ
- 傾き角度に応じた面のシェーディング
これらの拡張により、シンプルな重力キューブを高機能な 3D IMU 可視化ツールへ発展させることができます。