private void buttonScan_Click(object sender, EventArgs e)
{
joyconManager.Scan();
UpdateDebugType();
UpdateInfo();
}
private void buttonScan_Click(object sender, EventArgs e)
{
joyconManager.Scan();
UpdateDebugType();
label1.Text = "搜尋裝置成功";
buttonStart.Enabled = true;
}
private void buttonScan_Click(object sender, EventArgs e)
{
if (!scanned)
{
manager.Scan();
UpdateDebug();
InfoRefresh();
manager.Start();
timerUpdate.Enabled = true;
scanned = true;
}
}
/*
* Controller Button Packet / Slot 0:
*
* YawByte[0] -> L_Trigger (0-255)
* YawByte[1] -> R_Trigger (0-255)
* YawByte[2] -> L_System (0;1)
* YawByte[3] -> R_System (0;1)
* PitchByte[0] -> L_Menu (0;1)
* PitchByte[1] -> R_Menu (0;1)
* PitchByte[2] -> L_Grip (0;1)
* PitchByte[3] -> R_Grip (0;1)
* RollByte[0] -> L_TouchPadPress (0:1)
* RollByte[1] -> R_TouchPadPress (0:1)
* RollByte[2] -> [not used]
* RollByte[3] -> [not used]
* XByte[0] -> L_TouchPadAxisX (0-255)
* XByte[1] -> R_TouchPadAxisX (0-255)
* XByte[2] -> L_TouchPadAxisY (0-255)
* XByte[3] -> R_TouchPadAxisY (0-255)
* Y -> [not used]
* Z -> [not used]
*/
static void Main(string[] args)
{
Console.WriteLine("Beginning Setup");
SetFreePIEDllPath();
packets = new Data[4];
DriftYawOffset = new double[2];
JoyconPosition = new Vector3[2];
JoyconPosition[0] = new Vector3(0, 0, 0);
JoyconPosition[1] = new Vector3(0, 0, 0);
HomePosition = new Vector3[2];
HomePosition[0] = new Vector3(0, 0, 0);
HomePosition[1] = new Vector3(0, 0, 0);
joyconManager.Scan();
Thread.Sleep(1000);
if (joyconManager.j.Count < 2)
{
Console.WriteLine("0 or 1 JoyCon found" + Environment.NewLine + "Joycons disabled (yet)");
}
else
{
Console.WriteLine("Found " + joyconManager.j.Count + " JoyCons");
joyconManager.Start();
Console.WriteLine(Environment.NewLine + Environment.NewLine);
Console.WriteLine("Calibrating Sensors -> leave Joycons on a flat surface until calibration is finished");
Console.WriteLine("To Start the calibration press ENTER/RETURN");
Console.WriteLine("Awaiting calibration...");
if (joyconManager.j[0].isLeft)
{
L = 0;
R = 1;
}
else
{
L = 1;
R = 0;
}
if (!skipCalib)
{
calibFinished = new bool[2];
calibFinished[0] = false;
calibFinished[1] = false;
Console.ReadLine();
Console.WriteLine("Beginning calibration. This will take " + (DriftSamples * DriftSampleDelay / 1000) + " seconds");
calibration(L);
calibration(R);
Thread.Sleep(DriftSamples * DriftSampleDelay + 500);
resetOrientation(L);
resetOrientation(R);
Console.WriteLine("Calibration complete");
}
else
{
Console.WriteLine("!!!!! CALIBRATION SKIPPED !!!!!");
calibFinished = new bool[2];
calibFinished[0] = true;
calibFinished[1] = true;
}
}
Task.Run(async() =>
{
using (var udp = new UdpClient(port))
{
string[] data;
while (true)
{
//This UdpClient receives Data from the Raspberry Pi
var rec = await udp.ReceiveAsync();
string sup = Encoding.ASCII.GetString(rec.Buffer);
data = Regex.Split(sup, "aaaa");//Any non numeric seperator
//At the moment im only transfering 3 numbers (double)
X1 = Convert.ToDouble(data[0]) * x1m;
Y1 = Convert.ToDouble(data[1]) * y1m;
Z1 = (Convert.ToDouble(data[2]) * z1m) - z1o;
}
}
});
DateTime tick = DateTime.Now;
while (true)
{
if (joyconManager.j[L].GetButtonDown(Joycon.Button.CAPTURE))
{
resetOrientation(L);
}
if (joyconManager.j[R].GetButtonDown(Joycon.Button.HOME))
{
resetOrientation(R);
}
packets[0] = new Data
{
Yaw = ((joyconManager.j[R].GetButton(Joycon.Button.PLUS) ? 1:0) << 24) + ((joyconManager.j[L].GetButton(Joycon.Button.MINUS) ? 1 : 0) << 16) + ((joyconManager.j[R].GetButton(Joycon.Button.SHOULDER_2) ? 255 : 0) << 8) + (joyconManager.j[L].GetButton(Joycon.Button.SHOULDER_2) ? 255 : 0),
Pitch = ((joyconManager.j[R].GetButton(Joycon.Button.SL) ? 1 : 0) << 24) + ((joyconManager.j[L].GetButton(Joycon.Button.SR) ? 1 : 0) << 16) + ((joyconManager.j[R].GetButton(Joycon.Button.DPAD_LEFT) ? 1 : 0) << 8) + (joyconManager.j[L].GetButton(Joycon.Button.DPAD_LEFT) ? 1 : 0),
Roll = ((joyconManager.j[R].GetButton(Joycon.Button.STICK) ? 1 : 0) << 8) + (joyconManager.j[L].GetButton(Joycon.Button.STICK) ? 1 : 0),
X = ((int)((joyconManager.j[R].GetStick()[1] + 0.03f) * 0.5f * 255 + 0.5f * 255) << 24) + ((int)(joyconManager.j[L].GetStick()[1] * 0.5f * 255 + 0.5f * 255) << 16) + ((int)((joyconManager.j[R].GetStick()[1] + 0.03f) * 0.5f * 255 + 0.5f * 255) << 8) + (int)(joyconManager.j[L].GetStick()[0] * 0.5f * 255 + 0.5f * 255)
};
packets[1] = new Data
{
X = (float)X1,
Y = (float)Y1,
Z = (float)Z1
};
YAW2 = (getYaw(L) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftYawL) - DriftYawOffset[L]);
PITCH2 = (getPitch(L) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftPitchL));
ROLL2 = (getRoll(L) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftRollL));
calculatePosition(L);
X2 = JoyconPosition[L].X;
Y2 = JoyconPosition[L].Y;
Z2 = JoyconPosition[L].Z;
packets[2] = new Data
{
X = (float)X2,
Y = (float)Y2,
Z = (float)Z2,
Yaw = (float)YAW2,
Pitch = (float)PITCH2,
Roll = (float)ROLL2
};
YAW3 = (getYaw(R) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftYawR) - DriftYawOffset[R]);
PITCH3 = (getPitch(R) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftPitchR));
ROLL3 = (getRoll(R) - (DateTime.Now.Subtract(tick).TotalSeconds *DriftRollR));
calculatePosition(R);
X3 = JoyconPosition[R].X;
Y3 = JoyconPosition[R].Y;
Z3 = JoyconPosition[R].Z;
packets[3] = new Data
{
X = (float)X3,
Y = (float)Y3,
Z = (float)Z3,
Yaw = (float)YAW3,
Pitch = (float)PITCH3,
Roll = (float)ROLL3
};
var result = freepie_io_6dof_write(0, 4, packets);
if (result != 0)
{
throw new Exception("Could not write to IO slots. Try reinstalling FreePie and reboot your PC");
}
var res = freepie_io_6dof_read(1, 1, out headset);
}
}