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