![見出し画像](https://assets.st-note.com/production/uploads/images/107605571/rectangle_large_type_2_021c860d5fd9e850b3ed6fa8007afeab.png?width=1200)
M5STACKのATOM MatrixでG-Bowlもどき ~6~
前回見つけたこちらのページの内容を手計算するコードにしてみました。
コードをNumPyやulabを使わない形に変換してみて驚いたのは、回転行列を求めているのに一切三角関数が出てこない点でした。全ての計算は四則演算のみで行われています。
def rotation_matrix_3d(frm, to=[0, 0, 1.0]):
global r_matrix
to_minus = [-to[0], -to[1], -to[2]]
if frm == to:
r_matrix = [[1.0,0,0],[0,1.0,0],[0,0,1.0]]
if frm == to_minus:
r_matrix = [[-1.0,0,0],[0,-1.0,0],[0,0,-1.0]]
s = [frm[0]+to[0], frm[1]+to[1], frm[2]+to[2]]
s_dot = s[0]**2 + s[1]**2 + s[2]**2
# return 2.0 * np.outer(s, s) / np.dot(s, s) - np.identity(3)
a11 = 2.0 * s[0]*s[0] / s_dot - 1.0
a12 = 2.0 * s[0]*s[1] / s_dot
a13 = 2.0 * s[0]*s[2] / s_dot
a21 = 2.0 * s[1]*s[0] / s_dot
a22 = 2.0 * s[1]*s[1] / s_dot - 1.0
a23 = 2.0 * s[1]*s[2] / s_dot
a31 = 2.0 * s[2]*s[0] / s_dot
a32 = 2.0 * s[2]*s[1] / s_dot
a33 = 2.0 * s[2]*s[2] / s_dot - 1.0
r_matrix = [[a11,a12,a13],[a21,a22,a23],[a31,a32,a33]]
特にこのコードの検証はしていませんが、それっぽく動いているので合っているということにしますw。
あと今回嵌ったのは重力加速度の取り扱いでした。グローバル座標系でZ軸が上向きで加速度センサーが静止している時(重力加速度のみの時)は、重力の向きは下向きなのでZの値としては"-1"の値が得られると思っていたのですが実際に得られる値は"+1"でした。
その辺を詳しく書いてあったデータシートがあったので図を貼っておきます。
![](https://assets.st-note.com/img/1686119843233-FU6stLN1gg.png)
上の図はMPU6886のデータシートのものではありませんが、加速度センサーとしては上の図のようになるのが一般的なのでしょう。そんなわけで上のコードでは3次元ベクトルを回転させる先の初期値としてto=[0, 0, 1.0]としました。
言い忘れてましたが、今回実際にATOM Matrixを使うときのMPU6886の軸とグローバル座標系の軸が異なっているので、事前に軸の入れ替えをしたあとで加速度の計算は行うことにしました。ATOM Matrixに挿さるケーブルが下に来る状態の時に加速度センサーから得られるデータが上の図の軸の向きになるような軸の入れ替えをしています。
![](https://assets.st-note.com/img/1686121503767-0Typcdtbis.png?width=1200)
あとはATOM Matrixのスイッチが押されたら起動時に行っているのと同じ軸の初期化を行うようにしました。ATOM Matrixのスイッチは5x5のLEDになっている面がスイッチになっていて押すことができます。
スイッチを使うためのコードは以下の通りです。
SW_GPIO = const(39)
sw = Pin(SW_GPIO, Pin.IN, Pin.PULL_UP)
これでやりたかったことは全て盛り込めたので、今度こそ今回作ったG-Bowlもどきを実際の車につけて走ってみようと思います。
この記事が気に入ったらサポートをしてみませんか?