Wii角速度計File.1 換算式の評価 I
2015/10/25
話の前提は2011.7.25のエントリ。WiimoteLib1.8betaを用いてWiiモーションプラスを扱ってみた。そこから角速度計へと昇華させる時にはやはり、生データが一体何deg/secを表すのかを把握する必要がある。その試行錯誤のFile.1である。
とりあえず最大の手掛かり、WiiBrewのWiimotionPlusの項のDataFormatの解読を試みる。
以下、原文と拙い翻訳と備忘録解釈
++++++++++++++++++++++++++++++++++++++++++++++++
以下、通常モードで計測できる最大の角速度（幅）が1190°。回路から出力される電圧（幅）が2.7V。２つが比例関係にあり、それが14bit16384段階にAD変換されている事がお話の前提となっているようです。
Wiiリモコンが静止していると、生データは約8063。ただ、より正確な値を得るには、起動の度に校正した方がいい。
電圧の応答は生データの値8192に対して1.35V。角速度1°/secに対する電圧は2.27mV（2.7V÷1190°/s＝0.0022689…）。生データの8192が595°/sに相当（595×16384/8192=1190°）だから、大体生データの13.768が1°/secに相当する（8192/595=13.7680…)
つまり！！
という関係式がある、というわけである。
Slow(通常モード)/HighSpeedモードの切り替わりについて。HighSpeedモードの時は、通常モードの生データと角速度の関係に2000/440を掛けることで関係式が得られる。例えば、（静置状態を8063として、8083という値が得られたときの）生データの変化の20は通常モードでは1.45°/secに相当する（20/13.768=1.4526….）HighSpeedモードの時はこれに2000/440を掛けて、6.59°/secである。（1.4526×2000/440=6.6027….)
先ほどの式と合わせると、
といった換算式がある、というわけですな。
これを基に、角速度計を作成する。こだわりポイントは…
１．角速度を積分して、リモコンの傾き角を表示できるようにする。（１軸は図的に。）
２．軌道の度の校正をできるようにする。（静置状態で得られた５０個程度のデータの平均値をとりましょう。）
３．一応、外部のファイルに保存できる機能を付加する。
相変わらずのソースコード
Imports WiimoteLib
Public Class Form1
Private wm As New Wiimote
Private wx_zero, wy_zero, wz_zero As Integer
Private wx_now, wy_now, wz_now As Single
Private wx_sum, wy_sum, wz_sum As Single
Private wx, wy, wz As Long
Private sw As New System.IO.StreamWriter(“data.csv”, True, System.Text.Encoding.GetEncoding(“shift_jis”))
Private Declare Function timeGetTime Lib “winmm” Alias “timeGetTime” () As Long
Private save As Boolean
Private starttime As Long
Private passedtime As Long
Private Timercount1, Timercount2, Timercount3 As Long
Public Sub New()
InitializeComponent()
End Sub
Private Sub ConnectButton_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles ConnectButton.Click
wm.Connect()
wm.SetReportType(InputReport.IRExtensionAccel, True)
wm.SetLEDs(False, True, True, False)
ConnectButton.BackColor = Color.Pink
‘/////////////////静置時の値を一旦7880とする。
wx_zero = 7880
wy_zero = 7880
wz_zero = 7880
End Sub
Private Sub Timer1_Tick(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Timer1.Tick
Timercount2 = Timercount1
Timercount1 = timeGetTime
Dim ws As WiimoteState = wm.WiimoteState
passedtime = Timercount1 – Timercount2
BattBar.Value = ws.Battery
batt_Val.Text = ws.Battery
‘///Highモードならチェック。/
Yaw_ck.Checked = wm.WiimoteState.MotionPlusState.YawFast
Pitch_ck.Checked = wm.WiimoteState.MotionPlusState.PitchFast
Roll_ck.Checked = wm.WiimoteState.MotionPlusState.RollFast
StateLbl.Text = wm.WiimoteState.ExtensionType.ToString()
‘///生データから角速度に変換。
wx_now = (ws.MotionPlusState.RawValues.X – wx_zero) / 13.768 * (440 – 1560 * (wm.WiimoteState.MotionPlusState.YawFast)) / 440
wy_now = (ws.MotionPlusState.RawValues.Y – wy_zero) / 13.768 * (440 – 1560 * (wm.WiimoteState.MotionPlusState.PitchFast)) / 440
wz_now = (ws.MotionPlusState.RawValues.Z – wz_zero) / 13.768 * (440 – 1560 * (wm.WiimoteState.MotionPlusState.RollFast)) / 440
‘///積分してリモコンの傾き角を求める。
wx_sum = wx_sum + wx_now * (passedtime / 1000)
wy_sum = wy_sum + wy_now * (passedtime / 1000)
wz_sum = wz_sum + wz_now * (passedtime / 1000)
If (Henkan_ck.Checked = True) Then
‘///生データチェックボックスがチェックされていたら、生データを表示。
motionxval.Text = ws.MotionPlusState.RawValues.X
motionyval.Text = ws.MotionPlusState.RawValues.Y
motionzval.Text = ws.MotionPlusState.RawValues.Z
Else
motionxval.Text = Int(wx_now)
motionyval.Text = Int(wy_now)
motionzval.Text = Int(wz_now)
End If
‘///傾き角を表示
x_deg.Text = Int(wx_sum)
y_deg.Text = Int(wy_sum)
z_deg.Text = Int(wz_sum)
‘///図形描画
WiimoteShape.X1 = 82 + 90 * Math.Sin(-(wx_sum) * Math.PI / 180)
WiimoteShape.Y1 = 122 – (90 * Math.Cos(-(wx_sum) * Math.PI / 180))
‘///ファイルへの書き出し
If (save = True) Then
Timercount3 = Timercount1 – starttime
‘sw.WriteLine(Timercount3 & “,” & wx_now & “,” & wy_now & “,” & wz_now & “,” & wx_sum & “,” & wy_sum & “,” & wz_sum)
sw.WriteLine(Timercount3 & “,” & wx_zero & “,” & ws.MotionPlusState.RawValues.X & “,” & wx_now & “,” & wx_sum)
End If
End Sub
Private Sub Form1_FormClosing(ByVal sender As Object, ByVal e As FormClosingEventArgs) Handles MyBase.FormClosing
wm.SetLEDs(False, False, False, False)
wm.Disconnect()
End Sub
Private Sub IniMotionPlus_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles IniMotionPlus.Click
wm.InitializeMotionPlus()
End Sub
Private Sub calib_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles calib.Click
wx = 0
wy = 0
wz = 0
‘///静置状態にして、150ms間隔で50個データを取り、平均して校正。
For i = 0 To 49
Dim ws As WiimoteState = wm.WiimoteState
wx = wx + ws.MotionPlusState.RawValues.X
wy = wy + ws.MotionPlusState.RawValues.Y
wz = wz + ws.MotionPlusState.RawValues.Z
System.Threading.Thread.Sleep(150)
Next i
wx_zero = wx / 50
wy_zero = wy / 50
wz_zero = wz / 50
wx_sum = 0
wy_sum = 0
wz_sum = 0
Timercount1 = timeGetTime
End Sub
Private Sub Sum_reset_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Sum_reset.Click
wx_sum = 0
wy_sum = 0
wz_sum = 0
End Sub
Private Sub Save_button_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles Save_button.Click
If save = False Then
Save_button.Text = “記録中”
Save_button.BackColor = Color.Pink
save = True
startTime = timeGetTime()
‘sw.WriteLine(“timer” & “,” & “wx_now” & “,” & “wy_now” & “,” & “wz_now” & “,” & “wx_sum” & “,” & “wy_sum” & “,” & “wz_sum”)
sw.WriteLine(“timer” & “,” & “wx_zero” & “,” & “wx_raw” & “,” & “wx_now” & “,” & “wx_sum”)
Else
Save_button.Text = “記録”
Save_button.BackColor = Color.Silver
save = False
End If
End Sub
Private Sub Form1_Load(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load
Timercount1 = timeGetTime
End Sub
End Class
・落し物・
サンプルプログラム：WiiAngle_b.zip←右クリック＋対象をファイルに保存
サンプルソース一式：Wiitest20cs.zip←右クリック＋対象をファイルに保存
動かしてみた様子。起動からリモコンとの接続、校正まで。
リモコンを動かしてみた様子。日焼けした手。
そうです。リモコン１周360°を約550°として認識・表示している有様です。やはり、そんなに世の中甘くない。
文献を鵜呑みにせずに、換算式を再検証してみる必要あり、ということでしょうか。
ちなみに、等速円運動演示台に乗せて、徐々に回転数を上げ→下げした時に得られたグラフがこちら。
さながら「鬼が島」。何だが挙動に癖があり怪しい。ただ、滑らかにグラフがつながっているところをみると、なんとなくHighとSlowの対応が2000/440だってのはあっているように思われます。
Wii角速度計File.2 換算式の評価 Ⅱ →進む