HandDriverUPM/HandDriver/SDK/Android Bluetooth SDK/Sample/Sample.cs

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();
    }
}