private async void streamSwitch_Toggled(object sender, RoutedEventArgs e) { double angle1 = 0; double angle2 = 0; if (streamSwitch.IsOn) { myStopWatch.Start(); isRunning = true; Clear_Click(null, null); samples[0] = 0; samples[1] = 0; sensorFusions = new ISensorFusionBosch[2]; sensorFusion = metawears[0].GetModule <ISensorFusionBosch>(); sensorFusion.Configure(); // default settings is NDoF mode with +/-16g acc range and 2000dps gyro range await sensorFusion.Quaternion.AddRouteAsync(source => source.Stream(async data => { if (isRunning) { var quat = data.Value <Quaternion>(); var time = data.FormattedTimestamp.ToString(); var year = time.Substring(0, 4); var month = time.Substring(5, 2); var day = time.Substring(8, 2); var hour = time.Substring(11, 2); var minute = time.Substring(14, 2); var second = time.Substring(17, 2); var milli = time.Substring(20, 3); //print(time.ToString()); //print(time[0].ToString()); // Store data point if (record) { String newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}{12}", samples[0], year, month, day, hour, minute, second, milli, quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine); addPoint(newLine, 0); } //var secs = myStopWatch.ElapsedMilliseconds * 0.001; // Update counters samples[0]++; freq[0]++; // Save reference quaternion if (shouldCenter[0]) { centerQuats[0] = quat; shouldCenter[0] = false; centered[0] = true; } double angle = 0; double denom = 1; if (centered[0]) { WindowsQuaternion a = convertToWindowsQuaternion(centerQuats[0]); WindowsQuaternion b = convertToWindowsQuaternion(quat); quat = centerData(centerQuats[0], quat); angle = (angleMode) ? 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI) : 0; } else if (angleMode) { angle = 2 * Math.Acos(quat.W) * (180 / Math.PI); denom = Math.Sqrt(1 - Math.Pow(quat.W, 2)); denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors } angle = (angle > 250) ? 360 - angle : angle; angle1 = angle; await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => { // Add values to plot if ((bool)wCheckbox.IsChecked) { (model.Series[0] as LineSeries).Points.Add(new DataPoint(samples[0], (angleMode) ? angle : quat.W)); } if ((bool)xyzCheckbox.IsChecked) { (model.Series[1] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X / denom)); (model.Series[2] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y / denom)); (model.Series[3] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z / denom)); } // Display values numerically double[] values = { angleMode?angle : quat.W, (quat.X / denom), (quat.Y / denom), (quat.Z / denom) }; String[] labels = { angleMode ? "Angle: " : "W: ", "\nX: ", "\nY: ", "\nY: " }; String s = createOrientationText(labels, values); setText(s, 0); // Reset axes as needed if ((bool)wCheckbox.IsChecked || (bool)xyzCheckbox.IsChecked) { model.InvalidatePlot(true); //if (secs > MainViewModel.MAX_SECONDS) if (samples.Max() > MainViewModel.MAX_DATA_SAMPLES) { model.Axes[1].Reset(); //model.Axes[1].Maximum = secs; //model.Axes[1].Minimum = secs - MainViewModel.MAX_SECONDS; model.Axes[1].Maximum = samples.Max(); model.Axes[1].Minimum = (samples.Max() - MainViewModel.MAX_DATA_SAMPLES); model.Axes[1].Zoom(model.Axes[1].Minimum, model.Axes[1].Maximum); } } }); } })); sensorFusion.Quaternion.Start(); sensorFusion.Start(); if (numBoards == 2) { sensorFusion2 = metawears[1].GetModule <ISensorFusionBosch>(); sensorFusion2.Configure(); // default settings is NDoF mode with +/-16g acc range and 2000dps gyro range await sensorFusion2.Quaternion.AddRouteAsync(source => source.Stream(async data => { var quat = data.Value <Quaternion>(); // Store data point if (record) { String newLine = string.Format("{0},{1},{2},{3},{4}{5}", samples[0], quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine); addPoint(newLine, 1); } if (isRunning) { // Update counters samples[1]++; freq[1]++; // Center quaternion if needed if (shouldCenter[1]) { centerQuats[1] = quat; shouldCenter[1] = false; centered[1] = true; } double angle = 0; double denom = 1; if (centered[1]) { WindowsQuaternion a = convertToWindowsQuaternion(centerQuats[1]); WindowsQuaternion b = convertToWindowsQuaternion(quat); angle = 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI);; quat = centerData(centerQuats[1], quat); //System.Diagnostics.Debug.WriteLine(finalQuat.w.ToString() + " " + finalQuat.x.ToString() + " " + finalQuat.y.ToString() + " " + finalQuat.z.ToString()); } else if (angleMode) { angle = 2 * Math.Acos(quat.W) * (180 / Math.PI); denom = Math.Sqrt(1 - Math.Pow(quat.W, 2)); denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors } if (denom < 0.001) { denom = 1; } angle = (angle > 250) ? 360 - angle : angle; angle2 = angle; //var secs = myStopWatch.ElapsedMilliseconds * 0.001; await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => { // Display quaternion String s; // return; if (angleSwitch.IsOn) { if ((bool)wCheckbox.IsChecked) { (model.Series[4] as LineSeries).Points.Add(new DataPoint(samples[0], angle)); } if ((bool)xyzCheckbox.IsChecked) { (model.Series[5] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X / denom)); (model.Series[6] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y / denom)); (model.Series[7] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z / denom)); } s = "Angle: " + angle.ToString() + "\nX: " + (quat.X / denom).ToString() + "\nY: " + (quat.Y / denom).ToString() + "\nZ: " + (quat.Z / denom).ToString(); } else { if ((bool)wCheckbox.IsChecked) { (model.Series[4] as LineSeries).Points.Add(new DataPoint(samples[0], quat.W)); } if ((bool)xyzCheckbox.IsChecked) { (model.Series[5] as LineSeries).Points.Add(new DataPoint(samples[0], quat.X)); (model.Series[6] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Y)); (model.Series[7] as LineSeries).Points.Add(new DataPoint(samples[0], quat.Z)); } s = "W: " + quat.W.ToString() + "\nX: " + quat.X.ToString() + "\nY: " + quat.Y.ToString() + "\nZ: " + quat.Z.ToString(); } setText(s, 1); difference.Text = Math.Abs(angle1 - angle2).ToString(); }); } })); sensorFusion2.Quaternion.Start(); sensorFusion2.Start(); print("Sensor fusion should be running!"); } InitFreqTimer(); Clear.Background = new SolidColorBrush(Windows.UI.Colors.Red); } else { isRunning = false; sensorFusions[0] = sensorFusion; sensorFusions[1] = sensorFusion2; for (var i = 0; i < numBoards; i++) { sensorFusions[i].Stop(); sensorFusions[i].Quaternion.Stop(); metawears[i].TearDown(); freq[i] = 0; } timer1.Dispose(); myStopWatch.Stop(); myStopWatch.Reset(); } }
//accelerometer tests //private void updateAccelerometer(IData data) { // var accel = data.Value<Acceleration>(); // setText(accel.ToString(), 0); //} /// <summary> /// Routine used for collection of Quaternions data of Sensors. In the end, calls a new routine to plot processed Data. /// </summary> /// <param name="data"> Quaternion data collected by the sensor</param> /// <param name="sensorNumber"> number of the sensor used in project, starting by 0 for 1, 1 for 2 and etc </param> private async Task quaternionStreamRoutine(IData data, int sensorNumber) { if (isRunning) { var quat = data.Value <Quaternion>(); var eulerAngles = calculateEulerAngles(quat); var time = data.FormattedTimestamp.ToString(); var year = time.Substring(0, 4); var month = time.Substring(5, 2); var day = time.Substring(8, 2); var hour = time.Substring(11, 2); var minute = time.Substring(14, 2); var second = time.Substring(17, 2); var milli = time.Substring(20, 3); // Store data point if (record) { String newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}{12}", samples[0], year, month, day, hour, minute, second, milli, quat.W, quat.X, quat.Y, quat.Z, Environment.NewLine); addPoint(newLine, sensorNumber); newLine = string.Format("{0},{1},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11}", samples[0], year, month, day, hour, minute, second, milli, eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw, Environment.NewLine); addPointEuler(newLine, sensorNumber); } // Update counters samples[sensorNumber]++; freq[sensorNumber]++; // Save reference quaternion if (shouldCenter[sensorNumber]) { refEuler[sensorNumber] = eulerAngles; refQuats[sensorNumber] = quat; shouldCenter[sensorNumber] = false; centered[sensorNumber] = true; } double angle = 0; double denom = 1; if (centered[sensorNumber]) { WindowsQuaternion a = convertToWindowsQuaternion(refQuats[sensorNumber]); WindowsQuaternion b = convertToWindowsQuaternion(quat); quat = centerData(refQuats[sensorNumber], quat); eulerAngles = centerData(refEuler[sensorNumber], eulerAngles); angle = (angleMode) ? 2 * Math.Acos(WindowsQuaternion.Dot(a, b) / (a.Length() * b.Length())) * (180 / Math.PI) : 0; } else if (angleMode) { angle = 2 * Math.Acos(quat.W) * (180 / Math.PI); denom = Math.Sqrt(1 - Math.Pow(quat.W, 2)); denom = (denom < 0.001) ? 1 : denom; // avoid divide by zero type errors } angle = (angle > 180) ? 360 - angle : angle; await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () => { plotValues(angle, quat, eulerAngles, denom, sensorNumber); }); } }