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) { } }