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