JetRacerの環境で９軸センサーで２Dマッピングに挑戦。

jupyter labの環境で位置をプロットさせます。BNO055というセンサーで２Dマッピングさせてみました。まだ速度は、固定です。BNO055は補正できるようですが、加速度センサーでは車体の傾きにより重力により速度を算出するのは誤差が蓄積されるのでならかの方法でマシンから速度を取得する必要がありそうです。

%matplotlib inline

import time
import board
import busio
import adafruit_bno055
import numpy as np
import matplotlib.pyplot as plt
from IPython.display import display

i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_bno055.BNO055_I2C(i2c)

# 初期位置と角度
x, y = 0.0, 0.0
theta = 0.0

# 移動距離は毎秒300ミリ（あとでオドメトリで取得するコードを追加する）
distance_per_step = 0.3

# グラフ表示設定
plt.ion()
fig, ax = plt.subplots()
sc = ax.scatter([x], [y])
ax.set_xlim(-300, 300)
ax.set_ylim(-300, 300)
ax.set_xlabel("X(m)")
ax.set_ylabel("Y(m)")
ax.set_title("JetRacer_2D Mapping")
display_handle = display(fig, display_id=True)

def update_position():
    global x, y, theta
    
    x_positions = [x]
    y_positions = [y]

    while True:
        #オイラー角を使用
        euler_angles = sensor.euler
        plt.ion()

        if euler_angles is not None:
            heading, _, _ = euler_angles
            theta = np.deg2rad(heading)

            dx = distance_per_step * np.cos(theta)
            dy = distance_per_step * np.sin(theta)
            
            x -= dx
            y += dy

            x_positions.append(x)
            y_positions.append(y)

            sc.set_offsets(np.c_[x_positions, y_positions])
            ax.set_xlim(min(x_positions) - 1, max(x_positions) + 1)
            ax.set_ylim(min(y_positions) - 1, max(y_positions) + 1)
            plt.draw()
            display_handle.update(fig)
            #plt.pause(0.01)
            
            with open("JetRacer_2d_mapping_data.csv", "a") as f:
                f.write(f"{x},{y},{heading}\n")
            
        else:
            print("Waiting for sensor data...")

        time.sleep(0.01)

if __name__ == "__main__":
    try:
        update_position()
    except KeyboardInterrupt:
        print("Mapping interrupted.")
        plt.ioff()
        plt.show()