private void Init() { _xIMUSerial = new x_IMU_API.xIMUserial(_portAssignment.PortName); DataGatherer = new xIMUDataGatherer(1 / _samplePeriode); _ahrs = new MadgwickAHRS(_samplePeriode, 0.1f); _tracker = new MotionCalculator(this); _tracker.FilterStateChanged(_filterState); }
//static AHRS.MahonyAHRS AHRS = new AHRS.MahonyAHRS(1f / 256f, 5f); /// <summary> /// Main method. /// </summary> /// <param name="args"> /// Unused. /// </param> static void Main(string[] args) { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString()); try { // Connect to x-IMU Console.WriteLine("Searching for x-IMU..."); x_IMU_API.PortAssignment[] portAssignment = (new x_IMU_API.PortScanner(true, true)).Scan(); x_IMU_API.xIMUserial xIMUserial = new x_IMU_API.xIMUserial(portAssignment[0].PortName); xIMUserial.Open(); Console.WriteLine("Connected to x-IMU " + portAssignment[0].DeviceID + " on " + portAssignment[0].PortName + "."); // Show 3D cuboid forms Console.WriteLine("Showing 3D Cuboid forms..."); Form_3Dcuboid form_3DcuboidA = new Form_3Dcuboid(); Form_3Dcuboid form_3DcuboidB = new Form_3Dcuboid(new string[] { "Form_3Dcuboid/RightInv.png", "Form_3Dcuboid/LeftInv.png", "Form_3Dcuboid/BackInv.png", "Form_3Dcuboid/FrontInv.png", "Form_3Dcuboid/TopInv.png", "Form_3Dcuboid/BottomInv.png" }); form_3DcuboidA.Text += " A"; form_3DcuboidB.Text += " B"; BackgroundWorker backgroundWorkerA = new BackgroundWorker(); BackgroundWorker backgroundWorkerB = new BackgroundWorker(); backgroundWorkerA.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidA.ShowDialog(); }); backgroundWorkerB.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidB.ShowDialog(); }); backgroundWorkerA.RunWorkerAsync(); backgroundWorkerB.RunWorkerAsync(); // x-IMU data received events to update cuboid form xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(delegate(object s, x_IMU_API.QuaternionData e) { form_3DcuboidA.RotationMatrix = e.ConvertToRotationMatrix(); }); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(delegate(object s, x_IMU_API.CalInertialAndMagneticData e) { form_3DcuboidB.RotationMatrix = (new x_IMU_API.QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix(); }); // Algorithm uses IMU update method. Console.WriteLine("Algorithm running in IMU mode."); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); Console.WriteLine("Press any key to continue..."); Console.ReadKey(); // Algorithm uses AHRS update method. Console.WriteLine("Algorithm running in AHRS mode."); xIMUserial.CalInertialAndMagneticDataReceived -= new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS); } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }
/// <summary> /// Form load event to create objects and set default form values /// </summary> private void Form_main_Load(object sender, EventArgs e) { this.Text = Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString(); // Create x-IMU and gimbal serial objects xIMUserial = new x_IMU_API.xIMUserial(); xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(xIMUserial_QuaternionDataReceived); gimbalSerial = new GimbalSerial(); // Get ports names refreshXIMUportList(); refreshPololuPortList(); // Create and start form update timer formUpdateTimer = new System.Windows.Forms.Timer(); formUpdateTimer.Interval = 50; formUpdateTimer.Tick += new EventHandler(formUpdateTimer_Tick); formUpdateTimer.Start(); }
/// <summary> /// Main method. /// </summary> /// <param name="args"> /// Unused. /// </param> static void Main(string[] args) { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString()); try { while (true) { // Connect to x-IMU Console.WriteLine("Searching for x-IMU..."); x_IMU_API.PortAssignment[] portAssignment = (new x_IMU_API.PortScanner(true, true)).Scan(); x_IMU_API.xIMUserial xIMUserial = new x_IMU_API.xIMUserial(portAssignment[0].PortName); xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(xIMUserial_QuaternionDataReceived); xIMUserial.DigitalIODataReceived += new x_IMU_API.xIMUserial.onDigitalIODataReceived(xIMUserial_DigitalIODataReceived); xIMUserial.Open(); Console.WriteLine("Connected to x-IMU " + portAssignment[0].DeviceID + " on " + portAssignment[0].PortName + "."); // Poll for key press and detect if connection lost Console.WriteLine("Press Esc to exit or any other key to set cursor centre orientation (i.e. tare)."); int prevCount; do { prevCount = xIMUserial.PacketsReadCounter.TotalPackets; Thread.Sleep(1000); if (Console.KeyAvailable == true) { if (Console.ReadKey(true).Key == ConsoleKey.Escape) { return; } xIMUserial.SendCommandPacket(x_IMU_API.CommandCodes.AlgorithmTare); } } while (prevCount != xIMUserial.PacketsReadCounter.TotalPackets); Console.WriteLine("No data received from x-IMU. Closing port."); xIMUserial.Close(); } } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }
/// <summary> /// Main method. /// </summary> /// <param name="args"> /// Unused. /// </param> static void Main(string[] args) { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString()); try { // Create ballTracking object ballTracking = new BallTracking(0.03f, 1.0f / 256.0f); // Connect to x-IMU Console.WriteLine("Searching for x-IMU..."); x_IMU_API.PortAssignment[] portAssignment = (new x_IMU_API.PortScanner(true, true)).Scan(); x_IMU_API.xIMUserial xIMUserial = new x_IMU_API.xIMUserial(portAssignment[0].PortName); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialMagneticDataReceived); xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(xIMUserial_QuaternionDataReceived); xIMUserial.Open(); Console.WriteLine("Connected to x-IMU " + portAssignment[0].DeviceID + " on " + portAssignment[0].PortName + "."); // Send 'algorithm initialise' command Console.WriteLine("Sending 'algorithm initialise' command..."); xIMUserial.SendCommandPacket(x_IMU_API.CommandCodes.AlgorithmInitialise); // Running graphics Console.WriteLine("Running 3D Cuboid form..."); form_3Dcuboid = new Form_3Dcuboid(new float[] { 0.06f, 0.04f, 0.02f }, Form_3Dcuboid.CameraViews.Top, 3.0f); form_3Dcuboid.WindowState = FormWindowState.Maximized; form_3Dcuboid.ShowDialog(); form_3Dcuboid = null; // Closing form enables mouse control mode. Console.WriteLine("Mouse control active."); } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }
/// <summary> /// Form Load event initialises form objects, sets form control values and starts auto-connect process. /// </summary> private void Form_main_Load(object sender, EventArgs e) { this.Text = Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString(); // Create peripheral GUIs and assign to ShowHideButton batteryOscilloscope = new SimpleOscilloscope("Battery Data (V)", "Oscilloscope/batteryOscilloscope_settings.ini"); showHideButton_batteryGraph.Object = batteryOscilloscope; thermometerOscilloscope = new SimpleOscilloscope("Thermometer Data (°C)", "Oscilloscope/thermometerOscilloscope_settings.ini"); showHideButton_thermometerGraph.Object = thermometerOscilloscope; gyroscopeOscilloscope = new SimpleOscilloscope("Gyroscope Data (°/s)", "Oscilloscope/gyroscopeOscilloscope_settings.ini"); showHideButton_gyroscopeGraph.Object = gyroscopeOscilloscope; accelerometerOscilloscope = new SimpleOscilloscope("Accelerometer Data (g)", "Oscilloscope/accelerometerOscilloscope_settings.ini"); showHideButton_accelerometerGraph.Object = accelerometerOscilloscope; magnetometerOscilloscope = new SimpleOscilloscope("Magnetometer Data (Gauss)", "Oscilloscope/magnetometerOscilloscope_settings.ini"); showHideButton_magnetometerGraph.Object = magnetometerOscilloscope; eulerAnglesOscilloscope = new SimpleOscilloscope("Euler Angles Data (°)", "Oscilloscope/eulerAnglesOscilloscope_settings.ini"); showHideButton_eulerAnglesGraph.Object = eulerAnglesOscilloscope; form_3Dcuboid = new Form_3Dcuboid(); form_3Dcuboid.MinimizeInsteadOfClose = true; showHideButton_3Dcuboid.Object = form_3Dcuboid; form_digitalIOpanel = new Form_digitalIOpanel(); form_digitalIOpanel.OutputChanged += new Form_digitalIOpanel.onOutputChanged(digitalIOpanel_OutputChanged); showHideButton_digitalIOpanel.Object = form_digitalIOpanel; analogueInputAX0AX1oscilloscope = new SimpleOscilloscope("Analogue Input AX0 AX1 Data (lsb)", "Oscilloscope/analogueInputAX0AX1oscilloscope_settings.ini"); showHideButton_AX0andAX1graph.Object = analogueInputAX0AX1oscilloscope; analogueInputAX2AX3oscilloscope = new SimpleOscilloscope("Analogue Input AX2 AX3 Data (lsb)", "Oscilloscope/analogueInputAX2AX3oscilloscope_settings.ini"); showHideButton_AX2andAX3graph.Object = analogueInputAX2AX3oscilloscope; analogueInputAX4AX5oscilloscope = new SimpleOscilloscope("Analogue Input AX4 AX5 Data (lsb)", "Oscilloscope/analogueInputAX4AX5oscilloscope_settings.ini"); showHideButton_AX4andAX5graph.Object = analogueInputAX4AX5oscilloscope; analogueInputAX6AX7oscilloscope = new SimpleOscilloscope("Analogue Input AX6 AX7 Data (lsb)", "Oscilloscope/analogueInputAX6AX7oscilloscope_settings.ini"); showHideButton_AX6andAX7graph.Object = analogueInputAX6AX7oscilloscope; form_PWMoutputPanel = new Form_PWMoutputPanel(); form_PWMoutputPanel.ValuesChanged += new Form_PWMoutputPanel.onValuesChanged(PWMoutputPanel_ValuesChanged); showHideButton_PWMoutputPanel.Object = form_PWMoutputPanel; ADXL345_Aoscilloscope = new SimpleOscilloscope("ADXL234 A Data", "Oscilloscope/ADXL345Aoscilloscope_settings.ini"); showHideButton_ADXL345Agraph.Object = ADXL345_Aoscilloscope; ADXL345_Boscilloscope = new SimpleOscilloscope("ADXL234 B Data", "Oscilloscope/ADXL345Boscilloscope_settings.ini"); showHideButton_ADXL345Bgraph.Object = ADXL345_Boscilloscope; ADXL345_Coscilloscope = new SimpleOscilloscope("ADXL234 C Data", "Oscilloscope/ADXL345Coscilloscope_settings.ini"); showHideButton_ADXL345Cgraph.Object = ADXL345_Coscilloscope; ADXL345_Doscilloscope = new SimpleOscilloscope("ADXL234 D Data", "Oscilloscope/ADXL345Doscilloscope_settings.ini"); showHideButton_ADXL345Dgraph.Object = ADXL345_Doscilloscope; // Create x-IMU serial object xIMUserial = new x_IMU_API.xIMUserial(); xIMUserial.xIMUdataReceived += new x_IMU_API.xIMUserial.onxIMUdataReceived(xIMUserial_xIMUdataReceived); xIMUserial.ErrorDataReceived += new x_IMU_API.xIMUserial.onErrorDataReceived(xIMUserial_ErrorDataReceived); xIMUserial.CommandDataReceived += new x_IMU_API.xIMUserial.onCommandDataReceived(xIMUserial_CommandDataReceived); xIMUserial.RegisterDataReceived += new x_IMU_API.xIMUserial.onRegisterDataReceived(xIMUserial_RegisterDataReceived); xIMUserial.DateTimeDataReceived += new x_IMU_API.xIMUserial.onDateTimeDataReceived(xIMUserial_DateTimeDataReceived); xIMUserial.RawBatteryAndThermometerDataReceived += new x_IMU_API.xIMUserial.onRawBatteryAndThermometerDataReceived(xIMUserial_RawBatteryAndThermometerDataReceived); xIMUserial.CalBatteryAndThermometerDataReceived += new x_IMU_API.xIMUserial.onCalBatteryAndThermometerDataReceived(xIMUserial_CalBatteryAndThermometerDataReceived); xIMUserial.RawInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onRawInertialAndMagneticDataReceived(xIMUserial_RawInertialAndMagneticDataReceived); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceivedHardIronCal); xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(xIMUserial_QuaternionDataReceived); xIMUserial.DigitalIODataReceived += new x_IMU_API.xIMUserial.onDigitalIODataReceived(xIMUserial_DigitalIODataReceived); xIMUserial.RawAnalogueInputDataReceived += new x_IMU_API.xIMUserial.onRawAnalogueInputDataReceived(xIMUserial_RawAnalogueInputDataReceived); xIMUserial.CalAnalogueInputDataReceived += new x_IMU_API.xIMUserial.onCalAnalogueInputDataReceived(xIMUserial_CalAnalogueInputDataReceived); xIMUserial.PWMoutputDataReceived += new x_IMU_API.xIMUserial.onPWMoutputDataReceived(xIMUserial_PWMoutputDataReceived); xIMUserial.RawADXL345busDataReceived += new x_IMU_API.xIMUserial.onRawADXL345busDataReceived(xIMUserial_RawADXL345busDataReceived); xIMUserial.CalADXL345busDataReceived += new x_IMU_API.xIMUserial.onCalADXL345busDataReceived(xIMUserial_CalADXL345busDataReceived); // Create buffers to parse data between xIMUserial and Form_main threads dateTimeDataBuffer = null; registerDataBuffer = new x_IMU_API.RegisterData[(int)x_IMU_API.RegisterAddresses.NumRegisters]; // Create ToolTip for RegisterTreeView ToolTip toolTip = new ToolTip(); toolTip.ToolTipTitle = "Tip:"; toolTip.SetToolTip(registerTreeView, "Right-click for action menu"); // Set fixed form control values label_GUIversionNum.Text = Assembly.GetExecutingAssembly().GetName().Version.ToString(); label_APIversionNum.Text = FileVersionInfo.GetVersionInfo("x-IMU API.dll").FileVersion.ToString(); label_compatibleFirmwareVersionNums.Text = ""; for (int i = 0; i < Enum.GetValues(typeof(x_IMU_API.CompatibleFirmwareVersions)).Length; i++) { label_compatibleFirmwareVersionNums.Text += ((int[])Enum.GetValues(typeof(x_IMU_API.CompatibleFirmwareVersions)))[i].ToString() + ".x"; if (i < ((int[])Enum.GetValues(typeof(x_IMU_API.CompatibleFirmwareVersions))).Length - 1) { label_compatibleFirmwareVersionNums.Text += ", "; } } // Set default variable form control values button_refreshList.PerformClick(); textBox_dataLoggerFilePath.Text = Environment.GetFolderPath(Environment.SpecialFolder.Personal) + "\\" + "LoggedData"; textBox_collectHardIronCalDatasetFilePath.Text = Environment.GetFolderPath(Environment.SpecialFolder.Personal) + "\\" + "HardIronCal"; // Create and start form update timer formUpdateTimer = new System.Windows.Forms.Timer(); formUpdateTimer.Interval = 50; formUpdateTimer.Tick += new EventHandler(formUpdateTimer_Tick); formUpdateTimer.Start(); // Auto connect on start up toggleButton_openClosePort.PerformClick(); }
/// <summary> /// Button Click event to upload firmware. /// </summary> private void button_bootloaderUpload_Click(object sender, EventArgs e) { // Error if file not exist if (!File.Exists(textBox_bootloaderFilePath.Text)) { MessageBox.Show("File does not exist.", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); return; } // Error if selected port name = "Auto" if (string.Equals(comboBox_portName.Text, "Auto", StringComparison.CurrentCultureIgnoreCase)) { MessageBox.Show("Port name \"Auto\" cannot be used for bootloading. Please select the port name assigned to the USB connection.", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); return; } // User confirmation if (MessageBox.Show("Please ensure that the selected port name is the port assigned to the x-IMU USB connection else the upload will fail." + Environment.NewLine + Environment.NewLine + "The new firmware will be uploaded to the x-IMU and the current firmware will be lost." + Environment.NewLine + Environment.NewLine + "Do not switch off or disconnect the x-IMU while firmware is being uploaded as this may permanently damage the x-IMU.", "Warning", MessageBoxButtons.OKCancel, MessageBoxIcon.Warning) == System.Windows.Forms.DialogResult.Cancel) { return; } // Disable bootloader form controls textBox_bootloaderFilePath.Enabled = false; button_bootloaderBrowse.Enabled = false; button_bootloaderUpload.Enabled = false; button_bootloaderUpload.Text = "Uploading..."; this.Update(); // Close port is currently open bool reopenPort = false; if (xIMUserial.IsOpen) { xIMUserial.Close(); reopenPort = true; } // Perform firmware upload procedure try { // Reset device x_IMU_API.xIMUserial tempxIMUserial = new x_IMU_API.xIMUserial(comboBox_portName.Text); tempxIMUserial.Open(); tempxIMUserial.SendCommandPacket(x_IMU_API.CommandCodes.Reset); tempxIMUserial.Close(); // Run external bootloader process ProcessStartInfo processInfo = new ProcessStartInfo("Bootloader/16-Bit Flash Programmer.exe"); processInfo.Arguments = "-i " + "\\\\.\\" + comboBox_portName.Text + " -b 115200 \"" + textBox_bootloaderFilePath.Text + "\""; processInfo.UseShellExecute = false; Process process = Process.Start(processInfo); process.WaitForExit(); process.Close(); } catch (Exception ex) { MessageBox.Show(ex.Message, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error); } // Re-enable bootloader form controls textBox_bootloaderFilePath.Enabled = true; button_bootloaderBrowse.Enabled = true; button_bootloaderUpload.Enabled = true; button_bootloaderUpload.Text = "Upload"; // Re-open port if was closed prior to bootload if (reopenPort) { xIMUserial.Open(); } }