You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
324 lines
12 KiB
324 lines
12 KiB
1 month ago
|
using System.Collections.Generic;
|
||
|
|
using TMPro;
|
||
|
|
using UDESDK;
|
||
|
|
using UnityEngine;
|
||
|
|
using UnityEngine.UI;
|
||
|
|
|
||
|
|
public class BleManager : MonoBehaviour
|
||
|
|
{
|
||
|
|
public static UDE_SDK _SDK = new();
|
||
|
|
public string MyselfTest;
|
||
|
|
public string LeftDeviceName { get; private set; }
|
||
|
|
public string RightDeviceName { get; private set; }
|
||
|
|
public string LeftAlgorithmVer { get; private set; }
|
||
|
|
public string RightAlgorithmVer { get; private set; }
|
||
|
|
public bool LeftConnectState;
|
||
|
|
public bool RightConnectState;
|
||
|
|
|
||
|
|
[Header("Finger Joints")]
|
||
|
|
[Space]
|
||
|
|
public HandDriver[] handDrivers;
|
||
|
|
private Transform[] LeftHands;
|
||
|
|
private Vector3[] LeftHandsVec3;
|
||
|
|
private Transform[] RightHands;
|
||
|
|
private Vector3[] RightHandsVec3;
|
||
|
|
[HideInInspector]
|
||
|
|
public List<double> LeftRawData;
|
||
|
|
[HideInInspector]
|
||
|
|
public List<double> LeftRawAngleData;
|
||
|
|
[HideInInspector]
|
||
|
|
public List<double> RightRawData;
|
||
|
|
[HideInInspector]
|
||
|
|
public List<double> RightRawAngleData;
|
||
|
|
|
||
|
|
public double[] LeftCalibrationOffset { get; private set; }
|
||
|
|
public double[] RightCalibrationOffset { get; private set; }
|
||
|
|
|
||
|
|
public TextMeshProUGUI[] FPS;
|
||
|
|
public Text DebugText;
|
||
|
|
|
||
|
|
private UDE_SDK.FingerJointType[] JointOrder =
|
||
|
|
{
|
||
|
|
UDE_SDK.FingerJointType.Thumb1, UDE_SDK.FingerJointType.Thumb2, UDE_SDK.FingerJointType.Thumb3,
|
||
|
|
UDE_SDK.FingerJointType.Index1, UDE_SDK.FingerJointType.Index2, UDE_SDK.FingerJointType.Index3,
|
||
|
|
UDE_SDK.FingerJointType.Middle1, UDE_SDK.FingerJointType.Middle2, UDE_SDK.FingerJointType.Middle3,
|
||
|
|
UDE_SDK.FingerJointType.Ring1, UDE_SDK.FingerJointType.Ring2, UDE_SDK.FingerJointType.Ring3,
|
||
|
|
UDE_SDK.FingerJointType.Pinky1, UDE_SDK.FingerJointType.Pinky2, UDE_SDK.FingerJointType.Pinky3
|
||
|
|
};
|
||
|
|
|
||
|
|
private Dictionary<UDE_SDK.FingerJointType, Quaternion> LeftHandDefault = new();
|
||
|
|
private Dictionary<UDE_SDK.FingerJointType, Quaternion> RightHandDefault = new();
|
||
|
|
|
||
|
|
[HideInInspector]
|
||
|
|
public bool UsingAndroidService;
|
||
|
|
void Start()
|
||
|
|
{
|
||
|
|
gameObject.SetActive(false);
|
||
|
|
#if !UNITY_EDITOR
|
||
|
|
gameObject.SetActive(true);
|
||
|
|
handDrivers[0].UsingNetwork = false;
|
||
|
|
handDrivers[1].UsingNetwork = false;
|
||
|
|
#endif
|
||
|
|
if (UsingAndroidService) return;
|
||
|
|
_SDK.UDE_BleInit();
|
||
|
|
_SDK.UDE_BleStartSearching();
|
||
|
|
|
||
|
|
LeftHands = new Transform[15]
|
||
|
|
{
|
||
|
|
handDrivers[0].Thumb1,
|
||
|
|
handDrivers[0].Thumb2,
|
||
|
|
handDrivers[0].Thumb3,
|
||
|
|
handDrivers[0].Index1,
|
||
|
|
handDrivers[0].Index2,
|
||
|
|
handDrivers[0].Index3,
|
||
|
|
handDrivers[0].Middle1,
|
||
|
|
handDrivers[0].Middle2,
|
||
|
|
handDrivers[0].Middle3,
|
||
|
|
handDrivers[0].Ring1,
|
||
|
|
handDrivers[0].Ring2,
|
||
|
|
handDrivers[0].Ring3,
|
||
|
|
handDrivers[0].Pinky1,
|
||
|
|
handDrivers[0].Pinky2,
|
||
|
|
handDrivers[0].Pinky3
|
||
|
|
};
|
||
|
|
LeftHandsVec3 = new Vector3[15]
|
||
|
|
{
|
||
|
|
handDrivers[0].thumb1,
|
||
|
|
handDrivers[0].thumb2,
|
||
|
|
handDrivers[0].thumb3,
|
||
|
|
handDrivers[0].index1,
|
||
|
|
handDrivers[0].index2,
|
||
|
|
handDrivers[0].index3,
|
||
|
|
handDrivers[0].middle1,
|
||
|
|
handDrivers[0].middle2,
|
||
|
|
handDrivers[0].middle3,
|
||
|
|
handDrivers[0].ring1,
|
||
|
|
handDrivers[0].ring2,
|
||
|
|
handDrivers[0].ring3,
|
||
|
|
handDrivers[0].pinky1,
|
||
|
|
handDrivers[0].pinky2,
|
||
|
|
handDrivers[0].pinky3
|
||
|
|
};
|
||
|
|
|
||
|
|
RightHands = new Transform[15]
|
||
|
|
{
|
||
|
|
handDrivers[1].Thumb1,
|
||
|
|
handDrivers[1].Thumb2,
|
||
|
|
handDrivers[1].Thumb3,
|
||
|
|
handDrivers[1].Index1,
|
||
|
|
handDrivers[1].Index2,
|
||
|
|
handDrivers[1].Index3,
|
||
|
|
handDrivers[1].Middle1,
|
||
|
|
handDrivers[1].Middle2,
|
||
|
|
handDrivers[1].Middle3,
|
||
|
|
handDrivers[1].Ring1,
|
||
|
|
handDrivers[1].Ring2,
|
||
|
|
handDrivers[1].Ring3,
|
||
|
|
handDrivers[1].Pinky1,
|
||
|
|
handDrivers[1].Pinky2,
|
||
|
|
handDrivers[1].Pinky3
|
||
|
|
};
|
||
|
|
RightHandsVec3 = new Vector3[15]
|
||
|
|
{
|
||
|
|
handDrivers[1].thumb1,
|
||
|
|
handDrivers[1].thumb2,
|
||
|
|
handDrivers[1].thumb3,
|
||
|
|
handDrivers[1].index1,
|
||
|
|
handDrivers[1].index2,
|
||
|
|
handDrivers[1].index3,
|
||
|
|
handDrivers[1].middle1,
|
||
|
|
handDrivers[1].middle2,
|
||
|
|
handDrivers[1].middle3,
|
||
|
|
handDrivers[1].ring1,
|
||
|
|
handDrivers[1].ring2,
|
||
|
|
handDrivers[1].ring3,
|
||
|
|
handDrivers[1].pinky1,
|
||
|
|
handDrivers[1].pinky2,
|
||
|
|
handDrivers[1].pinky3
|
||
|
|
};
|
||
|
|
|
||
|
|
for (int i = 0; i < JointOrder.Length; ++i)
|
||
|
|
{
|
||
|
|
LeftHandDefault.Add(JointOrder[i], LeftHands[i].localRotation);
|
||
|
|
RightHandDefault.Add(JointOrder[i], RightHands[i].localRotation);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
bool finishConnect = false;
|
||
|
|
bool NeedCD = false;
|
||
|
|
float CDTime = 3;
|
||
|
|
float FirstSearching = 6;
|
||
|
|
|
||
|
|
public bool NeedChooseConnect = false;
|
||
|
|
public List<string> DevicesList { get; private set; }
|
||
|
|
public string LeftSpecificConnect = "";
|
||
|
|
public string RightSpecificConnect = "";
|
||
|
|
|
||
|
|
[HideInInspector]
|
||
|
|
public bool FinJoyCalibration = true;
|
||
|
|
public bool FinishCalibration { get; private set; } = false;
|
||
|
|
void Update()
|
||
|
|
{
|
||
|
|
if (UsingAndroidService) return;
|
||
|
|
|
||
|
|
DebugText.text = "";
|
||
|
|
DevicesList = _SDK.UDE_GetBleDeviceList();
|
||
|
|
|
||
|
|
LeftConnectState = _SDK.UDE_CheckDeviceIsConnect(LeftDeviceName);
|
||
|
|
RightConnectState = _SDK.UDE_CheckDeviceIsConnect(RightDeviceName);
|
||
|
|
if(FPS != null && FPS.Length == 2)
|
||
|
|
{
|
||
|
|
FPS[0].text = "FPS " + _SDK.UDE_GetDeviceTransferFPS(LeftDeviceName);
|
||
|
|
FPS[1].text = "FPS " + _SDK.UDE_GetDeviceTransferFPS(RightDeviceName);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (NeedCD)
|
||
|
|
{
|
||
|
|
CDTime -= Time.deltaTime;
|
||
|
|
NeedCD = !(CDTime <= 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
//DebugText.text = "Count " + DevicesList.Count;
|
||
|
|
if (!finishConnect)
|
||
|
|
{
|
||
|
|
if(DevicesList.Count <= 2)
|
||
|
|
{
|
||
|
|
NeedChooseConnect = false;
|
||
|
|
foreach (string deviceName in DevicesList)
|
||
|
|
{
|
||
|
|
if (deviceName[^1] == 'L' && string.IsNullOrEmpty(LeftDeviceName) && deviceName.Contains(MyselfTest) && !NeedCD)
|
||
|
|
{
|
||
|
|
LeftDeviceName = deviceName;
|
||
|
|
_SDK.UDE_ConnectCertainDevice(LeftDeviceName);
|
||
|
|
NeedCD = true;
|
||
|
|
}
|
||
|
|
else if (deviceName[^1] == 'R' && string.IsNullOrEmpty(RightDeviceName) && deviceName.Contains(MyselfTest) && !NeedCD)
|
||
|
|
{
|
||
|
|
RightDeviceName = deviceName;
|
||
|
|
_SDK.UDE_ConnectCertainDevice(RightDeviceName);
|
||
|
|
NeedCD = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
NeedChooseConnect = true;
|
||
|
|
if (string.IsNullOrEmpty(LeftDeviceName) && !string.IsNullOrEmpty(LeftSpecificConnect) && !NeedCD)
|
||
|
|
{
|
||
|
|
LeftDeviceName = LeftSpecificConnect;
|
||
|
|
_SDK.UDE_ConnectCertainDevice(LeftDeviceName);
|
||
|
|
NeedCD = true;
|
||
|
|
}
|
||
|
|
else if(string.IsNullOrEmpty(RightDeviceName) && !string.IsNullOrEmpty(RightSpecificConnect) && !NeedCD)
|
||
|
|
{
|
||
|
|
RightDeviceName = RightSpecificConnect;
|
||
|
|
_SDK.UDE_ConnectCertainDevice(RightDeviceName);
|
||
|
|
NeedCD = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (DevicesList.Count >= 2 && !string.IsNullOrEmpty(LeftDeviceName) && !string.IsNullOrEmpty(RightDeviceName))
|
||
|
|
{
|
||
|
|
_SDK.UDE_BleStopSearching();
|
||
|
|
finishConnect = true;
|
||
|
|
NeedChooseConnect = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//DebugText.text = "finishConnect " + finishConnect + "/" + LeftDeviceName + "/" + RightDeviceName;
|
||
|
|
|
||
|
|
if (LeftConnectState)
|
||
|
|
{
|
||
|
|
_SDK.UDE_RunningDataUpdate(LeftDeviceName);
|
||
|
|
var DataDict = _SDK.UDE_GetFingerRotations(LeftDeviceName);
|
||
|
|
DebugText.text += LeftDeviceName + "[" + _SDK.UDE_GetCalibrationType(LeftDeviceName) + "]:" + _SDK.UDE_GetRawAngleData(LeftDeviceName, true);
|
||
|
|
LeftRawData = _SDK.UDE_GetRawAngleData(LeftDeviceName, true);
|
||
|
|
LeftRawAngleData = _SDK.UDE_GetRawAngleData(LeftDeviceName);
|
||
|
|
LeftAlgorithmVer = _SDK.UDE_GetAlgorithmVersionName(LeftDeviceName);
|
||
|
|
if (DataDict != null)
|
||
|
|
{
|
||
|
|
int index = 0;
|
||
|
|
foreach (var joint in JointOrder)
|
||
|
|
{
|
||
|
|
LeftHands[index].localRotation = LeftHandDefault[joint];
|
||
|
|
LeftHandsVec3[index] = DataDict[joint];
|
||
|
|
_SDK.UDE_FingerRotate(LeftHands[index++], DataDict[joint]);
|
||
|
|
}
|
||
|
|
handDrivers[0].GetVec3Value(LeftHandsVec3);
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (RightConnectState)
|
||
|
|
{
|
||
|
|
_SDK.UDE_RunningDataUpdate(RightDeviceName);
|
||
|
|
var DataDict = _SDK.UDE_GetFingerRotations(RightDeviceName);
|
||
|
|
DebugText.text += RightDeviceName + "[" + _SDK.UDE_GetCalibrationType(RightDeviceName) + "]:" + _SDK.UDE_GetRawAngleData(RightDeviceName, true);
|
||
|
|
RightRawData = _SDK.UDE_GetRawAngleData(RightDeviceName, true);
|
||
|
|
RightRawAngleData = _SDK.UDE_GetRawAngleData(RightDeviceName);
|
||
|
|
RightAlgorithmVer = _SDK.UDE_GetAlgorithmVersionName(RightDeviceName);
|
||
|
|
if (DataDict != null)
|
||
|
|
{
|
||
|
|
int index = 0;
|
||
|
|
foreach (var joint in JointOrder)
|
||
|
|
{
|
||
|
|
RightHands[index].localRotation = RightHandDefault[joint];
|
||
|
|
RightHandsVec3[index] = DataDict[joint];
|
||
|
|
_SDK.UDE_FingerRotate(RightHands[index++], DataDict[joint]);
|
||
|
|
}
|
||
|
|
handDrivers[1].GetVec3Value(RightHandsVec3);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_SDK.UDE_GetCalibrationType(LeftDeviceName) == UDE_SDK.CalibrationType.Completed && _SDK.UDE_GetCalibrationType(RightDeviceName) == UDE_SDK.CalibrationType.Completed && !FinishCalibration)
|
||
|
|
{
|
||
|
|
Invoke(nameof(GetCalibrationOffset), 1f);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void Calibration(int step)
|
||
|
|
{
|
||
|
|
switch(step)
|
||
|
|
{
|
||
|
|
case 0:
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Fist, true);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Fist, true);
|
||
|
|
FinishCalibration = false;
|
||
|
|
FinJoyCalibration = false;
|
||
|
|
break;
|
||
|
|
case 1:
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Fist, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Fist, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Adduct, true);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Adduct, true);
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Adduct, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Adduct, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Stretch, true);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Stretch, true);
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
_SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Stretch, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Stretch, false);
|
||
|
|
_SDK.UDE_JoystickCenterCalibration(LeftDeviceName);
|
||
|
|
_SDK.UDE_JoystickCenterCalibration(RightDeviceName);
|
||
|
|
Invoke(nameof(GetCalibrationOffset), 1f);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void JoyRangeCalibration(bool control)
|
||
|
|
{
|
||
|
|
_SDK.UDE_JoystickRangeCalibration(LeftDeviceName, control);
|
||
|
|
_SDK.UDE_JoystickRangeCalibration(RightDeviceName, control);
|
||
|
|
FinJoyCalibration = !control;
|
||
|
|
}
|
||
|
|
|
||
|
|
void GetCalibrationOffset()
|
||
|
|
{
|
||
|
|
FinishCalibration = true;
|
||
|
|
LeftCalibrationOffset = _SDK.UDE_GetCalibrationInfoOffset(LeftDeviceName);
|
||
|
|
RightCalibrationOffset = _SDK.UDE_GetCalibrationInfoOffset(RightDeviceName);
|
||
|
|
}
|
||
|
|
}
|