public override async Task Start()
{
switch (SelectedAccelerometer)
{
case "MPU-9250":
Sensor = (await Mpu6050.ProbeAsync(Board.I2c, true))[0];
break;
case "ADXL-345":
Sensor = (await Adxl345.ProbeAsync(Board.I2c))[0];
break;
case "LIS3DH":
Sensor = (await Lis3dh.ProbeAsync(Board.I2c))[0];
break;
case "BNO055":
Sensor = (await Bno055.ProbeAsync(Board.I2c))[0];
break;
case "LSM303DLHC":
var lsm303dlhc = new Lsm303dlhc(Board.I2c);
Sensor = lsm303dlhc.Accel;
break;
}
Sensor.AutoUpdateWhenPropertyRead = false;
OnPropertyChanged(nameof(Sensor));
}
private void dataGridView_device_data_CellEndEdit(object sender, DataGridViewCellEventArgs e)
{
var register = Mpu6050.GetRegisterFromDescription(
dataGridView_device_data.Rows[e.RowIndex].Cells["description"].Value.ToString());
var data = (float)Convert.ToDouble(dataGridView_device_data.Rows[e.RowIndex].Cells["value"].Value);
if (Mpu6050.RegisterLength(register) == 4)
{
var floatValue = new ByteArrayFloat();
floatValue.FloatNumber = data;
_mpu6050.WriteData(_deviceId, (int)register, new List <Byte> {
floatValue.Byte0, floatValue.Byte1, floatValue.Byte2, floatValue.Byte3
}, true);
}
else if (Mpu6050.RegisterLength(register) == 2)
{
_mpu6050.WriteWord(_deviceId, (int)register, (int)data, true);
}
else
{
_mpu6050.WriteByte(_deviceId, (int)register, (int)data, true);
}
RefreshRegisters(_deviceId);
}
public MeadowApp()
{
Console.WriteLine("Initializing");
mpu5060 = new Mpu6050(Device.CreateI2cBus());
TestMpu6050();
}
public DeviceDataUpdater(Mpu6050 networkBus, DataGridView dgv, int id)
{
_dataGridView = dgv;
_mpu6050 = networkBus;
_thread = new Thread(UpdateData)
{
Interval = 0
};
_thread.Starting += InitializeRows;
_thread.Starting += InitializeStaticData;
_id = id;
}
public SearchDynamixels(Mpu6050 mpu6050, List <int> baudNumList, TreeView treeView, ToolStripProgressBar progressBar, ToolStrip toolStrip, ToolStripLabel percentLabel, ToolStripLabel stateLabel)
{
_treeView = treeView;
_progressBar = progressBar;
_toolStrip = toolStrip;
_toolStripPercent = percentLabel;
_searchThread = new Thread(Search)
{
Interval = 0
};
_mpu6050 = mpu6050;
_baudNumList = baudNumList;
_stateLabel = stateLabel;
}
public MeadowApp()
{
Console.WriteLine("Initializing");
mpu = new Mpu6050(Device.CreateI2cBus(), 0x69);
mpu.AccelerationChangeThreshold = 0.05f;
mpu.Updated += Mpu_Updated;
mpu.StartUpdating(500);
while (true)
{
Console.WriteLine($"{mpu.TemperatureC}°C");
Thread.Sleep(5000);
}
}
public MeadowApp()
{
Console.WriteLine("Initializing");
mpu = new Mpu6050(Device.CreateI2cBus(), 0x69);
mpu.AccelerationXChanged += OnRotated;
mpu.AccelerationYChanged += OnRotated;
mpu.AccelerationZChanged += OnRotated;
mpu.AccelerationChangeThreshold = 0.05f;
mpu.StartSampling(TimeSpan.FromMilliseconds(500));
while (true)
{
Console.WriteLine($"{mpu.TemperatureC}°C");
Thread.Sleep(5000);
}
}
private void toolStripButton_connect_Click(object sender, EventArgs e)
{
if (toolStripComboBox_com_port.SelectedItem == null)
{
return;
}
_mpu6050 = new Mpu6050(toolStripComboBox_com_port.SelectedItem.ToString(), 1);
if (_mpu6050.Open())
{
toolStripButton_connect.Enabled = false;
toolStripButton_disconnect.Enabled = true;
panel_search.Enabled = panel_search.Visible = true;
panel_device_data.Visible = panel_device_data.Enabled = false;
Console.WriteLine(String.Format("Connected !"));
return;
}
Console.WriteLine(String.Format("Connection Failed !"));
}
void Initialize()
{
var onboardLed = new RgbPwmLed(
device: Device,
redPwmPin: Device.Pins.OnboardLedRed,
greenPwmPin: Device.Pins.OnboardLedGreen,
bluePwmPin: Device.Pins.OnboardLedBlue);
onboardLed.SetColor(Color.Red);
up = new Led(Device.CreateDigitalOutputPort(Device.Pins.D13));
down = new Led(Device.CreateDigitalOutputPort(Device.Pins.D10));
left = new Led(Device.CreateDigitalOutputPort(Device.Pins.D11));
right = new Led(Device.CreateDigitalOutputPort(Device.Pins.D12));
mpu = new Mpu6050(Device.CreateI2cBus());
mpu.Updated += MpuUpdated;
onboardLed.SetColor(Color.Green);
}
public MeadowApp()
{
var led = new RgbLed(Device, Device.Pins.OnboardLedRed, Device.Pins.OnboardLedGreen, Device.Pins.OnboardLedBlue);
led.SetColor(RgbLed.Colors.Red);
up = new Led(Device.CreateDigitalOutputPort(Device.Pins.D15));
down = new Led(Device.CreateDigitalOutputPort(Device.Pins.D12));
left = new Led(Device.CreateDigitalOutputPort(Device.Pins.D14));
right = new Led(Device.CreateDigitalOutputPort(Device.Pins.D13));
mpu = new Mpu6050(Device.CreateI2cBus());
//mpu.AccelerationChangeThreshold = 0.05f;
//mpu.Updated += MpuUpdated;
//mpu.StartUpdating(100);
up.IsOn = true;
led.SetColor(RgbLed.Colors.Green);
}
public void Enable(Accessories accessory)
{
try
{
switch (accessory)
{
case Accessories.Camera:
if (camera is null)
{
this.camera = new Camera();
}
else
{
Disable(Accessories.Camera); this.camera = new Camera();
}
break;
case Accessories.IMU:
if (imu is null)
{
this.imu = new Mpu6050(I2cDevice.Create(
new I2cConnectionSettings(1, Mpu6050.DefaultI2cAddress)));
}
else
{
Disable(Accessories.IMU);
this.imu = new Mpu6050(I2cDevice.Create(
new I2cConnectionSettings(1, Mpu6050.DefaultI2cAddress)));
}
break;
case Accessories.MotorL:
if (motorL is null)
{
this.motorL = DCMotor.Create(6, 12, 13);
}
else
{
Disable(Accessories.MotorL); this.motorL = DCMotor.Create(6, 12, 13);
}
break;
case Accessories.MotorR:
if (motorR is null)
{
this.motorR = DCMotor.Create(26, 20, 21);
}
else
{
Disable(Accessories.MotorR); this.motorR = DCMotor.Create(26, 20, 21);
}
break;
case Accessories.Motors:
Enable(Accessories.MotorL);
Enable(Accessories.MotorR);
break;
case Accessories.ADC:
if (adc is null)
{
this.adc = new Tlc1543(24, 5, 23, 25);
}
else
{
Disable(Accessories.ADC); this.adc = new Tlc1543(24, 5, 23, 25);
}
break;
case Accessories.IR:
if (ir is null)
{
this.ir = new IrReceiver(17);
}
else
{
Disable(Accessories.IR); this.ir = new IrReceiver(17);
}
break;
case Accessories.Sonar:
if (sonar is null)
{
this.sonar = new Hcsr04(22, 27);
}
else
{
Disable(Accessories.Sonar); this.sonar = new Hcsr04(22, 27);
}
break;
case Accessories.LED:
if (led is null)
{
this.led = new Ws2812b(18, 4);
}
else
{
Disable(Accessories.LED); this.led = new Ws2812b(18, 4);
}
break;
case Accessories.CPUTemp:
if (cpuTemperature is null)
{
this.cpuTemperature = new CpuTemperature();
}
//else { Disable(led); this.led = new Ws2812b(18, 4); }
break;
case Accessories.All:
foreach (var item in Enum.GetValues(typeof(Accessories)))
{
Enable((Accessories)item);
}
break;
default:
Console.WriteLine("Something went wrong (Enabling accessories)");
break;
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Enabling accessory: {Enum.GetName(typeof(Accessories), accessory)} failed.");
System.Diagnostics.Debug.WriteLine($"Exception message: {ex.Message}");
Console.WriteLine($"Enabling accessory: {Enum.GetName(typeof(Accessories), accessory)} failed.");
Console.WriteLine($"Exception message: {ex.Message}");
}
}
public static void ImuTest(List <string> argsList, Mpu6050 imu)
{
Console.WriteLine($"IMU Test");
double delay;
if (argsList.Count > 1)
{
delay = Convert.ToDouble(argsList[1]);
}
else
{
delay = 10;
}
Kalman gx = new Kalman(), gy = new Kalman(), gz = new Kalman();
Kalman ax = new Kalman(), ay = new Kalman(), az = new Kalman();
Stopwatch sw = Stopwatch.StartNew();
var freq = (double)Stopwatch.Frequency;
Debug csv = new Debug();
Debug csv_filtered = new Debug();
string separator = ",";
string line = "T, Acc X, Acc Y, Acc Z, Gyro X, Gyro Y, Gyro Z, Temp" + Environment.NewLine;
string line_filtered = "T, Acc X, Acc Y, Acc Z, Gyro X, Gyro Y, Gyro Z, Temp" + Environment.NewLine;
if (argsList[2] == "csv")
{
csv.CSV_Write("testdata", line);
csv_filtered.CSV_Write("testdata_filtered", line_filtered);
}
Console.WriteLine("Run Gyroscope and Accelerometer Self Test:");
Console.WriteLine($"{imu.RunGyroscopeAccelerometerSelfTest()}");
//Console.WriteLine("Calibrate Gyroscope and Accelerometer:");
//Console.WriteLine($"{imu.CalibrateGyroscopeAccelerometer()}");
double time;
double last_time = 0;
while (true)
{
time = ((double)sw.ElapsedTicks / freq) * 1000;
if (time - last_time >= delay)
{
System.Numerics.Vector3 acc = imu.GetAccelerometer();
System.Numerics.Vector3 gyro = imu.GetGyroscopeReading();
var temp = imu.GetTemperature();
Console.Write($"T: {(time - last_time):F12} ");
line = $"{time:F12}" + separator;
line_filtered = $"{time:F12}" + separator;
Console.Write($"Acc");
Console.Write($" X: ");
Console.Write($"{acc.X,6:F2} ");
Console.Write($"{ax.getFilteredValue(acc.X),6:F2} ");
line += $"{acc.X,6:F2}" + separator;
line_filtered += $"{ax.getFilteredValue(acc.X),6:F2}" + separator;
Console.Write($" Y: ");
Console.Write($"{acc.Y,6:F2} ");
Console.Write($"{ay.getFilteredValue(acc.Y),6:F2} ");
line += $"{acc.Y,6:F2}" + separator;
line_filtered += $"{ay.getFilteredValue(acc.Y),6:F2}" + separator;
Console.Write($" Z: ");
Console.Write($"{acc.Z,6:F2} ");
Console.Write($"{az.getFilteredValue(acc.Z),6:F2} ");
line += $"{acc.Z,6:F2}" + separator;
line_filtered += $"{az.getFilteredValue(acc.Z),6:F2}" + separator;
Console.Write($" Gyro ");
Console.Write($" X: ");
Console.Write($"{gyro.X,8:F2} ");
Console.Write($"{gx.getFilteredValue(gyro.X),8:F2} ");
line += $"{gyro.X,8:F2}" + separator;
line_filtered += $"{gx.getFilteredValue(gyro.X),8:F2}" + separator;
Console.Write($" Y: ");
Console.Write($"{gyro.Y,8:F2} ");
Console.Write($"{gy.getFilteredValue(gyro.Y),8:F2} ");
line += $"{gyro.Y,8:F2}" + separator;
line_filtered += $"{gy.getFilteredValue(gyro.Y),8:F2}" + separator;
Console.Write($" Z: ");
Console.Write($"{gyro.Z,8:F2} ");
Console.Write($"{gz.getFilteredValue(gyro.Z),8:F2} ");
line += $"{gyro.Z,8:F2}" + separator;
line_filtered += $"{gz.getFilteredValue(gyro.Z),8:F2}" + separator;
Console.Write($"{temp.DegreesCelsius.ToString("0.00"),8:F2}°C");
line += $"{temp.DegreesCelsius.ToString("0.00"),8:F2}";
line_filtered += $"{temp.DegreesCelsius.ToString("0.00"),8:F2}";
Console.Write(Environment.NewLine);
line += Environment.NewLine;
line_filtered += Environment.NewLine;
last_time = time;
if (argsList[2] == "csv")
{
csv.CSV_Write("testdata", line);
csv_filtered.CSV_Write("testdata_filtered", line_filtered);
}
}
}
}
