static void Main(string[] args)
{
ZenClientHandle_t zenHandle = new ZenClientHandle_t();
OpenZen.ZenInit(zenHandle);
ExThread obj = new ExThread(zenHandle);
// Creating thread
// Using thread class
Thread thr = new Thread(new ThreadStart(obj.sensorEventThread));
thr.Start();
// start sensor listing, new sensors will be reported as Events in our event thread
OpenZen.ZenListSensorsAsync(zenHandle);
while (!obj.mSearchDone)
{
Console.WriteLine("Searching for sensors ...");
Thread.Sleep(1000);
}
if (obj.mFoundSensors.Count == 0)
{
Console.WriteLine("No sensor found on the system");
OpenZen.ZenShutdown(zenHandle);
thr.Abort();
return;
}
ZenSensorInitError sensorInitError = ZenSensorInitError.ZenSensorInitError_Max;
// try three connection attempts
for (int i = 0; i < 3; i++)
{
ZenSensorHandle_t sensorHandle = new ZenSensorHandle_t();
sensorInitError = OpenZen.ZenObtainSensor(zenHandle, obj.mFoundSensors[0], sensorHandle);
Console.WriteLine("Connecting to sensor " + obj.mFoundSensors[0].identifier + " on IO interface " + obj.mFoundSensors[0].ioType);
if (sensorInitError == ZenSensorInitError.ZenSensorInitError_None)
{
Console.WriteLine("Succesfully connected to sensor");
break;
}
}
if (sensorInitError != ZenSensorInitError.ZenSensorInitError_None)
{
Console.WriteLine("Could not connect to sensor");
System.Environment.Exit(1);
}
// stream some data
Thread.Sleep(5000);
OpenZen.ZenShutdown(zenHandle);
thr.Abort();
}
void OnDestroy()
{
if (mSensorHandle != null)
{
OpenZen.ZenReleaseSensor(mZenHandle, mSensorHandle);
}
OpenZen.ZenShutdown(mZenHandle);
// connection.Stop();
}
private void disconnect_Device4_Click(object sender, EventArgs e)
{
try
{
abort_Thread(4);
disconnect_Device4.Visible = false;
connect_Device4.Enabled = true;
device_List4.Enabled = true;
OpenZen.ZenReleaseSensor(device4_Handle, device4_Sensor);
}
catch (Exception)
{
MessageBox.Show("An Error Occured!");
}
}
// GUI
void OnGUI()
{
GUIStyle myButtonStyle = new GUIStyle(GUI.skin.button);
myButtonStyle.fontSize = 13;
GUIStyle labelDetails = new GUIStyle(GUI.skin.GetStyle("label"));
labelDetails.fontSize = 14;
labelDetails.normal.textColor = Color.black;
if (GUI.Button(new Rect(Screen.width - 170, 60, 150, 30), "IMU29 Heading Reset", myButtonStyle))
{
print("Performing Heading Reset for IMU29");
ZenComponentHandle_t mComponent = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle, OpenZen.g_zenSensorType_Imu, 0, mComponent);
// perform heading reset
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
}
if (GUI.Button(new Rect(Screen.width - 170, 100, 150, 30), "IMU98 Heading Reset", myButtonStyle))
{
print("Performing Heading Reset for IMU98");
ZenComponentHandle_t mComponent2 = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle2, OpenZen.g_zenSensorType_Imu, 0, mComponent2);
// perform heading reset
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
}
if (GUI.Button(new Rect(Screen.width - 170, 140, 150, 30), "IMUusb Heading Reset", myButtonStyle))
{
print("Performing Heading Reset for IMUusb");
ZenComponentHandle_t mComponent3 = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle3, OpenZen.g_zenSensorType_Imu, 0, mComponent3);
// perform heading reset
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
}
//if (GUI.Button(new Rect(20, 20, 150, 30), "Calibrate Mousepad", myButtonStyle))
//{
// print("Record palm position");
//
//}
GUI.Label(new Rect(20, 20, 150, 50),
"Timer: " + visualTime.ToString("f4") + " seconds.", labelDetails);
}
public static void main()
{
// Start of Code
ZenClientHandle_t zenHandle = new ZenClientHandle_t();
OpenZen.ZenInit(zenHandle);
ExThread obj = new ExThread(zenHandle);
// Creating thread
// Using thread class
thr = new Thread(new ThreadStart(obj.sensorEventThread));
thr.Start();
// start sensor listing, new sensors will be reported as Events in our event thread
OpenZen.ZenListSensorsAsync(zenHandle);
while (!obj.mSearchDone)
{
Console.WriteLine("Searching for sensors ...");
Thread.Sleep(1000);
}
ZenSensorInitError sensorInitError = ZenSensorInitError.ZenSensorInitError_Max;
// try three connection attempts
for (int i = 0; i < 10; i++)
{
ZenSensorHandle_t sensorHandle = new ZenSensorHandle_t();
sensorInitError = OpenZen.ZenObtainSensor(zenHandle, obj.mFoundSensors[0], sensorHandle);
if (sensorInitError == ZenSensorInitError.ZenSensorInitError_None)
{
Console.WriteLine("Succesfully connected to sensor");
break;
}
}
if (sensorInitError != ZenSensorInitError.ZenSensorInitError_None)
{
Console.WriteLine("Could not connect to sensor");
System.Environment.Exit(1);
}
}
// Non-static method
public void sensorEventThread()
{
try
{
while (true)
{
ZenEvent zenEvent = new ZenEvent();
//Console.WriteLine(mZenHandle);
if (OpenZen.ZenWaitForNextEvent(mZenHandle, zenEvent))
{
if (zenEvent.component.handle == 0)
{
// if the handle is on, its not a sensor event but a system wide
// event
switch (zenEvent.eventType)
{
case (int)ZenSensorEvent.ZenSensorEvent_SensorFound:
Console.WriteLine("found sensor event event " + zenEvent.data.sensorFound.name);
mFoundSensors.Add(zenEvent.data.sensorFound);
break;
case (int)ZenSensorEvent.ZenSensorEvent_SensorListingProgress:
if (zenEvent.data.sensorListingProgress.progress == 1.0)
{
mSearchDone = true;
}
break;
}
}
else
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
mImuEventCount++;
if (mImuEventCount % 100 == 0)
{
continue;
}
// read acceleration
OpenZenFloatArray fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.a);
// read angular velocity
OpenZenFloatArray fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.g);
// read euler angles
OpenZenFloatArray fr = OpenZenFloatArray.frompointer(zenEvent.data.imuData.r);
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
Console.WriteLine("Sensor data\n -> Acceleration a = " + fa.getitem(0) + " " + fa.getitem(1) + " " + fa.getitem(2));
Console.WriteLine(" -> Angular velocity g = " + fg.getitem(0) + " " + fg.getitem(1) + " " + fg.getitem(2));
Console.WriteLine(" -> Euler angles r = " + fr.getitem(0) + " " + +fr.getitem(1) + " " + fr.getitem(2));
Console.WriteLine(" -> Quaternion w = " + fq.getitem(0) + " x " + +fq.getitem(1) + " y " + +fq.getitem(2) + " z " + +fq.getitem(3));
break;
}
}
}
}
}
catch (ThreadAbortException)
{
}
}
private void retrieve_Port_Click(object sender, EventArgs e)
{
foundSensors.Clear();
device_List1.Items.Clear();
device_List2.Items.Clear();
device_List3.Items.Clear();
device_List4.Items.Clear();
OpenZen.ZenInit(zenHandle);
ZenEvent zenEvent = new ZenEvent();
Boolean searchDone = false;
// start sensor listing, new sensors will be reported as Events in our event thread
OpenZen.ZenListSensorsAsync(zenHandle);
// Searching for device
while (searchDone == false)
{
if (OpenZen.ZenWaitForNextEvent(zenHandle, zenEvent) && zenEvent.component.handle == 0)
{
// Handle Event
switch (zenEvent.eventType)
{
case (int)ZenSensorEvent.ZenSensorEvent_SensorFound:
//MessageBox.Show("Device : " + zenEvent.data.sensorFound.name);
ZenSensorDesc sensor = new ZenSensorDesc();
sensor.identifier = zenEvent.data.sensorFound.identifier;
sensor.baudRate = zenEvent.data.sensorFound.baudRate;
sensor.ioType = zenEvent.data.sensorFound.ioType;
sensor.name = zenEvent.data.sensorFound.name;
sensor.serialNumber = zenEvent.data.sensorFound.serialNumber;
foundSensors.Add(sensor);
break;
case (int)ZenSensorEvent.ZenSensorEvent_SensorListingProgress:
if (zenEvent.data.sensorListingProgress.progress == 1.0)
{
searchDone = true;
}
break;
}
}
}
// Listing Connected Device
if (foundSensors.Count > 0)
{
for (int i = 0; i < foundSensors.Count; i++)
{
device_List1.Items.Add(foundSensors[i].name);
device_List2.Items.Add(foundSensors[i].name);
device_List3.Items.Add(foundSensors[i].name);
device_List4.Items.Add(foundSensors[i].name);
}
device_List1.SelectedIndex = 0;
device_List2.SelectedIndex = 0;
device_List3.SelectedIndex = 0;
device_List4.SelectedIndex = 0;
}
else
{
MessageBox.Show("No Sensor Device is found");
}
}
// Non-static method
public void sensorEventThread()
{
try
{
OpenZen.ZenInit(device_Handle);
ZenEvent zenEvent = new ZenEvent();
Boolean connected = false;
ZenSensorInitError sensorInitError = ZenSensorInitError.ZenSensorInitError_Max;
// try three connection attempts
for (int i = 0; i < 1; i++)
{
sensorInitError = OpenZen.ZenObtainSensor(device_Handle, mSensor, device_Sensor);
if (sensorInitError == ZenSensorInitError.ZenSensorInitError_None)
{
connected = true;
// Enable Disconnect Button
MainForm mainForm = FormProvider.getMain();
switch (device_No)
{
case 1:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List1.Enabled = false;
mainForm.disconnect_Device1.Visible = true;
mainForm.connect_Device1.Enabled = false;
});
}
else
{
mainForm.device_List1.Enabled = false;
mainForm.disconnect_Device1.Visible = true;
mainForm.connect_Device1.Enabled = false;
}
break;
case 2:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List2.Enabled = false;
mainForm.disconnect_Device2.Visible = true;
mainForm.connect_Device2.Enabled = false;
});
}
else
{
mainForm.device_List2.Enabled = false;
mainForm.disconnect_Device2.Visible = true;
mainForm.connect_Device2.Enabled = false;
}
break;
case 3:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List3.Enabled = false;
mainForm.disconnect_Device3.Visible = true;
mainForm.connect_Device3.Enabled = false;
});
}
else
{
mainForm.device_List3.Enabled = false;
mainForm.disconnect_Device3.Visible = true;
mainForm.connect_Device3.Enabled = false;
}
break;
case 4:
if (mainForm.InvokeRequired)
{
mainForm.BeginInvoke((Action) delegate() {
mainForm.device_List4.Enabled = false;
mainForm.disconnect_Device4.Visible = true;
mainForm.connect_Device4.Enabled = false;
});
}
else
{
mainForm.device_List4.Enabled = false;
mainForm.disconnect_Device4.Visible = true;
mainForm.connect_Device4.Enabled = false;
}
break;
}
MessageBox.Show("Succesfully connected to sensor for Device #" + device_No.ToString());
break;
}
else
{
MessageBox.Show("Failed");
}
}
while (connected)
{
//Console.WriteLine(mZenHandle);
if (OpenZen.ZenWaitForNextEvent(device_Handle, zenEvent))
{
if (zenEvent.component.handle != 0)
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
count++;
// read raw accelerometer
OpenZenFloatArray raw_fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.aRaw);
// read calibrated accelerometer
OpenZenFloatArray fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.a);
// read raw accelerometer
OpenZenFloatArray raw_fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.gRaw);
// read calibrated gyroscope
OpenZenFloatArray fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.g);
// read raw magnetometer
OpenZenFloatArray raw_fb = OpenZenFloatArray.frompointer(zenEvent.data.imuData.bRaw);
// read calibrated magnetometer
OpenZenFloatArray fb = OpenZenFloatArray.frompointer(zenEvent.data.imuData.b);
// read euler angle
OpenZenFloatArray fr = OpenZenFloatArray.frompointer(zenEvent.data.imuData.r);
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
// Create new imuData object
IMUData imuData = new IMUData(
raw_fa.getitem(0), raw_fa.getitem(1), raw_fa.getitem(2),
fa.getitem(0), fa.getitem(1), fa.getitem(2),
raw_fg.getitem(0), raw_fg.getitem(1), raw_fg.getitem(2),
fg.getitem(0), fg.getitem(1), fg.getitem(2),
raw_fb.getitem(0), raw_fb.getitem(1), raw_fb.getitem(2),
fb.getitem(0), fb.getitem(1), fb.getitem(2),
fr.getitem(0), fr.getitem(1), fr.getitem(2),
fq.getitem(0), fq.getitem(1), fq.getitem(2)
); // Push data in to the new imuData Object
switch (device_No)
{
case (1):
transferDevice1_Data(imuData);
//Debug.WriteLine(count);
break;
case (2):
transferDevice2_Data(imuData);
break;
case (3):
transferDevice3_Data(imuData);
break;
case (4):
transferDevice4_Data(imuData);
Debug.WriteLine(count);
break;
}
break;
}
}
}
}
}
catch (ThreadAbortException)
{
}
}
// Start is called before the first frame update
void Start()
{
// connection = new UdpConnection();
// connection.StartConnection(sendIp, sendPort, receivePort);
Hand_demo.SetActive(!Hand_demo.activeSelf);
initializeJoints();
// create OpenZen
OpenZen.ZenInit(mZenHandle);
// Hint: to get the io type and identifer for all connected sensor,
// you cant start the DiscoverSensorScene. The information of all
// found sensors is printed in the debug console of Unity after
// the search is complete.
// print("Trying to connect to OpenZen Sensor on IO " + OpenZenIoType +
// " with sensor name " + OpenZenIdentifier);
var sensorInitError = OpenZen.ZenObtainSensorByName(mZenHandle,
OpenZenIoType,
OpenZenIdentifier,
0,
mSensorHandle);
if (sensorInitError != ZenSensorInitError.ZenSensorInitError_None)
{
print("Error while connecting to sensor 29.");
}
else
{
ZenComponentHandle_t mComponent = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle, OpenZen.g_zenSensorType_Imu, 0, mComponent);
// enable sensor streaming, normally on by default anyways
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_StreamData, true);
// set offset mode to heading
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
// set the sampling rate to 100 Hz
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_SamplingRate, 100);
// filter mode using accelerometer & gyroscope & magnetometer
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_FilterMode, 1);
// Ensure the Orientation data is streamed out
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle, mComponent,
(int)EZenImuProperty.ZenImuProperty_OutputQuat, true);
print("Sensor 29 configuration complete");
}
// print("Trying to connect to OpenZen Sensor on IO " + OpenZenIoType +
// " with sensor name " + OpenZenIdentifier2);
var sensorInitError2 = OpenZen.ZenObtainSensorByName(mZenHandle,
OpenZenIoType,
OpenZenIdentifier2,
0,
mSensorHandle2);
if (sensorInitError2 != ZenSensorInitError.ZenSensorInitError_None)
{
print("Error while connecting to sensor 98.");
}
else
{
ZenComponentHandle_t mComponent2 = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle2, OpenZen.g_zenSensorType_Imu, 0, mComponent2);
// enable sensor streaming, normally on by default anyways
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_StreamData, true);
// set offset mode to heading
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
// set the sampling rate to 100 Hz
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_SamplingRate, 100);
// filter mode using accelerometer & gyroscope & magnetometer
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_FilterMode, 1);
// Ensure the Orientation data is streamed out
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle2, mComponent2,
(int)EZenImuProperty.ZenImuProperty_OutputQuat, true);
print("Sensor 98 configuration complete");
}
var sensorInitError3 = OpenZen.ZenObtainSensorByName(mZenHandle,
OpenZenIoType2.ToString(),
OpenZenIdentifier3,
0,
mSensorHandle3);
if (sensorInitError3 != ZenSensorInitError.ZenSensorInitError_None)
{
print("Error while connecting to sensor USB.");
}
else
{
ZenComponentHandle_t mComponent3 = new ZenComponentHandle_t();
OpenZen.ZenSensorComponentsByNumber(mZenHandle, mSensorHandle3, OpenZen.g_zenSensorType_Imu, 0, mComponent3);
// enable sensor streaming, normally on by default anyways
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_StreamData, true);
// set offset mode to heading
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_OrientationOffsetMode, 1);
// set the sampling rate to 100 Hz
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_SamplingRate, 100);
// filter mode using accelerometer & gyroscope & magnetometer
OpenZen.ZenSensorComponentSetInt32Property(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_FilterMode, 1);
// Ensure the Orientation data is streamed out
OpenZen.ZenSensorComponentSetBoolProperty(mZenHandle, mSensorHandle3, mComponent3,
(int)EZenImuProperty.ZenImuProperty_OutputQuat, true);
print("Sensor USB configuration complete");
}
// Gripper calibration here:
Debug.Log("Begin Gripper Calibration");
}
// Update is called once per frame
void Update()
{
// assign q and x0 to be hand's orentation and position
q = Hand_demo.transform.rotation;
x0 = Hand_demo.transform.position;
Debug.DrawRay(x0, q * Vector3.up, Color.green);
hand_orientation = q * Vector3.up;
currentObjects = Counter.currentWires;
// check if there's collision with a wire
if (currentObjects != null)
{
if (currentObjects.Count != 0)
{
float dist_closest = Mathf.Infinity;
Vector3 hc_orientation = Vector3.zero;
Vector3 orientation_closest = Vector3.zero;
float angle_closest = Mathf.Infinity;
float angle = angle_closest;
foreach (var currentObject in currentObjects)
{
// Debug.Log(currentObject.name);
if (currentObject.name.Contains("curve"))
{
if (currentObject.name.Contains("curve1"))
{
Vector3 s_offset = new Vector3(0.0125f + 0.05f, 0f, 0.0125f);
Vector3 center = currentObject.transform.position + s_offset;
dist = distanceTocenter(x0, center, radius_halfcircle);
hc_orientation = x0 - center;
Debug.DrawRay(center, hc_orientation, Color.red);
angle = Vector3.Angle(hc_orientation, hand_orientation);
angle -= 90;
angle = Mathf.Abs(angle);
// Debug.Log(angle);
}
else if (currentObject.name.Contains("curve2"))
{
Vector3 s2_offset = new Vector3(-0.0125f - 0.05f, 0f, 0.0125f);
Vector3 center = currentObject.transform.position + s2_offset;
dist = distanceTocenter(x0, center, radius_halfcircle);
hc_orientation = x0 - center;
Debug.DrawRay(center, hc_orientation, Color.red);
angle = Vector3.Angle(hc_orientation, hand_orientation);
angle -= 90;
angle = Mathf.Abs(angle);
// Debug.Log(angle);
}
}
else
{
Vector3 wire_orientation = currentObject.transform.rotation * Vector3.up;
Debug.DrawRay(currentObject.transform.position, wire_orientation, Color.red);
x1 = currentObject.transform.position;
s = wire_orientation;
dist = distanceToclosestPoint(x0, x1, s);
angle = Vector3.Angle(wire_orientation, hand_orientation);
}
if (dist < dist_closest)
{
dist_closest = dist;
angle_closest = angle;
}
}
if (Counter.recordData)
{
SaveFile(filepath, dist_closest, angle_closest);
}
// Debug.Log("Returned cloeset distance:");
// Debug.Log(dist_closest);
}
}
// generate a new data file that has incremental index
if (Input.GetKeyDown(KeyCode.N))
{
Debug.Log("Generating new data file...");
string[] file_array = filepath_origin.Split('.');
filepath = file_array[0] + fileCounter.ToString() + '.' + file_array[1];
Debug.Log(filepath);
fileCounter++;
}
// toggle visibility
if (Input.GetKeyDown(KeyCode.V))
{
ToggleVisibility();
}
// switch tasks
if (Input.GetKeyDown(KeyCode.C))
{
SwitchTasks();
}
// using IMU input
if (Input.GetKeyDown(KeyCode.I))
{
print("Now using IMU input");
isIMU = true;
isUDP = false;
}
// using UDP input
if (Input.GetKeyDown(KeyCode.U))
{
print("Now using udp input");
isIMU = false;
isUDP = true;
}
// check if current data is for a new trial, must hit green button to initiate new try
if (Counter.isNewTry)
{
using (System.IO.StreamWriter file = new System.IO.StreamWriter(filepath, true))
{
file.WriteLine("New Try");
}
Counter.isNewTry = false;
}
if (timerclass.timeStart)
{
timer += Time.deltaTime;
visualTime = timer;
}
if (timerclass.timeReset)
{
timer = 0.0f;
timerclass.timeReset = false;
}
if (timerclass.timeRecord)
{
Debug.Log("Task complete!");
/*
* Debug.Log("Task elapsed time is: ");
* Debug.Log(visualTime);
* Debug.Log("peg counter is: ");
* Debug.Log(Counter.pegcounter);
* Debug.Log("wire counter is: ");
* Debug.Log(Counter.wirecounter);
*/
timerclass.timeRecord = false;
}
if (MyButton.mom)
{
if (gripper_max == 0 && Counter.gripper_calibration % 2 == 1)
{
gripper_max = Counter.gripper_value;
Debug.Log("grippermax value is " + gripper_max);
gripper_min = 0;
}
if (gripper_min == 0 && Counter.gripper_calibration % 2 == 0)
{
gripper_min = Counter.gripper_value;
Debug.Log("grippermin value is " + gripper_min);
if (!MyButton.tog)
{
gripper_min_now = gripper_min;
gripper_max_now = gripper_max;
Debug.Log("gripper calibration result: " + gripper_max_now + " " + gripper_min_now);
}
gripper_max = 0;
}
}
if (MyButton.tog)
{
// Debug.Log("Begin Pre-planned path");
}
// run if there is an openZen event
while (true)
{
ZenEvent zenEvent = new ZenEvent();
// read all events which are waiting for us
// use the rotation from the newest IMU event
if (!OpenZen.ZenPollNextEvent(mZenHandle, zenEvent))
{
break;
}
// if compontent handle = 0, this is a OpenZen wide event,
// like sensor search
if (zenEvent.component.handle != 0)
{
if (zenEvent.sensor.handle == mSensorHandle.handle)
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
// Unity Quaternion constructor has order x,y,z,w
// Furthermore, y and z axis need to be flipped to
// convert between the LPMS and Unity coordinate system
Quaternion sensorOrientation = new Quaternion(fq.getitem(1),
fq.getitem(3),
fq.getitem(2),
fq.getitem(0));
IMU29.transform.rotation = sensorOrientation;
HumanjointList[1].transform.rotation = sensorOrientation;
break;
}
}
if (zenEvent.sensor.handle == mSensorHandle2.handle)
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
// Unity Quaternion constructor has order x,y,z,w
// Furthermore, y and z axis need to be flipped to
// convert between the LPMS and Unity coordinate system
Quaternion sensorOrientation = new Quaternion(fq.getitem(1),
fq.getitem(3),
fq.getitem(2),
fq.getitem(0));
IMU98.transform.rotation = sensorOrientation;
float angle = 0.0f;
Vector3 axis = Vector3.zero;
sensorOrientation.ToAngleAxis(out angle, out axis);
// Debug.DrawRay(IMU98.transform.position, axis, Color.green);
// Rotate elbow joint
HumanjointList[2].transform.rotation = Quaternion.AngleAxis(angle, axis);
break;
}
}
if (zenEvent.sensor.handle == mSensorHandle3.handle)
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
// read quaternion
OpenZenFloatArray fq = OpenZenFloatArray.frompointer(zenEvent.data.imuData.q);
// Unity Quaternion constructor has order x,y,z,w
// Furthermore, y and z axis need to be flipped to
// convert between the LPMS and Unity coordinate system
Quaternion sensorOrientation = new Quaternion(fq.getitem(1),
fq.getitem(3),
fq.getitem(2),
fq.getitem(0));
IMUusb.transform.rotation = sensorOrientation;
float angle = 0.0f;
Vector3 axis = Vector3.zero;
sensorOrientation.ToAngleAxis(out angle, out axis);
// Rotate wrist joint
Quaternion rawquat = Quaternion.AngleAxis(angle, axis);
Quaternion usboffset = Quaternion.AngleAxis(195, Vector3.up);
Quaternion usboffset2 = Quaternion.AngleAxis(90, Vector3.forward);
Quaternion usboffset3 = Quaternion.AngleAxis(-15, Vector3.right);
HumanjointList[3].transform.rotation = rawquat * usboffset * usboffset2 * usboffset3;
break;
}
}
}
}
// run if the mode is set to IMU control
// if (isIMU)
if (isIMU && !MyButton.tog)
{
last_pos = Hand_demo.transform.position;
last_rot = Hand_demo.transform.rotation;
last_FK = Vector3.zero;
last_FKrot = Quaternion.identity;
}
if (isIMU && MyButton.tog)
{
// trans1 is the upper arm length
// trans2 is the lower arm length
Vector3 scale = new Vector3(1, 1, 1);
Vector3 trans0 = HumanjointList[1].transform.position;
Vector3 trans1 = new Vector3(lu, 0f, 0f); // apply calibrated link length of upperarm
Vector3 trans2 = new Vector3(lf, 0f, 0f); // apply calibrated link length of forearm
Vector3 trans3 = new Vector3(lh, 0f, 0f); // apply calibrated link length of hand
Matrix4x4 TrueShoulder = HumanjointList[1].transform.localToWorldMatrix;
Matrix4x4 World2Shoulder = Matrix4x4.TRS(trans0, HumanjointList[1].transform.rotation, scale);
Matrix4x4 Shoulder2Elbow = Matrix4x4.TRS(trans1, Quaternion.Inverse(HumanjointList[1].transform.rotation) * HumanjointList[2].transform.rotation, scale);
Matrix4x4 Elbow2Wrist = Matrix4x4.TRS(trans2, Quaternion.Inverse(HumanjointList[2].transform.rotation) * HumanjointList[3].transform.rotation, scale);
Vector3 Wrist2Palm = new Vector3(lh, 0f, 0f);
Matrix4x4 FK = World2Shoulder * Shoulder2Elbow * Elbow2Wrist;
Vector3 FKpos = FK.MultiplyPoint3x4(Wrist2Palm);
Vector3 FKpos1 = new Vector3(FK[0, 3], FK[1, 3], FK[2, 3]);
Quaternion rot = QuaternionFromMatrix(FK);
// Quaternion handoffset = Quaternion.AngleAxis(-90, Vector3.right);
Quaternion handoffset = Quaternion.AngleAxis(0, Vector3.right);
// Calculate actual gripper value here:
float actual_gripper = Mathf.InverseLerp((float)gripper_min_now, (float)gripper_max_now, (float)Counter.gripper_value);
// Debug.Log(actual_gripper);
// visualize the gripper here:
float left_z = Mathf.Lerp(3, -15, actual_gripper);
//Debug.Log("left is: "+left_z);
Left_Anchor.transform.localEulerAngles = new Vector3(0.0f, 0.0f, left_z);
float right_z = Mathf.Lerp(-3, 15, actual_gripper);
//Debug.Log("right is: " + right_z);
Right_Anchor.transform.localEulerAngles = new Vector3(0.0f, 0.0f, right_z);
// Assign pos and rot here for end-effector
Hand_demo.transform.position = FKpos;
Hand_demo.transform.rotation = rot * handoffset;
// send UDP
// Backup string sendF = VectorFromMatrix(FK);
string sendF = FKpos1.x.ToString() + "," + FKpos1.y.ToString() + "," + FKpos1.z.ToString() + "," + rot.x.ToString() + "," + rot.y.ToString() + "," + rot.z.ToString() + "," + rot.w.ToString();
if (sendmsg)
{
//Debug.Log(FK);
//Debug.Log(sendF);
// connection.Send(sendF);
}
if (Input.GetKeyDown(KeyCode.S))
{
sendmsg = !sendmsg;
if (sendmsg == true)
{
Debug.Log("Sending UDP");
}
else
{
Debug.Log("Stopped Sending UDP");
}
}
if (Input.GetKeyDown(KeyCode.R))
{
print("calibration sequence: bl,br,tr,tl");
print("Recorded Transformation Matrices: ");
calibration(calibrationfile, World2Shoulder, Shoulder2Elbow, Elbow2Wrist);
}
if (Input.GetKeyDown(KeyCode.A))
{
Debug.Log("New Calibration Trail");
using (System.IO.StreamWriter file = new System.IO.StreamWriter(calibrationfile, true))
{
file.WriteLine("New Try \n");
}
}
if (Input.GetKeyDown(KeyCode.J))
{
// Serialize transformation matrix to mock json string
string mockF = VectorFromMatrix(FK);
Debug.Log("Use the following for Simulated Json strings");
Debug.Log(mockF);
generateSphere = true;
if (generateSphere)
{
}
}
}
if (isUDP)
{
if (UDPInfo.lastReceivedUDPPacket != null)
{
// output json stream
// Debug.Log(UDPInfo.lastReceivedUDPPacket);
Matrix4x4 m = parser.GetMatrix4X4(UDPInfo.lastReceivedUDPPacket);
Vector3 UDPpos = m.MultiplyPoint3x4(Vector3.zero);
// Debug.Log(UDPpos);
Vector3 offset = new Vector3(0f, 0.15f, 0.92f);
Hand_demo.transform.position = UDPpos + offset;
Quaternion rot = QuaternionFromMatrix(m);
Quaternion handoffset = Quaternion.AngleAxis(0, Vector3.right);
Hand_demo.transform.rotation = rot * handoffset;
string sendU = Hand_demo.transform.position.x.ToString() + "," + Hand_demo.transform.position.y.ToString() + "," + Hand_demo.transform.position.z.ToString() + "," + Hand_demo.transform.rotation.x.ToString() + "," + Hand_demo.transform.rotation.y.ToString() + "," + Hand_demo.transform.rotation.z.ToString() + "," + Hand_demo.transform.rotation.w.ToString();
if (sendmsg)
{
//Debug.Log(FK);
//Debug.Log(sendU);
// connection.Send(sendU);
}
if (Input.GetKeyDown(KeyCode.S))
{
sendmsg = !sendmsg;
if (sendmsg == true)
{
Debug.Log("Sending UDP with MTM input");
}
else
{
Debug.Log("Stopped Sending UDP with MTM input");
}
}
}
}
}
// Non-static method
public void sensorEventThread()
{
try
{
while (true)
{
ZenEvent zenEvent = new ZenEvent();
//Console.WriteLine(mZenHandle);
if (OpenZen.ZenWaitForNextEvent(mZenHandle, zenEvent))
{
if (zenEvent.component.handle == 0)
{
// if the handle is on, its not a sensor event but a system wide
// event
switch (zenEvent.eventType)
{
case (int)ZenSensorEvent.ZenSensorEvent_SensorFound:
Console.WriteLine("found sensor event event " + zenEvent.data.sensorFound.name);
mFoundSensors.Add(zenEvent.data.sensorFound);
break;
case (int)ZenSensorEvent.ZenSensorEvent_SensorListingProgress:
if (zenEvent.data.sensorListingProgress.progress == 1.0)
{
mSearchDone = true;
}
break;
}
}
else
{
switch (zenEvent.eventType)
{
case (int)ZenImuEvent.ZenImuEvent_Sample:
mImuEventCount++;
if (mImuEventCount % 100 == 0)
{
continue;
}
// read acceleration
OpenZenFloatArray fa = OpenZenFloatArray.frompointer(zenEvent.data.imuData.a);
// read euler angles
OpenZenFloatArray fg = OpenZenFloatArray.frompointer(zenEvent.data.imuData.g);
// read quaternion
OpenZenFloatArray fb = OpenZenFloatArray.frompointer(zenEvent.data.imuData.b);
// Output Data
IMU_Display_Monitor imu = FormProvider.getIMU_Display_Monitor();
// Accelerometer
imu.acx = fa.getitem(0);
imu.acy = fa.getitem(1);
imu.acz = fa.getitem(2);
// Gyroscope
imu.gyx = fg.getitem(0);
imu.gyy = fg.getitem(1);
imu.gyz = fg.getitem(2);
// Magnetometer
imu.mgx = fb.getitem(0);
imu.mgy = fb.getitem(1);
imu.mgz = fb.getitem(2);
break;
}
}
}
}
}
catch (ThreadAbortException)
{
}
}
