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.
375 lines
13 KiB
375 lines
13 KiB
1 month ago
|
using System.Collections;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using UnityEngine;
|
||
|
|
using UDESDK;
|
||
|
|
using UnityEngine.UI;
|
||
|
|
using System.Linq;
|
||
|
|
using System;
|
||
|
|
using System.Reflection;
|
||
|
|
|
||
|
|
public class Sample : MonoBehaviour
|
||
|
|
{
|
||
|
|
public static UDE_SDK _SDK = new();
|
||
|
|
|
||
|
|
public GameObject BlePerfab;
|
||
|
|
public GameObject Content;
|
||
|
|
public List<GameObject> DevicePool = new();
|
||
|
|
|
||
|
|
public Text LogTxt;
|
||
|
|
public GameObject[] Panels;
|
||
|
|
|
||
|
|
private Action<string, bool> DeviceListener;
|
||
|
|
|
||
|
|
[Header("Finger Joints")]
|
||
|
|
[Space]
|
||
|
|
public HandDriver[] handDrivers;
|
||
|
|
private Transform[] LeftHands;
|
||
|
|
private Transform[] RightHands;
|
||
|
|
|
||
|
|
public Image[] JoysticksBtn;
|
||
|
|
private GameObject[] Joysticks;
|
||
|
|
public Image[] Trackpad;
|
||
|
|
public Image[] ButtonA;
|
||
|
|
public Image[] ButtonB;
|
||
|
|
public Image[] TriggerBtn;
|
||
|
|
public Image[] GripBtn;
|
||
|
|
|
||
|
|
public Button[] AutoCalibration;
|
||
|
|
private float time = 0;
|
||
|
|
|
||
|
|
private static string[] TwoDeviceName = new string[2] { "", ""};
|
||
|
|
private Color[] ActiveColor = new Color[2] { new(0.4622f, 0.4622f, 0.4622f), new(0.2467f, 0.3565f, 0.6792f) };
|
||
|
|
|
||
|
|
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<int, string> TypeToName = new()
|
||
|
|
{
|
||
|
|
{ -1, "A Finish"},
|
||
|
|
{ -2, "B Finish"},
|
||
|
|
{ 0, "Not Start"},
|
||
|
|
{ 1, "A start"},
|
||
|
|
{ 2, "B Start"},
|
||
|
|
{ 3, "C Start"},
|
||
|
|
{ 4, "Completed"}
|
||
|
|
};
|
||
|
|
private Dictionary<UDE_SDK.FingerJointType, Quaternion> LeftHandDefault = new();
|
||
|
|
private Dictionary<UDE_SDK.FingerJointType, Quaternion> RightHandDefault = new();
|
||
|
|
|
||
|
|
// Start is called before the first frame update
|
||
|
|
void Start()
|
||
|
|
{
|
||
|
|
_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
|
||
|
|
};
|
||
|
|
|
||
|
|
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
|
||
|
|
};
|
||
|
|
|
||
|
|
for (int i = 0; i < JointOrder.Length; ++i)
|
||
|
|
{
|
||
|
|
LeftHandDefault.Add(JointOrder[i], LeftHands[i].localRotation);
|
||
|
|
RightHandDefault.Add(JointOrder[i], RightHands[i].localRotation);
|
||
|
|
}
|
||
|
|
DeviceListener += DeviceConnectListen;
|
||
|
|
Joysticks = new GameObject[2] { JoysticksBtn[0].transform.GetChild(0).gameObject, JoysticksBtn[1].transform.GetChild(0).gameObject };
|
||
|
|
AutoCalibration[0].onClick.AddListener(() => { StartCoroutine(nameof(CalibrationClockLeft)); });
|
||
|
|
AutoCalibration[1].onClick.AddListener(() => { StartCoroutine(nameof(CalibrationClockRight)); });
|
||
|
|
Application.targetFrameRate = 100;
|
||
|
|
}
|
||
|
|
bool test = false;
|
||
|
|
public void RefreshList()
|
||
|
|
{
|
||
|
|
var list = _SDK.UDE_GetBleDeviceList();
|
||
|
|
if (test)
|
||
|
|
{
|
||
|
|
for(int i = 0;i<list.Count; ++i)
|
||
|
|
{
|
||
|
|
if (DevicePool.Find(gameobj => gameobj.name == list[i]) != null)
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
if (string.IsNullOrEmpty(list[i]))
|
||
|
|
{
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if (i >= DevicePool.Count)
|
||
|
|
{
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
GameObject go = DevicePool[i];
|
||
|
|
go.SetActive(true);
|
||
|
|
go.name = list[i];
|
||
|
|
var bleDevice = go.AddComponent<BleDevice>();
|
||
|
|
bleDevice.CreateBleDevice(go, list[i], "", DeviceListener);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
foreach (var DeviceName in list)
|
||
|
|
{
|
||
|
|
if (DevicePool.Find(gameobj => gameobj.name == DeviceName) != null)
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
GameObject go = Instantiate(BlePerfab);
|
||
|
|
go.name = DeviceName;
|
||
|
|
var bleDevice = go.AddComponent<BleDevice>();
|
||
|
|
go.transform.parent = Content.transform;
|
||
|
|
bleDevice.CreateBleDevice(go, DeviceName, "", DeviceListener);
|
||
|
|
DevicePool.Add(go);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
bool CalibReady = true;
|
||
|
|
// Update is called once per frame
|
||
|
|
void Update()
|
||
|
|
{
|
||
|
|
if(Input.GetKeyDown(KeyCode.F1))
|
||
|
|
{
|
||
|
|
if (DevicePool.Find(gameobj => gameobj.name == "DeviceName") == null)
|
||
|
|
{
|
||
|
|
GameObject go = Instantiate(BlePerfab);
|
||
|
|
go.name = "DeviceName";
|
||
|
|
var bleDevice = go.AddComponent<BleDevice>();
|
||
|
|
go.transform.parent = Content.transform;
|
||
|
|
bleDevice.CreateBleDevice(go, "DeviceName", "", DeviceListener);
|
||
|
|
DevicePool.Add(go);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
LogTxt.text = "";
|
||
|
|
foreach (GameObject go in DevicePool)
|
||
|
|
{
|
||
|
|
if(!go.GetComponent<BleDevice>().IsConnected)
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
string DeviceName = go.name;
|
||
|
|
if (go.name[^1] == 'L')
|
||
|
|
{
|
||
|
|
TwoDeviceName[0] = go.name;
|
||
|
|
}
|
||
|
|
else if (go.name[^1] == 'R')
|
||
|
|
{
|
||
|
|
TwoDeviceName[1] = go.name;
|
||
|
|
}
|
||
|
|
_SDK.UDE_RunningDataUpdate(DeviceName);
|
||
|
|
string msg = " " + string.Join(", ", _SDK.UDE_GetRawAngleData(DeviceName, true).ToArray());
|
||
|
|
string msg2 = " " + string.Join(", ", _SDK.UDE_GetRawAngleData(DeviceName).ToArray());
|
||
|
|
|
||
|
|
LogTxt.text += DeviceName + " [" + TypeToName[(int)_SDK.UDE_GetCalibrationType(DeviceName)] + "] \nRaw Data:" + msg + "\n" + "Angle:" + msg2 + "\n";
|
||
|
|
|
||
|
|
var DataDict = _SDK.UDE_GetFingerRotations(DeviceName);
|
||
|
|
if(DataDict == null)
|
||
|
|
{
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
int index = 0;
|
||
|
|
foreach(var joint in JointOrder)
|
||
|
|
{
|
||
|
|
if (go.name[^1] == 'L')
|
||
|
|
{
|
||
|
|
LeftHands[index].localRotation = LeftHandDefault[joint];
|
||
|
|
LeftHands[index++].Rotate(DataDict[joint]);
|
||
|
|
}
|
||
|
|
else if (go.name[^1] == 'R')
|
||
|
|
{
|
||
|
|
RightHands[index].localRotation = RightHandDefault[joint];
|
||
|
|
RightHands[index++].Rotate(DataDict[joint]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
var ExtraDeviceData = _SDK.UDE_GetInputDeviceData(DeviceName);
|
||
|
|
index = 0;
|
||
|
|
if(go.name[^1] == 'R')
|
||
|
|
{
|
||
|
|
index = 1;
|
||
|
|
}
|
||
|
|
float joy_x = ExtraDeviceData.joyX;
|
||
|
|
float joy_y = ExtraDeviceData.joyY;
|
||
|
|
Joysticks[index].GetComponentInChildren<Text>().text = Math.Round(joy_x, 2) + ", " + Math.Round(joy_y, 2);
|
||
|
|
Debug.Log($"Joy{index} {joy_x} {joy_y}");
|
||
|
|
if (Math.Pow(joy_x, 2) + Math.Pow(joy_y, 2) <= 1)
|
||
|
|
{
|
||
|
|
Joysticks[index].transform.localPosition = new Vector3(64 * joy_x, 64 * joy_y, 0);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
var k = joy_y / joy_x;
|
||
|
|
var Fix_x = joy_x > 0 ? Math.Sqrt(1 / (1 + Math.Pow(k, 2))) : -Math.Sqrt(1 / (1 + Math.Pow(k, 2)));
|
||
|
|
var Fix_y = k * Fix_x;
|
||
|
|
Joysticks[index].transform.localPosition = new Vector3(64 * (float)Fix_x, 64 * (float)Fix_y, 0);
|
||
|
|
}
|
||
|
|
JoysticksBtn[index].color = ExtraDeviceData.joyButton ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
Trackpad[index].color = ExtraDeviceData.trackpad_touch ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
ButtonA[index].color = ExtraDeviceData.aButton ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
ButtonB[index].color = ExtraDeviceData.bButton ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
TriggerBtn[index].color = ExtraDeviceData.trgButton ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
GripBtn[index].color = ExtraDeviceData.grab ? ActiveColor[1] : ActiveColor[0];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void DeviceConnectListen(string DeviceName, bool state)
|
||
|
|
{
|
||
|
|
string msg = state ? "in connecting" : "disconnected";
|
||
|
|
Debug.Log(DeviceName + " is " + msg);
|
||
|
|
foreach (GameObject go in DevicePool)
|
||
|
|
{
|
||
|
|
if (go.name == DeviceName)
|
||
|
|
{
|
||
|
|
go.GetComponent<BleDevice>().IsConnected = state;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public void VibrationButtonL(int placeindex)
|
||
|
|
{
|
||
|
|
_SDK.UDE_ControlVibration(TwoDeviceName[0], placeindex, 20, 5);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void VibrationButtonR(int placeindex)
|
||
|
|
{
|
||
|
|
_SDK.UDE_ControlVibration(TwoDeviceName[1], placeindex, 20, 5);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void JoystickCenterCalib(int index)
|
||
|
|
{
|
||
|
|
_SDK.UDE_JoystickCenterCalibration(TwoDeviceName[index]);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void JoystickRangeCalibStart(int index)
|
||
|
|
{
|
||
|
|
_SDK.UDE_JoystickRangeCalibration(TwoDeviceName[index], true);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void JoystickRangeCalibStop(int index)
|
||
|
|
{
|
||
|
|
_SDK.UDE_JoystickRangeCalibration(TwoDeviceName[index], false);
|
||
|
|
}
|
||
|
|
|
||
|
|
public void ChangePage()
|
||
|
|
{
|
||
|
|
if (Panels[0].activeSelf)
|
||
|
|
{
|
||
|
|
Panels[0].SetActive(false);
|
||
|
|
Panels[1].SetActive(true);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
Panels[0].SetActive(true);
|
||
|
|
Panels[1].SetActive(false);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private void OnApplicationQuit()
|
||
|
|
{
|
||
|
|
_SDK.UDE_BleStopSearching();
|
||
|
|
}
|
||
|
|
|
||
|
|
private IEnumerator CalibrationClockLeft()
|
||
|
|
{
|
||
|
|
if(time % 3 == 0)
|
||
|
|
{
|
||
|
|
switch (time)
|
||
|
|
{
|
||
|
|
case 0:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Fist, true);
|
||
|
|
Debug.Log($"{TwoDeviceName[0]} {TwoDeviceName[1]} a start");
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Fist, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Adduct, true);
|
||
|
|
Debug.Log("a fin, b start");
|
||
|
|
break;
|
||
|
|
case 6:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Adduct, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Stretch, true);
|
||
|
|
Debug.Log("b fin, c start");
|
||
|
|
break;
|
||
|
|
case 9:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[0], UDE_SDK.CalibrationType.Stretch, false);
|
||
|
|
Debug.Log("all finished");
|
||
|
|
time = 0;
|
||
|
|
yield break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
yield return new WaitForSeconds(1);
|
||
|
|
time++;
|
||
|
|
yield return CalibrationClockLeft();
|
||
|
|
}
|
||
|
|
|
||
|
|
private IEnumerator CalibrationClockRight()
|
||
|
|
{
|
||
|
|
if (time % 3 == 0)
|
||
|
|
{
|
||
|
|
switch (time)
|
||
|
|
{
|
||
|
|
case 0:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Fist, true);
|
||
|
|
Debug.Log($"{TwoDeviceName[0]} {TwoDeviceName[1]} a start");
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Fist, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Adduct, true);
|
||
|
|
Debug.Log("a fin, b start");
|
||
|
|
break;
|
||
|
|
case 6:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Adduct, false);
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Stretch, true);
|
||
|
|
Debug.Log("b fin, c start");
|
||
|
|
break;
|
||
|
|
case 9:
|
||
|
|
_SDK.UDE_CalibrationMotionController(TwoDeviceName[1], UDE_SDK.CalibrationType.Stretch, false);
|
||
|
|
Debug.Log("all finished");
|
||
|
|
time = 0;
|
||
|
|
yield break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
yield return new WaitForSeconds(1);
|
||
|
|
time++;
|
||
|
|
yield return CalibrationClockRight();
|
||
|
|
}
|
||
|
|
}
|