private void streamSwitch_Toggled(object sender, RoutedEventArgs e)
{
if (streamSwitch.IsOn)
{
sensorfusion.Quaternion.Start();
sensorfusion.Start();
}
else
{
sensorfusion.Stop();
sensorfusion.Quaternion.Stop();
}
}
private void streamSwitch_Toggled(object sender, RoutedEventArgs e)
{
if (streamSwitch.IsOn)
{
m_SensorFusion.EulerAngles.Start();
m_SensorFusion.Start();
}
else
{
m_SensorFusion.Stop();
m_SensorFusion.EulerAngles.Stop();
}
}
protected async void OnNavigatedTo2(NavigationEventArgs e)
{
base.OnNavigatedTo(e);
var samples = 0;
var model = (DataContext as MainViewModel).MyModel;
metawear = MbientLab.MetaWear.Win10.Application.GetMetaWearBoard(e.Parameter as BluetoothLEDevice);
//accelerometer = metawear.GetModule<IAccelerometer>();
//gyro = metawear.GetModule<IGyroBmi160>();
//gyro.Configure();
//accelerometer.Configure(odr: 100f, range: 8f);
m_SensorFusion = metawear.GetModule <ISensorFusionBosch>();
gyro = metawear.GetModule <IGyroBmi160>();
gyro.Configure(OutputDataRate._100Hz);
accelerometer = metawear.GetModule <IAccelerometer>();
accelerometer.Configure(odr: 100f, range: 8f);
m_SensorFusion.Configure();
await m_SensorFusion.EulerAngles.AddRouteAsync(source =>
{
source.Stream(async data =>
{
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
var value = data.Value <EulerAngles>();
(model.Series[0] as LineSeries).Points.Add(new DataPoint(samples, value.Pitch));
(model.Series[1] as LineSeries).Points.Add(new DataPoint(samples, value.Roll));
(model.Series[2] as LineSeries).Points.Add(new DataPoint(samples, value.Yaw));
samples++;
model.InvalidatePlot(true);
if (samples > MainViewModel.MAX_DATA_SAMPLES)
{
model.Axes[1].Reset();
model.Axes[1].Maximum = samples;
model.Axes[1].Minimum = (samples - MainViewModel.MAX_DATA_SAMPLES);
model.Axes[1].Zoom(model.Axes[1].Minimum, model.Axes[1].Maximum);
}
GyroText.Text = value.ToString();
});
});
});
m_SensorFusion.Start();
m_SensorFusion.EulerAngles.Start();
}
static async Task Main(string[] args)
{
IMetaWearBoard board = null;
bool succeed = false;
int retries = 5;
//while (!succeed && retries > 0)
{
//try
{
board = MbientLab.MetaWear.NetStandard.Application.GetMetaWearBoard(args[0]);
//var board = BLEMetawear.Application.GetMetaWearBoard(args[0]);
board.TimeForResponse = 100000;
board.OnUnexpectedDisconnect += OnDisconneted;
await board.InitializeAsync();
succeed = true;
}
//catch
{
retries--;
}
}
ILed led = null;
ISensorFusionBosch sensor = null;
if (board.IsConnected)
{
led = board.GetModule <ILed>();
led.EditPattern(MbientLab.MetaWear.Peripheral.Led.Color.Green, MbientLab.MetaWear.Peripheral.Led.Pattern.Solid);
led.Play();
sensor = board.GetModule <ISensorFusionBosch>();
sensor.Configure();
var rout = await sensor.EulerAngles.AddRouteAsync(source =>
{
try
{
source.Stream(data =>
{
var value = data.Value <EulerAngles>();
var AngularRead = (int)value.Roll;
var OrbitalRead = (int)value.Pitch;
//Console.Clear();
Console.Write($"\rroll: {value.Roll} pitch:{value.Pitch} Yaw:{value.Yaw} ");
//Rotate(-value.Pitch, 0 , -value.Yaw);
});
}
catch (Exception ex)
{
Console.Write(ex.Message);
//LogException(LoggerCategory, ex, "Could not initialize IMU stream callback");
throw;
}
});
sensor.EulerAngles.Start();
sensor.Start();
}
ConsoleKeyInfo key = new ConsoleKeyInfo();
while (key.Key != ConsoleKey.Q)
{
key = Console.ReadKey();
}
if (board.IsConnected)
{
if (led != null)
{
led.Stop(true);
}
if (sensor != null)
{
sensor.EulerAngles.Stop();
sensor.Stop();
}
await board.DisconnectAsync();
//board.TearDown();
}
}
private async void OnConnect(object sender, RoutedEventArgs e)
{
var item = lst.SelectedItem as BLEDevice;
if (item != null)
{
if (m_Metaware == null)
{
await MbientLab.MetaWear.NetStandard.Application.ClearDeviceCacheAsync(item.Mac);
m_Metaware = MbientLab.MetaWear.NetStandard.Application.GetMetaWearBoard(item.Mac);
m_Metaware.TimeForResponse = 100000;
}
if (m_Metaware.IsConnected)
{
await Disconnect();
}
else
{
try
{
int retries = 5;
do
{
try
{
btnConnect.Content = "Connecting...";
btnConnect.IsEnabled = false;
m_Metaware.OnUnexpectedDisconnect += OnDisconneted;
await m_Metaware.InitializeAsync();
retries = -1;
}
catch
{
retries--;
}
} while (retries > 0);
btnConnect.IsEnabled = true;
if (m_Metaware.IsConnected)
{
var batteryLevel = await m_Metaware.ReadBatteryLevelAsync();
var led = m_Metaware.GetModule <ILed>();
led.EditPattern(MbientLab.MetaWear.Peripheral.Led.Color.Green, MbientLab.MetaWear.Peripheral.Led.Pattern.Solid);
led.Play();
item.BatteryLevel = batteryLevel;
BatteryText.Text = $" Battery:{batteryLevel}";
btnConnect.Content = "Disconnect";
m_Connected = true;
//m_Metaware.GetModule<IMagnetometerBmm150>();
//m_Gyro = m_Metaware.GetModule<IGyroBmi160>();
//m_Gyro.Configure(OutputDataRate._100Hz);
//m_Accelerometer = m_Metaware.GetModule<IAccelerometer>();
//m_Accelerometer.Configure(odr: 100f, range: 8f);
m_SensorFusion = m_Metaware.GetModule <ISensorFusionBosch>();
m_SensorFusion.Configure();
var rout = await m_SensorFusion.EulerAngles.AddRouteAsync(source =>
{
try
{
source.Stream(async data =>
{
await Dispatcher.InvokeAsync(() =>
{
var value = data.Value <EulerAngles>();
AngleText.Text = value.ToString();
Rotate(-value.Roll, -value.Pitch, -value.Yaw);
//Rotate(-value.Pitch, 0 , -value.Yaw);
});
});
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
});
if (!rout.Valid)
{
MessageBox.Show("rout invalid");
}
await m_SensorFusion.Gravity.AddRouteAsync(source =>
{
source.Stream(async data =>
{
await Dispatcher.InvokeAsync(() =>
{
var value = data.Value <Acceleration>();
GravityText.Text = value.ToString();
//Rotate(-value.Roll, -value.Pitch, -value.Yaw);
//Rotate(-value.Pitch, 0 , -value.Yaw);
});
});
});
//var calib = await m_SensorFusion.ReadCalibrationStateAsync();
m_SensorFusion.Gravity.Start();
m_SensorFusion.EulerAngles.Start();
m_SensorFusion.Start();
//Scanner.Stop();
}
else
{
throw new InvalidOperationException("Could not connect");
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
btnConnect.Content = "Connect";
m_Connected = false;
}
}
}
}
protected async override void OnNavigatedTo(NavigationEventArgs e)
{
base.OnNavigatedTo(e);
var samples = 0;
var model = (DataContext as MainViewModel).MyModel;
metawear = MbientLab.MetaWear.Win10.Application.GetMetaWearBoard(e.Parameter as BluetoothLEDevice);
//accelerometer = metawear.GetModule<IAccelerometer>();
//gyro = metawear.GetModule<IGyroBmi160>();
//gyro.Configure();
//accelerometer.Configure(odr: 100f, range: 8f);
m_SensorFusion = metawear.GetModule <ISensorFusionBosch>();
m_SensorFusion.Configure();
await m_SensorFusion.EulerAngles.AddRouteAsync(source =>
{
source.Stream(async data =>
{
await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
{
var value = data.Value <EulerAngles>();
(model.Series[0] as LineSeries).Points.Add(new DataPoint(samples, value.Pitch));
(model.Series[1] as LineSeries).Points.Add(new DataPoint(samples, value.Roll));
(model.Series[2] as LineSeries).Points.Add(new DataPoint(samples, value.Yaw));
samples++;
model.InvalidatePlot(true);
if (samples > MainViewModel.MAX_DATA_SAMPLES)
{
model.Axes[1].Reset();
model.Axes[1].Maximum = samples;
model.Axes[1].Minimum = (samples - MainViewModel.MAX_DATA_SAMPLES);
model.Axes[1].Zoom(model.Axes[1].Minimum, model.Axes[1].Maximum);
}
});
});
});
//await accelerometer.PackedAcceleration.AddRouteAsync(source => source.Stream(async data =>
//{
// var value = data.Value<Acceleration>();
// await CoreApplication.MainView.CoreWindow.Dispatcher.RunAsync(CoreDispatcherPriority.Normal, () =>
// {
// (model.Series[0] as LineSeries).Points.Add(new DataPoint(samples, value.X));
// (model.Series[1] as LineSeries).Points.Add(new DataPoint(samples, value.Y));
// (model.Series[2] as LineSeries).Points.Add(new DataPoint(samples, value.Z));
// samples++;
// model.InvalidatePlot(true);
// if (samples > MainViewModel.MAX_DATA_SAMPLES)
// {
// model.Axes[1].Reset();
// model.Axes[1].Maximum = samples;
// model.Axes[1].Minimum = (samples - MainViewModel.MAX_DATA_SAMPLES);
// model.Axes[1].Zoom(model.Axes[1].Minimum, model.Axes[1].Maximum);
// }
// });
//}));
//await gyro.AngularVelocity.AddRouteAsync(source =>
// {
// source.Stream(async data=>
// {
// var dt = data.Value<AngularVelocity>();
// await Dispatcher.RunAsync(CoreDispatcherPriority.High, async () =>
// {
// GyroText.Text = dt.ToString();//=$" X = {dt.X}";
// });
// }
// );
// });
//await gyro.PullConfigAsync();
//gyro.Start();
//gyro.AngularVelocity.Start();
m_SensorFusion.Start();
m_SensorFusion.EulerAngles.Start();
}
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();
}
}