private void OpenBones(string path)
{
data.Clear();
if (File.Exists(path))
{
using (TextReader tr = new StreamReader(path))
{
string line;
while ((line = tr.ReadLine()) != null)
{
if (line.StartsWith("//") ||
line.StartsWith(";") ||
line.StartsWith("#") ||
line.StartsWith("\""))
{
continue;
}
string[] row = line.Split(',');
try
{
var joint = new JointConfigure(row);
data.Add(joint);
}
catch (Exception e)
{
log.WriteLine("ボーン構造読み取りエラー: " + line);
}
}
}
}
}
private void MainForm_Load(object sender, EventArgs e)
{
EnabledChanger = new EnabledChangeHandler(EnabledChange);
github = new GitHubReleaseCheck("ixsiid", "JointTracker", "firmware");
github.OnComplete += OnCompleteFirmwareVersionCheck;
github.Start();
python = new ProcessStartInfo("cmd", "/c python -m pip list esptool");
python.RedirectStandardError = true;
python.RedirectStandardOutput = true;
python.UseShellExecute = false;
python.CreateNoWindow = true;
InstallESPTool();
gvBone.Columns.AddRange(JointConfigure.GetColumns());
bones = new List <JointConfigure>();
string defaultCsvFile = "default_bones.csv";
data = new BindingSource()
{
DataSource = bones
};
gvBone.DataSource = data;
OpenBones(defaultCsvFile);
RefreshListSerialPort();
}
private void bRowAdd_Click(object sender, EventArgs e)
{
var bone = new JointConfigure(new string[] { "True", "VMT_", "1", "1", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "1.0" });
data.Add(bone);
}
private void bParentConfigure_Click(object sender, EventArgs e)
{
bParentConfigure.Enabled = false;
SerialPort port;
try
{
port = new SerialPort(cbParentPort.SelectedItem.ToString(), 115200);
}
catch (Exception exc)
{
log.WriteLine("ポートを開くのに失敗しました。\n > " + exc.Message);
return;
}
log.WriteLine("ParentデバイスのConfigureを開始します");
Task configureTask = Task.Run(() =>
{
const UInt32 cmd_wifi = 0x128422fd;
const UInt32 cmd_reboot = 0xff8d91a8;
const UInt32 cmd_bonecount = 0x38771d81;
const UInt32 cmd_fixbone = 0xf729adc8;
const UInt32 cmd_movable = 0xab8cf912;
const UInt32 cmd_host = 0x431fac89;
JointConfigure[] fix = bones.Where(x => x.IsMovableBone == JointConfigure.BoneKind.Fix).ToArray();
JointConfigure[] mov = bones.Where(x => x.IsMovableBone == JointConfigure.BoneKind.Movable).ToArray();
int fix_count = fix.Length;
int mov_count = mov.Length;
bool host_configured = false;
string[] ipValues = tbIp.Text.Split('.').ToArray();
if (ipValues.Length != 4)
{
host_configured = true;
log.WriteLineAsync("IP addressの書式が間違っています。Configureされません");
}
byte[] ip = { 0, 0, 0, 0 };
if (ipValues.All(x => byte.TryParse(x, out _)))
{
ip = ipValues.Select(x => byte.Parse(x)).ToArray();
}
else
{
host_configured = true;
log.WriteLineAsync("IP addressの書式が間違っています。Configureされません");
}
port.Open();
bool wifi_configured = tbSsid.Text == "";
bool count_configured = bones.Count == 0;
int fix_configured_index = 0;
int mov_configured_index = 0;
if (wifi_configured)
{
log.WriteLineAsync("SSIDが空白なため、Configureされません");
}
if (count_configured)
{
log.WriteLineAsync("ボーン構造が空白なため、Configureされません");
}
byte[] buffer = new byte[128];
int err_line_count = 0;
port.WriteLine("dummy");
while (true)
{
string line = port.ReadLine();
if (!line.EndsWith(">\r"))
{
log.WriteLineAsync("> " + line);
err_line_count++;
if (err_line_count > 64)
{
log.WriteLineAsync("デバイスとの対話モードに失敗しました。");
break;
}
continue;
}
byte[] bs;
int b = 0;
UInt32 length;
if (!wifi_configured)
{
length = 4 + 4 + 32 + 32;
bs = BitConverter.GetBytes(length);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
bs = BitConverter.GetBytes(cmd_wifi);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
for (int i = 0; i < 64; i++)
{
buffer[b + i] = 0x00;
}
for (int i = 0; i < 31 && i < tbSsid.Text.Length; i++)
{
buffer[b++] = (byte)tbSsid.Text[i];
}
b = 4 + 4 + 32;
for (int i = 0; i < 31 && i < tbPassphrase.Text.Length; i++)
{
buffer[b++] = (byte)tbPassphrase.Text[i];
}
port.BaseStream.Write(buffer, 0, (int)length);
wifi_configured = true;
log.WriteLineAsync("WiFi情報を設定しました");
Task.Delay(1000);
continue;
}
if (!host_configured)
{
length = 4 + 4 + 4 + 4;
bs = BitConverter.GetBytes(length);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
bs = BitConverter.GetBytes(cmd_host);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
for (int i = 0; i < 4; i++)
{
buffer[b++] = ip[i];
}
byte[] _port = BitConverter.GetBytes((ushort)nmPort.Value);
for (int i = 0; i < 2; i++)
{
buffer[b++] = _port[i];
}
buffer[b++] = 0;
buffer[b++] = 0;
port.BaseStream.Write(buffer, 0, (int)length);
host_configured = true;
log.WriteLineAsync("ホスト情報を設定しました");
Task.Delay(1000);
continue;
}
if (!count_configured)
{
length = 4 + 4 + 4;
bs = BitConverter.GetBytes(length);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
bs = BitConverter.GetBytes(cmd_bonecount);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
buffer[b++] = (byte)fix_count;
buffer[b++] = (byte)mov_count;
buffer[b++] = 0;
buffer[b++] = 0;
port.BaseStream.Write(buffer, 0, (int)length);
count_configured = true;
log.WriteLineAsync("ボーン構造の設定を開始しました");
Task.Delay(1000);
continue;
}
if ((fix_configured_index + mov_configured_index) < (fix_count + mov_count))
{
length = 4 + 4 + 20 + 4 + 12 + 16;
bs = BitConverter.GetBytes(length);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
bool mode_fix = fix_configured_index < fix_count;
bs = BitConverter.GetBytes(mode_fix ? cmd_fixbone : cmd_movable);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
int t = mode_fix ? fix_configured_index : mov_configured_index;
JointConfigure target = mode_fix ? fix[t] : mov[t];
for (int i = 0; i < 20; i++)
{
buffer[b + i] = 0;
}
bs = Encoding.ASCII.GetBytes(target.JointRootSerial);
for (int i = 0; i < 19 && i < bs.Length; i++)
{
buffer[b + i] = bs[i];
}
b += 20;
buffer[b++] = 0;
buffer[b++] = (byte)t;
buffer[b++] = target.WorkerAddress;
buffer[b++] = target.TrackerIndex;
float[] fs = { target.PositionX, target.PositionY, target.PositionZ, target.QuaternionX, target.QuaternionY, target.QuaternionZ, target.QuaternionW };
for (int i = 0; i < fs.Length; i++)
{
bs = BitConverter.GetBytes(fs[i]);
for (int j = 0; j < 4; j++)
{
buffer[b++] = bs[j];
}
}
port.BaseStream.Write(buffer, 0, (int)length);
if (mode_fix)
{
fix_configured_index++;
}
else
{
mov_configured_index++;
}
log.WriteLineAsync((mode_fix ? "固定" : "可動") + "ボーン [" + t + "]を設定しました");
Task.Delay(1000);
continue;
}
log.WriteLineAsync("全ての情報を設定しました");
length = 4 + 4;
bs = BitConverter.GetBytes(length);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
bs = BitConverter.GetBytes(cmd_reboot);
for (int i = 0; i < 4; i++)
{
buffer[b++] = bs[i];
}
port.BaseStream.Write(buffer, 0, (int)length);
log.WriteLineAsync("デバイスを再起動させます");
break;
}
port.Close();
ChangeEnableAsync(bParentConfigure, true);
ChangeEnableAsync(bWorkerConfigure, true);
});
}
