|
|
using Auto_Calibration; |
|
|
using System; |
|
|
using System.Collections; |
|
|
using System.Collections.Generic; |
|
|
using System.Linq; |
|
|
using UnityEngine; |
|
|
|
|
|
public class BleHandDriver : MonoBehaviour |
|
|
{ |
|
|
private AndroidJavaClass AR1_linear_AA; |
|
|
private AndroidJavaClass AR1_linear_AD; |
|
|
private AndroidJavaClass AR1_linear_AE; |
|
|
private AndroidJavaClass AR1_linear_AG; |
|
|
|
|
|
private string Version; |
|
|
|
|
|
public BleHandDriver(string _DeviceName) |
|
|
{ |
|
|
DeviceName = _DeviceName; |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_X_Center") != 0) |
|
|
{ |
|
|
x_center_data = PlayerPrefs.GetFloat($"{DeviceName}_X_Center"); |
|
|
} |
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_Y_Center") != 0) |
|
|
{ |
|
|
y_center_data = PlayerPrefs.GetFloat($"{DeviceName}_Y_Center"); |
|
|
} |
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_X_MAX") != 0) |
|
|
{ |
|
|
x_max_data = PlayerPrefs.GetFloat($"{DeviceName}_X_MAX"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_Y_MAX") != 0) |
|
|
{ |
|
|
y_max_data = PlayerPrefs.GetFloat($"{DeviceName}_Y_MAX"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_X_MIN") != 0) |
|
|
{ |
|
|
x_min_data = PlayerPrefs.GetFloat($"{DeviceName}_X_MIN"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_Y_MIN") != 0) |
|
|
{ |
|
|
y_min_data = PlayerPrefs.GetFloat($"{DeviceName}_Y_MIN"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_DeadZone") != 0) |
|
|
{ |
|
|
dead_zone = PlayerPrefs.GetFloat($"{DeviceName}_DeadZone"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_TriggerValue") != 0) |
|
|
{ |
|
|
trigger_value = PlayerPrefs.GetFloat($"{DeviceName}_TriggerValue"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_GrabValue") != 0) |
|
|
{ |
|
|
grab_value = PlayerPrefs.GetFloat($"{DeviceName}_GrabValue"); |
|
|
} |
|
|
|
|
|
if (PlayerPrefs.GetFloat($"{DeviceName}_TrackpadValue") != 0) |
|
|
{ |
|
|
trackpad_value = PlayerPrefs.GetFloat($"{DeviceName}_TrackpadValue"); |
|
|
} |
|
|
|
|
|
AR1_linear_AA = new("com.example.ar1_linear1.AR1_linear_AA"); |
|
|
AR1_linear_AD = new("com.example.ar1_linear1.AR1_linear_AD"); |
|
|
AR1_linear_AE = new("com.example.ar1_linear1.AR1_linear_AE"); |
|
|
AR1_linear_AG = new("com.example.ar1_linear1.AR1_linear_AG"); |
|
|
} |
|
|
|
|
|
public enum HandType |
|
|
{ |
|
|
Left, |
|
|
Right |
|
|
} |
|
|
|
|
|
public enum Axis |
|
|
{ |
|
|
x = -3, |
|
|
y = -2, |
|
|
z = -1, |
|
|
x_n, |
|
|
y_n, |
|
|
z_n |
|
|
} |
|
|
|
|
|
public enum CalibrationType |
|
|
{ |
|
|
AdductFin = -2, |
|
|
FistFin = -1, |
|
|
None, //δ<EFBFBD>궨 |
|
|
Fist, //<EFBFBD><EFBFBD>ȭ |
|
|
Adduct, //<EFBFBD><EFBFBD>£ |
|
|
Stretch, //<EFBFBD>ſ<EFBFBD> |
|
|
Completed //<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
} |
|
|
|
|
|
|
|
|
private float f4, f5, f6, f7, f8, f9, f10, f11, f12, f13, f14, f15, f16, f17, f18; |
|
|
|
|
|
private float a4 = 0, |
|
|
a5 = 0, |
|
|
a6 = 0, |
|
|
a7 = 0, |
|
|
a8 = 0, |
|
|
a9 = 0, |
|
|
a10 = 0, |
|
|
a11, |
|
|
a12 = 0, |
|
|
a13 = 0, |
|
|
a14 = 0, |
|
|
a15; |
|
|
|
|
|
private float n1 = 0, |
|
|
n2 = 0, |
|
|
n3 = 0, |
|
|
n4 = 0, |
|
|
n5 = 0, |
|
|
n6 = 0, |
|
|
n7 = 0, |
|
|
n8 = 0, |
|
|
n9 = 0, |
|
|
n10 = 0, |
|
|
n11 = 0, |
|
|
n12 = 0, |
|
|
n13 = 0, |
|
|
n14 = 0, |
|
|
n15 = 0; |
|
|
|
|
|
private float h4 = 0, |
|
|
h5 = 0, |
|
|
h6 = 0, |
|
|
h7 = 0, |
|
|
h8 = 0, |
|
|
h9 = 0, |
|
|
h10 = 0, |
|
|
h11 = 0, |
|
|
h12 = 0, |
|
|
h13 = 0, |
|
|
h14 = 0, |
|
|
h15 = 0, |
|
|
h16 = 0, |
|
|
h17 = 0; |
|
|
|
|
|
private float m4 = 0, |
|
|
m5 = 0, |
|
|
m6 = 0, |
|
|
m7 = 0, |
|
|
m8 = 0, |
|
|
m9 = 0, |
|
|
m10 = 0, |
|
|
m11 = 0, |
|
|
m12 = 0, |
|
|
m13 = 0, |
|
|
m14 = 0, |
|
|
m15 = 0; |
|
|
|
|
|
private float dot4 = 0, |
|
|
dot5 = 0, |
|
|
dot6 = 0, |
|
|
dot7 = 0, |
|
|
dot8 = 0, |
|
|
dot9 = 0, |
|
|
dot10 = 0, |
|
|
dot11 = 0, |
|
|
dot12 = 0, |
|
|
dot13 = 0, |
|
|
dot14 = 0, |
|
|
dot15 = 0; |
|
|
|
|
|
private Queue<float> f4Queue = new Queue<float>(); |
|
|
private Queue<float> f5Queue = new Queue<float>(); |
|
|
private Queue<float> f6Queue = new Queue<float>(); |
|
|
private Queue<float> f7Queue = new Queue<float>(); |
|
|
private Queue<float> f8Queue = new Queue<float>(); |
|
|
private Queue<float> f9Queue = new Queue<float>(); |
|
|
private Queue<float> f10Queue = new Queue<float>(); |
|
|
private Queue<float> f11Queue = new Queue<float>(); |
|
|
private Queue<float> f12Queue = new Queue<float>(); |
|
|
private Queue<float> f13Queue = new Queue<float>(); |
|
|
private Queue<float> f14Queue = new Queue<float>(); |
|
|
private Queue<float> f15Queue = new Queue<float>(); |
|
|
private const int QUEUE_MAX_SIZE = 10; |
|
|
|
|
|
public int HandDeviceIndex = 1; |
|
|
public HandType Hand; |
|
|
|
|
|
public Transform Wrist; |
|
|
|
|
|
[Header("<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵƫ<EFBFBD><EFBFBD>")] |
|
|
public Axis Pitch = Axis.z; |
|
|
|
|
|
public Axis Roll = Axis.x_n; |
|
|
|
|
|
public Axis Yaw = Axis.y_n; |
|
|
|
|
|
[Header("IMU<EFBFBD><EFBFBD><EFBFBD><EFBFBD>")] public bool HasIMU = false; |
|
|
|
|
|
[Header("Ĵָ<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ϵ<EFBFBD><EFBFBD>")][Range(0, 1)] public float coefficient = 0.6f; |
|
|
|
|
|
[Header("Ĵָ<EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>ƫ<EFBFBD><EFBFBD>")] public Vector3 Thumb1Offset; |
|
|
|
|
|
public string DeviceName; |
|
|
|
|
|
public float time; |
|
|
|
|
|
string line; |
|
|
|
|
|
private CalibrationType mCalibrationType = CalibrationType.None; |
|
|
|
|
|
private double[] pre_sensor_data; |
|
|
|
|
|
private List<double> angle_data = new(); |
|
|
private List<double> raw_data = new(); |
|
|
|
|
|
bool isCalibration = false; |
|
|
|
|
|
private float x_center_data = 1850; |
|
|
private float y_center_data = 1850; |
|
|
|
|
|
private float x_max_data = 3750; |
|
|
private float x_min_data = 620; |
|
|
private float y_min_data = 620; |
|
|
private float y_max_data = 3750; |
|
|
|
|
|
private bool centerCalib = false; |
|
|
private bool rangeCalib = false; |
|
|
|
|
|
private float dead_zone = 0.15f; |
|
|
|
|
|
private float trackpad_value = 0.3f; |
|
|
|
|
|
private float trigger_value = 0.5f; |
|
|
|
|
|
private float grab_value = 0.5f; |
|
|
|
|
|
private InputData input; |
|
|
|
|
|
private bool SaveCalibrationDataPermission = false; |
|
|
private bool ReadControllerSavedData = true; |
|
|
|
|
|
private int _curCalibTimes = 0; |
|
|
|
|
|
public void Method_update() |
|
|
{ |
|
|
if (time < 0) |
|
|
{ |
|
|
time = 5; |
|
|
} |
|
|
else |
|
|
{ |
|
|
time -= Time.deltaTime; |
|
|
} |
|
|
|
|
|
line = UDE_API.GetDeviceDataByName(DeviceName); |
|
|
if (string.IsNullOrEmpty(line)) |
|
|
{ |
|
|
return; |
|
|
} |
|
|
//Debug.Log("Line " + line); |
|
|
var data = line.Split(','); |
|
|
|
|
|
raw_data = new(); |
|
|
foreach (string str in data) |
|
|
{ |
|
|
raw_data.Add(Convert.ToDouble(str)); |
|
|
} |
|
|
|
|
|
f4 = Convert.ToSingle(data[0]); |
|
|
f5 = Convert.ToSingle(data[1]); |
|
|
f6 = Convert.ToSingle(data[2]); |
|
|
f7 = Convert.ToSingle(data[3]); |
|
|
f8 = Convert.ToSingle(data[4]); |
|
|
f9 = Convert.ToSingle(data[5]); |
|
|
f10 = Convert.ToSingle(data[6]); |
|
|
f11 = Convert.ToSingle(data[7]); |
|
|
f12 = Convert.ToSingle(data[8]); |
|
|
f13 = Convert.ToSingle(data[9]); |
|
|
f14 = Convert.ToSingle(data[10]); |
|
|
f15 = Convert.ToSingle(data[11]); |
|
|
if (data.Length > 12 && data.Length <= 16) |
|
|
{ |
|
|
f16 = Convert.ToSingle(data[12]); |
|
|
f17 = Convert.ToSingle(data[13]); |
|
|
f18 = Convert.ToSingle(data[14]); |
|
|
} |
|
|
else if (data.Length > 16) |
|
|
{ |
|
|
f16 = Convert.ToSingle(data[16]); |
|
|
f17 = Convert.ToSingle(data[17]); |
|
|
f18 = Convert.ToSingle(data[18]); |
|
|
} |
|
|
|
|
|
if (ReadControllerSavedData) |
|
|
{ |
|
|
ReadControllerSavedData = false; |
|
|
var x_temp = PlayerPrefs.GetFloat(DeviceName + "_X_Center"); |
|
|
if (x_temp != 0) |
|
|
{ |
|
|
x_center_data = x_temp; |
|
|
y_center_data = PlayerPrefs.GetFloat(DeviceName + "_Y_Center"); |
|
|
} |
|
|
|
|
|
var max_temp = PlayerPrefs.GetFloat(DeviceName + "_X_Max"); |
|
|
if (max_temp != 0) |
|
|
{ |
|
|
x_max_data = max_temp; |
|
|
x_min_data = PlayerPrefs.GetFloat(DeviceName + "_X_Min"); |
|
|
y_max_data = PlayerPrefs.GetFloat(DeviceName + "_Y_Max"); |
|
|
y_min_data = PlayerPrefs.GetFloat(DeviceName + "_Y_Min"); |
|
|
} |
|
|
} |
|
|
|
|
|
float f25 = (f16 - x_center_data) / ((x_max_data - x_min_data) / 2); |
|
|
float f26 = (f17 - y_center_data) / ((y_max_data - y_min_data) / 2); |
|
|
|
|
|
if (f25 > 1) |
|
|
{ |
|
|
f25 = 1; |
|
|
} |
|
|
|
|
|
if (f25 < -1) |
|
|
{ |
|
|
f25 = -1; |
|
|
} |
|
|
|
|
|
if (f26 > 1) |
|
|
{ |
|
|
f26 = 1; |
|
|
} |
|
|
|
|
|
if (f26 < -1) |
|
|
{ |
|
|
f26 = -1; |
|
|
} |
|
|
|
|
|
if (f25 <= dead_zone && f25 >= -dead_zone) |
|
|
{ |
|
|
f25 = 0; |
|
|
} |
|
|
|
|
|
if (f26 <= dead_zone && f26 >= -dead_zone) |
|
|
{ |
|
|
f26 = 0; |
|
|
} |
|
|
|
|
|
input = new InputData(); |
|
|
input.joyX = f25; |
|
|
input.joyY = f26; |
|
|
|
|
|
switch (f18) |
|
|
{ |
|
|
case 1: |
|
|
input.bButton = true; |
|
|
break; |
|
|
case 2: |
|
|
input.aButton = true; |
|
|
break; |
|
|
case 3: |
|
|
input.menu = true; |
|
|
break; |
|
|
case 4: |
|
|
input.joyButton = true; |
|
|
break; |
|
|
case 5: |
|
|
break; |
|
|
case 6: |
|
|
break; |
|
|
case 7: |
|
|
break; |
|
|
} |
|
|
|
|
|
if (centerCalib) |
|
|
{ |
|
|
x_center_data = f16; |
|
|
y_center_data = f17; |
|
|
centerCalib = false; |
|
|
|
|
|
PlayerPrefs.SetFloat(DeviceName + "_X_Center", f16); |
|
|
PlayerPrefs.SetFloat(DeviceName + "_Y_Center", f17); |
|
|
} |
|
|
|
|
|
if (rangeCalib) |
|
|
{ |
|
|
|
|
|
if (f16 > 600) |
|
|
{ |
|
|
x_min_data = Math.Min(x_min_data, f16); |
|
|
} |
|
|
|
|
|
if (f16 < 3800) |
|
|
{ |
|
|
x_max_data = Math.Max(x_max_data, f16); |
|
|
} |
|
|
|
|
|
|
|
|
if (f17 > 600) |
|
|
{ |
|
|
y_min_data = Math.Min(y_min_data, f17); |
|
|
} |
|
|
if (f17 < 3800) |
|
|
{ |
|
|
y_max_data = Math.Max(y_max_data, f17); |
|
|
} |
|
|
} |
|
|
|
|
|
if (mCalibrationType == CalibrationType.None && PlayerPrefs.GetInt(DeviceName) == 1) |
|
|
{ |
|
|
string[] CalibSavedData = PlayerPrefs.GetString(DeviceName + "_CalibDT").Split(","); |
|
|
int index = 0; |
|
|
|
|
|
n4 = float.Parse(CalibSavedData[index++]); |
|
|
n5 = float.Parse(CalibSavedData[index++]); |
|
|
n6 = float.Parse(CalibSavedData[index++]); |
|
|
n7 = float.Parse(CalibSavedData[index++]); |
|
|
n8 = float.Parse(CalibSavedData[index++]); |
|
|
n9 = float.Parse(CalibSavedData[index++]); |
|
|
n10 = float.Parse(CalibSavedData[index++]); |
|
|
n11 = float.Parse(CalibSavedData[index++]); |
|
|
n12 = float.Parse(CalibSavedData[index++]); |
|
|
n13 = float.Parse(CalibSavedData[index++]); |
|
|
n14 = float.Parse(CalibSavedData[index++]); |
|
|
n15 = float.Parse(CalibSavedData[index++]); |
|
|
|
|
|
h4 = float.Parse(CalibSavedData[index++]); |
|
|
h5 = float.Parse(CalibSavedData[index++]); |
|
|
h6 = float.Parse(CalibSavedData[index++]); |
|
|
h7 = float.Parse(CalibSavedData[index++]); |
|
|
h8 = float.Parse(CalibSavedData[index++]); |
|
|
h9 = float.Parse(CalibSavedData[index++]); |
|
|
h10 = float.Parse(CalibSavedData[index++]); |
|
|
h11 = float.Parse(CalibSavedData[index++]); |
|
|
h12 = float.Parse(CalibSavedData[index++]); |
|
|
h13 = float.Parse(CalibSavedData[index++]); |
|
|
h14 = float.Parse(CalibSavedData[index++]); |
|
|
h15 = float.Parse(CalibSavedData[index]); |
|
|
|
|
|
mCalibrationType = CalibrationType.Completed; |
|
|
} |
|
|
|
|
|
#region Dynamic Calibration |
|
|
//if (_curCalibTimes < 1000) |
|
|
//{ |
|
|
// if (m4 < f4) |
|
|
// { |
|
|
// m4 = f4; |
|
|
// if (m4 < h4) |
|
|
// { |
|
|
// m4 = h4; |
|
|
// } |
|
|
// } |
|
|
// if (m5 < f5) |
|
|
// { |
|
|
// m5 = f5; |
|
|
// if (m5 < h5) |
|
|
// { |
|
|
// m5 = h5; |
|
|
// } |
|
|
// } |
|
|
// if (m6 < f6) |
|
|
// { |
|
|
// m6 = f6; |
|
|
// if (m6 < h6) |
|
|
// { |
|
|
// m6 = h6; |
|
|
// } |
|
|
// } |
|
|
// if (m7 < f7) |
|
|
// { |
|
|
// m7 = f7; |
|
|
// if (m7 < h7) |
|
|
// { |
|
|
// m7 = h7; |
|
|
// } |
|
|
// } |
|
|
// if (m9 < f9) |
|
|
// { |
|
|
// m9 = f9; |
|
|
// if (m9 < h9) |
|
|
// { |
|
|
// m9 = h9; |
|
|
// } |
|
|
// } |
|
|
// if (m10 < f10) |
|
|
// { |
|
|
// m10 = f10; |
|
|
// if (m10 < h10) |
|
|
// { |
|
|
// m10 = h10; |
|
|
// } |
|
|
// } |
|
|
// if (m12 < f12) |
|
|
// { |
|
|
// m12 = f12; |
|
|
// if (m12 < h12) |
|
|
// { |
|
|
// m12 = h12; |
|
|
// } |
|
|
// } |
|
|
// if (m13 < f13) |
|
|
// { |
|
|
// m13 = f13; |
|
|
// if (m13 < h13) |
|
|
// { |
|
|
// m13 = h13; |
|
|
// } |
|
|
// } |
|
|
// if (m15 < f15) |
|
|
// { |
|
|
// m15 = f15; |
|
|
// if (m15 < h15) |
|
|
// { |
|
|
// m15 = h15; |
|
|
// } |
|
|
// } |
|
|
// _curCalibTimes++; |
|
|
|
|
|
//} |
|
|
//else |
|
|
//{ |
|
|
// //Debug.LogError("<EFBFBD><EFBFBD>̬<EFBFBD>궨<EFBFBD><EFBFBD><EFBFBD><EFBFBD>"); |
|
|
// //Debug.LogError($"{_deviceName}:{m4},{m5},{m6},{m7},{m8},{m9},{m10},{m11},{m12},{m13},{m14},{m15}"); |
|
|
// // f4<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m4 < f4) |
|
|
// { |
|
|
// UpdateQueue(f4Queue, f4); |
|
|
// if (f4Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f4Queue) < 10) |
|
|
// { |
|
|
// dot4 = f4 - m4; |
|
|
// m4 = f4; |
|
|
// n4 += dot4; |
|
|
// h4 += dot4; |
|
|
// } |
|
|
// } |
|
|
// // f5<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m5 < f5) |
|
|
// { |
|
|
// UpdateQueue(f5Queue, f5); |
|
|
// if (f5Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f5Queue) < 10) |
|
|
// { |
|
|
// dot5 = f5 - m5; |
|
|
// m5 = f5; |
|
|
// n5 += dot5; |
|
|
// h5 += dot5; |
|
|
// } |
|
|
// } |
|
|
// // f6<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m6 < f6) |
|
|
// { |
|
|
// UpdateQueue(f6Queue, f6); |
|
|
// if (f6Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f6Queue) < 10) |
|
|
// { |
|
|
// dot6 = f6 - m6; |
|
|
// m6 = f6; |
|
|
// n6 += dot6; |
|
|
// h6 += dot6; |
|
|
// } |
|
|
// } |
|
|
|
|
|
// // f7<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m7 < f7) |
|
|
// { |
|
|
// UpdateQueue(f7Queue, f7); |
|
|
// if (f7Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f7Queue) < 10) |
|
|
// { |
|
|
// dot7 = f7 - m7; |
|
|
// m7 = f7; |
|
|
// n7 += dot7; |
|
|
// h7 += dot7; |
|
|
// } |
|
|
// } |
|
|
// // f9<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m9 < f9) |
|
|
// { |
|
|
// UpdateQueue(f9Queue, f9); |
|
|
// if (f9Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f9Queue) < 10) |
|
|
// { |
|
|
// dot9 = f9 - m9; |
|
|
// m9 = f9; |
|
|
// n9 += dot9; |
|
|
// h9 += dot9; |
|
|
// } |
|
|
// } |
|
|
// // f10<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m10 < f10) |
|
|
// { |
|
|
// UpdateQueue(f10Queue, f10); |
|
|
// if (f10Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f10Queue) < 10) |
|
|
// { |
|
|
// dot10 = f10 - m10; |
|
|
// m10 = f10; |
|
|
// n10 += dot10; |
|
|
// h10 += dot10; |
|
|
// } |
|
|
// } |
|
|
// // f12<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m12 < f12) |
|
|
// { |
|
|
// UpdateQueue(f12Queue, f12); |
|
|
// if (f12Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f12Queue) < 10) |
|
|
// { |
|
|
// dot12 = f12 - m12; |
|
|
// m12 = f12; |
|
|
// n12 += dot12; |
|
|
// h12 += dot12; |
|
|
// } |
|
|
// } |
|
|
// // f13<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m13 < f13) |
|
|
// { |
|
|
// UpdateQueue(f13Queue, f13); |
|
|
// if (f13Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f13Queue) < 10) |
|
|
// { |
|
|
// dot13 = f13 - m13; |
|
|
// m13 = f13; |
|
|
// n13 += dot13; |
|
|
// h13 += dot13; |
|
|
// } |
|
|
// } |
|
|
// // f15<EFBFBD><EFBFBD><EFBFBD><EFBFBD> |
|
|
// if (m15 < f15) |
|
|
// { |
|
|
// UpdateQueue(f15Queue, f15); |
|
|
// if (f15Queue.Count == QUEUE_MAX_SIZE && CalculateVariance(f15Queue) < 10) |
|
|
// { |
|
|
// dot15 = f15 - m15; |
|
|
// m15 = f15; |
|
|
// n15 += dot15; |
|
|
// h15 += dot15; |
|
|
// } |
|
|
// } |
|
|
//} |
|
|
#endregion |
|
|
|
|
|
if (mCalibrationType == CalibrationType.Completed) |
|
|
{ |
|
|
if (SaveCalibrationDataPermission) |
|
|
{ |
|
|
//<EFBFBD>궨<EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD>α궨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¼һ<EFBFBD><EFBFBD> |
|
|
SaveCalibrationDataPermission = false; |
|
|
PlayerPrefs.SetInt(DeviceName, 1); |
|
|
|
|
|
string DataSet = n4 + "," + n5 + "," + n6 + "," + n7 + "," + n8 + "," + n9 + "," + n10 + "," + n11 + "," + n12 + "," + n13 + "," + n14 + "," + n15 + ","; |
|
|
DataSet += h4 + "," + h5 + "," + h6 + "," + h7 + "," + h8 + "," + h9 + "," + h10 + "," + h11 + "," + h12 + "," + h13 + "," + h14 + "," + h15; |
|
|
|
|
|
PlayerPrefs.SetString(DeviceName + "_CalibDT", DataSet); |
|
|
//Debug.Log("<EFBFBD>Ѽ<EFBFBD>¼<EFBFBD>±궨<EFBFBD><EFBFBD><EFBFBD><EFBFBD>"); |
|
|
} |
|
|
|
|
|
double[] org_f = new double[12] { f4, f5, f6, f7, f8, f9, f10, f11, f12, f13, f14, f15 }; |
|
|
double[] org_n = new double[12] { n4, n5, n6, n7, n8, n9, n10, n11, n12, n13, n14, n15 }; |
|
|
double[] org_h = new double[12] { h4, h5, h6, h7, h8, h9, h10, h11, h12, h13, h14, h15 }; |
|
|
float nn = 100; |
|
|
double[] org_a = Auto_Cali.AutoCali(org_f, org_n, org_h, nn); |
|
|
|
|
|
a4 = Convert.ToSingle(org_a[0]); |
|
|
a5 = Convert.ToSingle(org_a[1]); |
|
|
a6 = Convert.ToSingle(org_a[2]); |
|
|
a7 = Convert.ToSingle(org_a[3]); |
|
|
a8 = Convert.ToSingle(org_a[4]); |
|
|
a9 = Convert.ToSingle(org_a[5]); |
|
|
a10 = Convert.ToSingle(org_a[6]); |
|
|
a11 = Convert.ToSingle(org_a[7]); |
|
|
a12 = Convert.ToSingle(org_a[8]); |
|
|
a13 = Convert.ToSingle(org_a[9]); |
|
|
a14 = Convert.ToSingle(org_a[10]); |
|
|
a15 = Convert.ToSingle(org_a[11]); |
|
|
|
|
|
double[][] Y = new double[12][]; |
|
|
for (int i = 0; i < 12; i++) |
|
|
{ |
|
|
Y[i] = new double[1]; |
|
|
} |
|
|
|
|
|
Y[0][0] = Convert.ToDouble(a4); |
|
|
Y[1][0] = Convert.ToDouble(a5); |
|
|
Y[2][0] = Convert.ToDouble(a6); |
|
|
Y[3][0] = Convert.ToDouble(a7); |
|
|
Y[4][0] = Convert.ToDouble(a8); |
|
|
Y[5][0] = Convert.ToDouble(a9); |
|
|
Y[6][0] = Convert.ToDouble(a10); |
|
|
Y[7][0] = Convert.ToDouble(a11); |
|
|
Y[8][0] = Convert.ToDouble(a12); |
|
|
Y[9][0] = Convert.ToDouble(a13); |
|
|
Y[10][0] = Convert.ToDouble(a14); |
|
|
Y[11][0] = Convert.ToDouble(a15); |
|
|
double[] b = new double[12]; |
|
|
b[0] = Convert.ToDouble(a4); |
|
|
b[1] = Convert.ToDouble(a5); |
|
|
b[2] = Convert.ToDouble(a6); |
|
|
b[3] = Convert.ToDouble(a7); |
|
|
b[4] = Convert.ToDouble(a8); |
|
|
b[5] = Convert.ToDouble(a9); |
|
|
b[6] = Convert.ToDouble(a10); |
|
|
b[7] = Convert.ToDouble(a11); |
|
|
b[8] = Convert.ToDouble(a12); |
|
|
b[9] = Convert.ToDouble(a13); |
|
|
b[10] = Convert.ToDouble(a14); |
|
|
b[11] = Convert.ToDouble(a15); |
|
|
if (pre_sensor_data == null) |
|
|
{ |
|
|
pre_sensor_data = new double[12]; |
|
|
pre_sensor_data[0] = Convert.ToDouble(a4); |
|
|
pre_sensor_data[1] = Convert.ToDouble(a5); |
|
|
pre_sensor_data[2] = Convert.ToDouble(a6); |
|
|
pre_sensor_data[3] = Convert.ToDouble(a7); |
|
|
pre_sensor_data[4] = Convert.ToDouble(a8); |
|
|
pre_sensor_data[5] = Convert.ToDouble(a9); |
|
|
pre_sensor_data[6] = Convert.ToDouble(a10); |
|
|
pre_sensor_data[7] = Convert.ToDouble(a11); |
|
|
pre_sensor_data[8] = Convert.ToDouble(a12); |
|
|
pre_sensor_data[9] = Convert.ToDouble(a13); |
|
|
pre_sensor_data[10] = Convert.ToDouble(a14); |
|
|
pre_sensor_data[11] = Convert.ToDouble(a15); |
|
|
return; |
|
|
} |
|
|
|
|
|
double[] result = new double[26]; |
|
|
|
|
|
if (string.IsNullOrEmpty(DeviceName)) |
|
|
{ |
|
|
result = AR1_linear_AE.CallStatic<double[]>("Senser2Angle", b, pre_sensor_data); |
|
|
Version = AR1_linear_AE.GetStatic<string>("Version"); |
|
|
} |
|
|
else if (DeviceName[5] == 'A') |
|
|
{ |
|
|
result = AR1_linear_AA.CallStatic<double[]>("Senser2Angle", b, pre_sensor_data); |
|
|
Version = AR1_linear_AA.GetStatic<string>("Version"); |
|
|
} |
|
|
else if (DeviceName[5] == 'B') |
|
|
{ |
|
|
result = AR1_linear_AD.CallStatic<double[]>("Senser2Angle", b, pre_sensor_data); |
|
|
Version = AR1_linear_AD.GetStatic<string>("Version"); |
|
|
} |
|
|
else if (DeviceName[5] == 'C' || DeviceName[5] == 'D') |
|
|
{ |
|
|
result = AR1_linear_AE.CallStatic<double[]>("Senser2Angle", b, pre_sensor_data); |
|
|
Version = AR1_linear_AE.GetStatic<string>("Version"); |
|
|
} |
|
|
else if (DeviceName[5] == 'E' || DeviceName[5] == 'F') |
|
|
{ |
|
|
result = AR1_linear_AG.CallStatic<double[]>("Senser2Angle", b, pre_sensor_data); |
|
|
Version = AR1_linear_AG.GetStatic<string>("Version"); |
|
|
} |
|
|
|
|
|
if (DeviceName.Contains('L')) |
|
|
{ |
|
|
string str = string.Empty; |
|
|
for (int i = 0; i < result.Length; i++) |
|
|
{ |
|
|
str += result[i] + ","; |
|
|
} |
|
|
Debug.LogError("Left Res: " + str); |
|
|
} |
|
|
Array.Copy(b, pre_sensor_data, pre_sensor_data.Length); |
|
|
angle_data = new List<double>(result); |
|
|
//Debug.Log("Debug " + angle_data.ToArray().ToString()); |
|
|
|
|
|
if (Math.Abs(result[5]) >= trigger_value * 100) |
|
|
{ |
|
|
input.trgButton = true; |
|
|
|
|
|
input.trgValue = (float)((Math.Abs(result[5]) - trigger_value * 100) / 65); |
|
|
} |
|
|
|
|
|
if (Math.Abs(result[9]) >= grab_value * 100 && Math.Abs(result[13]) >= grab_value * 100) |
|
|
input.grab = true; |
|
|
|
|
|
if (Math.Abs(result[0]) >= trackpad_value * 60) |
|
|
{ |
|
|
input.trackpad_touch = true; |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
private float CalculateVariance(Queue<float> queue) |
|
|
{ |
|
|
if (queue.Count == 0) return 0; |
|
|
float mean = queue.Average(); |
|
|
return queue.Sum(x => (x - mean) * (x - mean)) / queue.Count; |
|
|
} |
|
|
|
|
|
void UpdateQueue(Queue<float> queue, float value) |
|
|
{ |
|
|
if (queue.Count >= QUEUE_MAX_SIZE) |
|
|
{ |
|
|
queue.Dequeue(); |
|
|
} |
|
|
queue.Enqueue(value); |
|
|
} |
|
|
|
|
|
public bool StartCalibration(CalibrationType calibrationType) |
|
|
{ |
|
|
if (isCalibration) |
|
|
{ |
|
|
return false; |
|
|
} |
|
|
isCalibration = true; |
|
|
mCalibrationType = calibrationType; |
|
|
return true; |
|
|
} |
|
|
|
|
|
public bool StopCalibration(CalibrationType calibrationType) |
|
|
{ |
|
|
|
|
|
//line = UDE_API.GetDeviceDataByName(DeviceName); |
|
|
if (string.IsNullOrEmpty(line)) |
|
|
{ |
|
|
return false; |
|
|
} |
|
|
if (!isCalibration) |
|
|
{ |
|
|
return false; |
|
|
} |
|
|
//Debug.Log(line); |
|
|
//var data = line.Split(','); |
|
|
|
|
|
//f4 = Convert.ToSingle(data[0]); |
|
|
//f5 = Convert.ToSingle(data[1]); |
|
|
//f6 = Convert.ToSingle(data[2]); |
|
|
//f7 = Convert.ToSingle(data[3]); |
|
|
//f8 = Convert.ToSingle(data[4]); |
|
|
//f9 = Convert.ToSingle(data[5]); |
|
|
//f10 = Convert.ToSingle(data[6]); |
|
|
//f11 = Convert.ToSingle(data[7]); |
|
|
//f12 = Convert.ToSingle(data[8]); |
|
|
//f13 = Convert.ToSingle(data[9]); |
|
|
//f14 = Convert.ToSingle(data[10]); |
|
|
//f15 = Convert.ToSingle(data[11]); |
|
|
//if (data.Length > 12 && data.Length <= 16) |
|
|
//{ |
|
|
// f16 = Convert.ToSingle(data[12]); |
|
|
// f17 = Convert.ToSingle(data[13]); |
|
|
// f18 = Convert.ToSingle(data[14]); |
|
|
//} |
|
|
//else if (data.Length > 16) |
|
|
//{ |
|
|
// f16 = Convert.ToSingle(data[16]); |
|
|
// f17 = Convert.ToSingle(data[17]); |
|
|
// f18 = Convert.ToSingle(data[18]); |
|
|
//} |
|
|
|
|
|
switch (calibrationType) |
|
|
{ |
|
|
case CalibrationType.Fist: |
|
|
h4 = f4; |
|
|
h5 = f5; |
|
|
h6 = f6; |
|
|
h7 = f7; |
|
|
|
|
|
h9 = f9; |
|
|
h10 = f10; |
|
|
h12 = f12; |
|
|
h13 = f13; |
|
|
h15 = f15; |
|
|
isCalibration = false; |
|
|
mCalibrationType = CalibrationType.FistFin; |
|
|
SaveCalibrationDataPermission = true; |
|
|
break; |
|
|
case CalibrationType.Adduct: |
|
|
|
|
|
n4 = f4; |
|
|
n5 = f5; |
|
|
n6 = f6; |
|
|
n7 = f7; |
|
|
n8 = f8; |
|
|
n9 = f9; |
|
|
n10 = f10; |
|
|
n11 = f11; |
|
|
n12 = f12; |
|
|
n13 = f13; |
|
|
n14 = f14; |
|
|
n15 = f15; |
|
|
isCalibration = false; |
|
|
mCalibrationType = CalibrationType.AdductFin; |
|
|
|
|
|
break; |
|
|
case CalibrationType.Stretch: |
|
|
|
|
|
h8 = f8; |
|
|
h11 = f11; |
|
|
h14 = f14; |
|
|
isCalibration = false; |
|
|
mCalibrationType = CalibrationType.Completed; |
|
|
break; |
|
|
} |
|
|
return true; |
|
|
} |
|
|
|
|
|
public CalibrationType GetCalibrationType() |
|
|
{ |
|
|
return mCalibrationType; |
|
|
} |
|
|
|
|
|
public List<double> GetAngleData() |
|
|
{ |
|
|
return angle_data; |
|
|
} |
|
|
|
|
|
public List<double> GetRawData() |
|
|
{ |
|
|
return raw_data; |
|
|
} |
|
|
|
|
|
public void CalibrationCenterData() |
|
|
{ |
|
|
centerCalib = true; |
|
|
ReadControllerSavedData = true; |
|
|
} |
|
|
|
|
|
public void StartCalibrationRangeData() |
|
|
{ |
|
|
rangeCalib = true; |
|
|
} |
|
|
|
|
|
public void StopCalibrationRangeData() |
|
|
{ |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_X_MAX", x_max_data); |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_X_MIN", x_min_data); |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_Y_MAX", y_max_data); |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_Y_MIN", y_min_data); |
|
|
ReadControllerSavedData = true; |
|
|
rangeCalib = false; |
|
|
} |
|
|
|
|
|
public InputData GetInputData() |
|
|
{ |
|
|
return input; |
|
|
} |
|
|
|
|
|
public void SetDeadZone(float dead_zone) |
|
|
{ |
|
|
this.dead_zone = dead_zone; |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_DeadZone", dead_zone); |
|
|
} |
|
|
|
|
|
public float GetDeadZone() |
|
|
{ |
|
|
return dead_zone; |
|
|
} |
|
|
|
|
|
public void SetTriggerValue(float trigger_value) |
|
|
{ |
|
|
this.trigger_value = trigger_value; |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_TriggerValue", trigger_value); |
|
|
} |
|
|
|
|
|
public float GetTriggerValue() |
|
|
{ |
|
|
return trigger_value; |
|
|
} |
|
|
|
|
|
public void SetGrabValue(float grab_value) |
|
|
{ |
|
|
this.grab_value = grab_value; |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_GrabValue", grab_value); |
|
|
} |
|
|
|
|
|
public float GetGrabValue() |
|
|
{ |
|
|
return grab_value; |
|
|
} |
|
|
|
|
|
public void SetTrackpadValue(float trackpad_value) |
|
|
{ |
|
|
this.trackpad_value = trackpad_value; |
|
|
PlayerPrefs.SetFloat($"{DeviceName}_TrackpadValue", trackpad_value); |
|
|
} |
|
|
|
|
|
public float GetTrackpadValue() |
|
|
{ |
|
|
return trackpad_value; |
|
|
} |
|
|
|
|
|
public double[] GetCalibrationInfoOffset() |
|
|
{ |
|
|
double[] calibration_info = new double[12]; |
|
|
double[] org_n = new double[12] { n4, n5, n6, n7, n8, n9, n10, n11, n12, n13, n14, n15 }; |
|
|
double[] org_h = new double[12] { h4, h5, h6, h7, h8, h9, h10, h11, h12, h13, h14, h15 }; |
|
|
for (int i = 0; i < 12; i++) |
|
|
{ |
|
|
calibration_info[i] = Mathf.Abs((float)(org_n[i] - org_h[i])); |
|
|
} |
|
|
|
|
|
return calibration_info; |
|
|
} |
|
|
|
|
|
public string GetAlgorithmVerName() |
|
|
{ |
|
|
return Version; |
|
|
} |
|
|
|
|
|
} |
