pose prediction from IMU sensor

[Hattyoriiiiiii]

https://team432.jpn.org/?p=666

[tn493]

負の数や小数点以下桁数に対応しました。

import smbus2 as sb
from time import sleep

def twos_comp(val, bits):
    if (val & (1 << (bits-1))) != 0:
        val = val - (1 << bits)
    return val

def r(addr, reg): return bus.read_byte_data(addr, reg)
def w(addr, reg, val): bus.write_byte_data(addr, reg, val)
def read_acc(): return [round(twos_comp(r(0x19, rA) << 8 | r(0x19, rA - 1), 16) / 16 * 0.0098, 2) for rA in (0x03, 0x05, 0x07)]
def read_gyro(): return [round(twos_comp(r(0x69, rG) << 8 | r(0x69, rG - 1), 16) / 131, 2) for rG in (0x03, 0x05, 0x07)]
def read_mag(): return [round(twos_comp(r(0x13, rM) << 8 | r(0x13, rM + 1), 16) / 16, 2) for rM in (0x42, 0x44, 0x46)]

bus = sb.SMBus(1)
w(0x19, 0x0F, 0x05)
w(0x69, 0x0F, 0x00)
w(0x13, 0x4B, 0x01);
sleep(0.1);
w(0x13, 0x4C, 0x00);
# w(0x13, 0x4B, 0x83);

while 1:
    sleep(0.1)
    print("Acc:", read_acc(), "Gyro:", read_gyro(), "Mag:", read_mag())
#    print(f"Acc: {read_acc():.2f} Gyro: {read_gyro():.2f} Mag: {read_mag():.2f}")

[tn493]

Slack通知テスト

[tn493]

コメント通知テスト

[tn493]

服部さんありがとうございました、求めていた機能でした。