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.
323 lines
12 KiB
323 lines
12 KiB
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); |
|
} |
|
}
|
|
|