diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 64b39d246e..dd535df3cd 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -70,55 +70,81 @@ ADIS16448_IMU::ADIS16448_IMU() : ADIS16448_IMU(kZ, SPI::Port::kMXP, 4) {} ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, uint16_t cal_time) - : m_yaw_axis(yaw_axis), m_spi_port(port) { - // Force the IMU reset pin to toggle on startup (doesn't require DS enable) - // Relies on the RIO hardware by default configuring an output as low - // and configuring an input as high Z. The 10k pull-up resistor internal to - // the IMU then forces the reset line high for normal operation. - DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low - Wait(10_ms); // Wait 10ms - delete m_reset_out; - new DigitalInput(18); // Set MXP DIO8 high - Wait(500_ms); // Wait 500ms for reset to complete - - ConfigCalTime(cal_time); - - // Configure standard SPI - if (!SwitchToStandardSPI()) { - return; + : m_yaw_axis(yaw_axis), + m_spi_port(port), + m_simDevice("Gyro:ADIS16448", port) { + if (m_simDevice) { + m_simGyroAngleX = + m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0); + m_simGyroAngleY = + m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0); + m_simGyroAngleZ = + m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0); + m_simGyroRateX = + m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0); + m_simGyroRateY = + m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0); + m_simGyroRateZ = + m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0); + m_simAccelX = + m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0); + m_simAccelY = + m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0); + m_simAccelZ = + m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0); } - // Set IMU internal decimation to 819.2 SPS - WriteRegister(SMPL_PRD, 0x0001); - // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) - WriteRegister(MSC_CTRL, 0x0016); - // Disable IMU internal Bartlett filter - WriteRegister(SENS_AVG, 0x0400); - // Clear offset registers - WriteRegister(XGYRO_OFF, 0x0000); - WriteRegister(YGYRO_OFF, 0x0000); - WriteRegister(ZGYRO_OFF, 0x0000); - // Configure and enable auto SPI - if (!SwitchToAutoSPI()) { - return; + if (!m_simDevice) { + // Force the IMU reset pin to toggle on startup (doesn't require DS enable) + // Relies on the RIO hardware by default configuring an output as low + // and configuring an input as high Z. The 10k pull-up resistor internal to + // the IMU then forces the reset line high for normal operation. + DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low + Wait(10_ms); // Wait 10ms + delete m_reset_out; + new DigitalInput(18); // Set MXP DIO8 high + Wait(500_ms); // Wait 500ms for reset to complete + + ConfigCalTime(cal_time); + + // Configure standard SPI + if (!SwitchToStandardSPI()) { + return; + } + + // Set IMU internal decimation to 819.2 SPS + WriteRegister(SMPL_PRD, 0x0001); + // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) + WriteRegister(MSC_CTRL, 0x0016); + // Disable IMU internal Bartlett filter + WriteRegister(SENS_AVG, 0x0400); + // Clear offset registers + WriteRegister(XGYRO_OFF, 0x0000); + WriteRegister(YGYRO_OFF, 0x0000); + WriteRegister(ZGYRO_OFF, 0x0000); + // Configure and enable auto SPI + if (!SwitchToAutoSPI()) { + return; + } + // Notify DS that IMU calibration delay is active + REPORT_WARNING( + "ADIS16448 IMU Detected. Starting initial calibration delay."); + // Wait for whatever time the user set as the start-up delay + Wait(static_cast(m_calibration_time) * 1.2_s); + // Execute calibration routine + Calibrate(); + // Reset accumulated offsets + Reset(); + // Tell the acquire loop that we're done starting up + m_start_up_mode = false; + + // Let the user know the IMU was initiallized successfully + REPORT_WARNING("ADIS16448 IMU Successfully Initialized!"); + + // TODO: Find what the proper pin is to turn this LED + // Drive MXP PWM5 (IMU ready LED) low (active low) + new DigitalOutput(19); } - // Notify DS that IMU calibration delay is active - REPORT_WARNING("ADIS16448 IMU Detected. Starting initial calibration delay."); - // Wait for whatever time the user set as the start-up delay - Wait(static_cast(m_calibration_time) * 1.2_s); - // Execute calibration routine - Calibrate(); - // Reset accumulated offsets - Reset(); - // Tell the acquire loop that we're done starting up - m_start_up_mode = false; - - // Let the user know the IMU was initiallized successfully - REPORT_WARNING("ADIS16448 IMU Successfully Initialized!"); - - // TODO: Find what the proper pin is to turn this LED - // Drive MXP PWM5 (IMU ready LED) low (active low) - new DigitalOutput(19); // Report usage and post data to DS HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0); @@ -739,11 +765,11 @@ double ADIS16448_IMU::GetAngle() const { double ADIS16448_IMU::GetRate() const { switch (m_yaw_axis) { case kX: - return GetGyroInstantX(); + return GetGyroRateX(); case kY: - return GetGyroInstantY(); + return GetGyroRateY(); case kZ: - return GetGyroInstantZ(); + return GetGyroRateZ(); default: return 0.0; } @@ -754,46 +780,73 @@ ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const { } double ADIS16448_IMU::GetGyroAngleX() const { + if (m_simGyroAngleX) { + return m_simGyroAngleX.Get(); + } std::scoped_lock sync(m_mutex); return m_integ_gyro_x; } double ADIS16448_IMU::GetGyroAngleY() const { + if (m_simGyroAngleY) { + return m_simGyroAngleY.Get(); + } std::scoped_lock sync(m_mutex); return m_integ_gyro_y; } double ADIS16448_IMU::GetGyroAngleZ() const { + if (m_simGyroAngleZ) { + return m_simGyroAngleZ.Get(); + } std::scoped_lock sync(m_mutex); return m_integ_gyro_z; } -double ADIS16448_IMU::GetGyroInstantX() const { +double ADIS16448_IMU::GetGyroRateX() const { + if (m_simGyroRateX) { + return m_simGyroRateX.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_x; } -double ADIS16448_IMU::GetGyroInstantY() const { +double ADIS16448_IMU::GetGyroRateY() const { + if (m_simGyroRateY) { + return m_simGyroRateY.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_y; } -double ADIS16448_IMU::GetGyroInstantZ() const { +double ADIS16448_IMU::GetGyroRateZ() const { + if (m_simGyroRateZ) { + return m_simGyroRateZ.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_z; } -double ADIS16448_IMU::GetAccelInstantX() const { +double ADIS16448_IMU::GetAccelX() const { + if (m_simAccelX) { + return m_simAccelX.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_x; } -double ADIS16448_IMU::GetAccelInstantY() const { +double ADIS16448_IMU::GetAccelY() const { + if (m_simAccelY) { + return m_simAccelY.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_y; } -double ADIS16448_IMU::GetAccelInstantZ() const { +double ADIS16448_IMU::GetAccelZ() const { + if (m_simAccelZ) { + return m_simAccelZ.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_z; } @@ -843,6 +896,10 @@ double ADIS16448_IMU::GetTemperature() const { return m_temp; } +int ADIS16448_IMU::GetPort() const { + return m_spi_port; +} + /** * @brief Builds a Sendable object to push IMU data to the driver station. * diff --git a/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp index 94eed095a9..8f2718b9bd 100644 --- a/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp @@ -69,55 +69,80 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, CalibrationTime cal_time) : m_yaw_axis(yaw_axis), m_spi_port(port), - m_calibration_time(static_cast(cal_time)) { - // Force the IMU reset pin to toggle on startup (doesn't require DS enable) - // Relies on the RIO hardware by default configuring an output as low - // and configuring an input as high Z. The 10k pull-up resistor internal to - // the IMU then forces the reset line high for normal operation. - DigitalOutput* m_reset_out = - new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low - Wait(10_ms); // Wait 10ms - delete m_reset_out; - new DigitalInput(27); // Set SPI CS2 (IMU RST) high - Wait(500_ms); // Wait 500ms for reset to complete - - // Configure standard SPI - if (!SwitchToStandardSPI()) { - return; + m_calibration_time(static_cast(cal_time)), + m_simDevice("Gyro:ADIS16470", port) { + if (m_simDevice) { + m_simGyroAngleX = + m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0); + m_simGyroAngleY = + m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0); + m_simGyroAngleZ = + m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0); + m_simGyroRateX = + m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0); + m_simGyroRateY = + m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0); + m_simGyroRateZ = + m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0); + m_simAccelX = + m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0); + m_simAccelY = + m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0); + m_simAccelZ = + m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0); } - // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = - // 400Hz) - WriteRegister(DEC_RATE, 0x0004); - // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and - // PoP - WriteRegister(MSC_CTRL, 0x0001); - // Configure IMU internal Bartlett filter - WriteRegister(FILT_CTRL, 0x0000); - // Configure continuous bias calibration time based on user setting - WriteRegister(NULL_CNFG, m_calibration_time | 0x700); + if (!m_simDevice) { + // Force the IMU reset pin to toggle on startup (doesn't require DS enable) + // Relies on the RIO hardware by default configuring an output as low + // and configuring an input as high Z. The 10k pull-up resistor internal to + // the IMU then forces the reset line high for normal operation. + DigitalOutput* m_reset_out = + new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low + Wait(10_ms); // Wait 10ms + delete m_reset_out; + new DigitalInput(27); // Set SPI CS2 (IMU RST) high + Wait(500_ms); // Wait 500ms for reset to complete - // Notify DS that IMU calibration delay is active - REPORT_WARNING("ADIS16470 IMU Detected. Starting initial calibration delay."); + // Configure standard SPI + if (!SwitchToStandardSPI()) { + return; + } - // Wait for samples to accumulate internal to the IMU (110% of user-defined - // time) - Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1}); + // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) + // = 400Hz) + WriteRegister(DEC_RATE, 0x0004); + // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation + // and PoP + WriteRegister(MSC_CTRL, 0x0001); + // Configure IMU internal Bartlett filter + WriteRegister(FILT_CTRL, 0x0000); + // Configure continuous bias calibration time based on user setting + WriteRegister(NULL_CNFG, m_calibration_time | 0x700); - // Write offset calibration command to IMU - WriteRegister(GLOB_CMD, 0x0001); + // Notify DS that IMU calibration delay is active + REPORT_WARNING( + "ADIS16470 IMU Detected. Starting initial calibration delay."); - // Configure and enable auto SPI - if (!SwitchToAutoSPI()) { - return; + // Wait for samples to accumulate internal to the IMU (110% of user-defined + // time) + Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1}); + + // Write offset calibration command to IMU + WriteRegister(GLOB_CMD, 0x0001); + + // Configure and enable auto SPI + if (!SwitchToAutoSPI()) { + return; + } + + // Let the user know the IMU was initiallized successfully + REPORT_WARNING("ADIS16470 IMU Successfully Initialized!"); + + // Drive SPI CS3 (IMU ready LED) low (active low) + new DigitalOutput(28); } - // Let the user know the IMU was initiallized successfully - REPORT_WARNING("ADIS16470 IMU Successfully Initialized!"); - - // Drive SPI CS3 (IMU ready LED) low (active low) - new DigitalOutput(28); - // Report usage and post data to DS HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0); @@ -708,6 +733,23 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle, *not coincide with the IMU Z axis. **/ double ADIS16470_IMU::GetAngle() const { + switch (m_yaw_axis) { + case kX: + if (m_simGyroAngleX) { + return m_simGyroAngleX.Get(); + } + break; + case kY: + if (m_simGyroAngleY) { + return m_simGyroAngleY.Get(); + } + break; + case kZ: + if (m_simGyroAngleZ) { + return m_simGyroAngleZ.Get(); + } + break; + } std::scoped_lock sync(m_mutex); return m_integ_angle; } @@ -716,11 +758,11 @@ double ADIS16470_IMU::GetRate() const { std::scoped_lock sync(m_mutex); switch (m_yaw_axis) { case kX: - return m_gyro_x; + return GetGyroRateX(); case kY: - return m_gyro_y; + return GetGyroRateY(); case kZ: - return m_gyro_z; + return GetGyroRateZ(); default: return 0.0; } @@ -730,32 +772,50 @@ ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const { return m_yaw_axis; } -double ADIS16470_IMU::GetGyroInstantX() const { +double ADIS16470_IMU::GetGyroRateX() const { + if (m_simGyroRateX) { + return m_simGyroRateX.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_x; } -double ADIS16470_IMU::GetGyroInstantY() const { +double ADIS16470_IMU::GetGyroRateY() const { + if (m_simGyroRateY) { + return m_simGyroRateY.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_y; } -double ADIS16470_IMU::GetGyroInstantZ() const { +double ADIS16470_IMU::GetGyroRateZ() const { + if (m_simGyroRateZ) { + return m_simGyroRateZ.Get(); + } std::scoped_lock sync(m_mutex); return m_gyro_z; } -double ADIS16470_IMU::GetAccelInstantX() const { +double ADIS16470_IMU::GetAccelX() const { + if (m_simAccelX) { + return m_simAccelX.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_x; } -double ADIS16470_IMU::GetAccelInstantY() const { +double ADIS16470_IMU::GetAccelY() const { + if (m_simAccelY) { + return m_simAccelY.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_y; } -double ADIS16470_IMU::GetAccelInstantZ() const { +double ADIS16470_IMU::GetAccelZ() const { + if (m_simAccelZ) { + return m_simAccelZ.Get(); + } std::scoped_lock sync(m_mutex); return m_accel_z; } @@ -780,6 +840,10 @@ double ADIS16470_IMU::GetYFilteredAccelAngle() const { return m_accelAngleY; } +int ADIS16470_IMU::GetPort() const { + return m_spi_port; +} + /** * @brief Builds a Sendable object to push IMU data to the driver station. * diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp new file mode 100644 index 0000000000..96f33abc56 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/simulation/ADIS16448_IMUSim.h" + +#include +#include + +using namespace frc::sim; + +ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) { + frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()}; + m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); + m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); + m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); + m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); + m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); + m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); + m_simAccelX = deviceSim.GetDouble("accel_x"); + m_simAccelY = deviceSim.GetDouble("accel_y"); + m_simAccelZ = deviceSim.GetDouble("accel_z"); +} + +void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) { + m_simGyroAngleX.Set(angle.value()); +} + +void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) { + m_simGyroAngleY.Set(angle.value()); +} + +void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) { + m_simGyroAngleZ.Set(angle.value()); +} + +void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) { + m_simGyroRateX.Set(rate.value()); +} + +void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) { + m_simGyroRateY.Set(rate.value()); +} + +void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) { + m_simGyroRateZ.Set(rate.value()); +} + +void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { + m_simAccelX.Set(accel.value() / 9.81); +} + +void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) { + m_simAccelY.Set(accel.value() / 9.81); +} + +void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) { + m_simAccelZ.Set(accel.value() / 9.81); +} diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp new file mode 100644 index 0000000000..f111679919 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/simulation/ADIS16470_IMUSim.h" + +#include +#include + +using namespace frc::sim; + +ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) { + frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()}; + m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); + m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); + m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); + m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); + m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); + m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); + m_simAccelX = deviceSim.GetDouble("accel_x"); + m_simAccelY = deviceSim.GetDouble("accel_y"); + m_simAccelZ = deviceSim.GetDouble("accel_z"); +} + +void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) { + m_simGyroAngleX.Set(angle.value()); +} + +void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) { + m_simGyroAngleY.Set(angle.value()); +} + +void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) { + m_simGyroAngleZ.Set(angle.value()); +} + +void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) { + m_simGyroRateX.Set(rate.value()); +} + +void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) { + m_simGyroRateY.Set(rate.value()); +} + +void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) { + m_simGyroRateZ.Set(rate.value()); +} + +void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { + m_simAccelX.Set(accel.value() / 9.81); +} + +void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) { + m_simAccelY.Set(accel.value() / 9.81); +} + +void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) { + m_simAccelZ.Set(accel.value() / 9.81); +} diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index faa4199e0a..beae5b1788 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -23,16 +23,12 @@ #include #include +#include #include #include #include #include -// Not always defined in cmath (not part of standard) -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - namespace frc { /** * Use DMA SPI to read rate, acceleration, and magnetometer data from the @@ -138,17 +134,17 @@ class ADIS16448_IMU : public nt::NTSendable, double GetGyroAngleZ() const; - double GetGyroInstantX() const; + double GetGyroRateX() const; - double GetGyroInstantY() const; + double GetGyroRateY() const; - double GetGyroInstantZ() const; + double GetGyroRateZ() const; - double GetAccelInstantX() const; + double GetAccelX() const; - double GetAccelInstantY() const; + double GetAccelY() const; - double GetAccelInstantZ() const; + double GetAccelZ() const; double GetXComplementaryAngle() const; @@ -174,6 +170,13 @@ class ADIS16448_IMU : public nt::NTSendable, int ConfigDecRate(uint16_t DecimationRate); + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + int GetPort() const; + void InitSendable(nt::NTSendableBuilder& builder) override; private: @@ -320,6 +323,17 @@ class ADIS16448_IMU : public nt::NTSendable, std::thread m_acquire_task; + hal::SimDevice m_simDevice; + hal::SimDouble m_simGyroAngleX; + hal::SimDouble m_simGyroAngleY; + hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; + hal::SimDouble m_simAccelX; + hal::SimDouble m_simAccelY; + hal::SimDouble m_simAccelZ; + struct NonMovableMutexWrapper { wpi::mutex mutex; NonMovableMutexWrapper() = default; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 9feff5fdd1..78aa1eabdd 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -134,17 +135,17 @@ class ADIS16470_IMU : public nt::NTSendable, double GetRate() const; - double GetGyroInstantX() const; + double GetGyroRateX() const; - double GetGyroInstantY() const; + double GetGyroRateY() const; - double GetGyroInstantZ() const; + double GetGyroRateZ() const; - double GetAccelInstantX() const; + double GetAccelX() const; - double GetAccelInstantY() const; + double GetAccelY() const; - double GetAccelInstantZ() const; + double GetAccelZ() const; double GetXComplementaryAngle() const; @@ -161,6 +162,13 @@ class ADIS16470_IMU : public nt::NTSendable, // IMU yaw axis IMUAxis m_yaw_axis; + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + int GetPort() const; + void InitSendable(nt::NTSendableBuilder& builder) override; private: @@ -368,6 +376,17 @@ class ADIS16470_IMU : public nt::NTSendable, std::thread m_acquire_task; + hal::SimDevice m_simDevice; + hal::SimDouble m_simGyroAngleX; + hal::SimDouble m_simGyroAngleY; + hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; + hal::SimDouble m_simAccelX; + hal::SimDouble m_simAccelY; + hal::SimDouble m_simAccelZ; + struct NonMovableMutexWrapper { wpi::mutex mutex; NonMovableMutexWrapper() = default; diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h new file mode 100644 index 0000000000..b7dfe8ab64 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h @@ -0,0 +1,106 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +namespace frc { + +class ADIS16448_IMU; + +namespace sim { + +/** + * Class to control a simulated ADIS16448 IMU. + */ +class ADIS16448_IMUSim { + public: + /** + * Constructs from a ADIS16448_IMU object. + * + * @param imu ADIS16448_IMU to simulate + */ + explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu); + + /** + * Sets the X axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleX(units::degree_t angle); + + /** + * Sets the Y axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleY(units::degree_t angle); + + /** + * Sets the Z axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleZ(units::degree_t angle); + + /** + * Sets the X axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateX(units::degrees_per_second_t rate); + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateY(units::degrees_per_second_t rate); + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateZ(units::degrees_per_second_t rate); + + /** + * Sets the X axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelX(units::meters_per_second_squared_t accel); + + /** + * Sets the Y axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelY(units::meters_per_second_squared_t accel); + + /** + * Sets the Z axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelZ(units::meters_per_second_squared_t accel); + + private: + hal::SimDouble m_simGyroAngleX; + hal::SimDouble m_simGyroAngleY; + hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; + hal::SimDouble m_simAccelX; + hal::SimDouble m_simAccelY; + hal::SimDouble m_simAccelZ; +}; + +} // namespace sim +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h new file mode 100644 index 0000000000..e1135aeb09 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h @@ -0,0 +1,106 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +namespace frc { + +class ADIS16470_IMU; + +namespace sim { + +/** + * Class to control a simulated ADIS16470 IMU. + */ +class ADIS16470_IMUSim { + public: + /** + * Constructs from a ADIS16470_IMU object. + * + * @param imu ADIS16470_IMU to simulate + */ + explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu); + + /** + * Sets the X axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleX(units::degree_t angle); + + /** + * Sets the Y axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleY(units::degree_t angle); + + /** + * Sets the Z axis angle (CCW positive). + * + * @param angle The angle. + */ + void SetGyroAngleZ(units::degree_t angle); + + /** + * Sets the X axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateX(units::degrees_per_second_t rate); + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateY(units::degrees_per_second_t rate); + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param rate The angular rate. + */ + void SetGyroRateZ(units::degrees_per_second_t rate); + + /** + * Sets the X axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelX(units::meters_per_second_squared_t accel); + + /** + * Sets the Y axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelY(units::meters_per_second_squared_t accel); + + /** + * Sets the Z axis acceleration. + * + * @param accel The acceleration. + */ + void SetAccelZ(units::meters_per_second_squared_t accel); + + private: + hal::SimDouble m_simGyroAngleX; + hal::SimDouble m_simGyroAngleY; + hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; + hal::SimDouble m_simAccelX; + hal::SimDouble m_simAccelY; + hal::SimDouble m_simAccelZ; +}; + +} // namespace sim +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java index a583e12f31..206cdb0ac0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java @@ -15,6 +15,8 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.wpilibj.interfaces.Gyro; @@ -162,6 +164,17 @@ public class ADIS16448_IMU implements Gyro, NTSendable { private DigitalOutput m_status_led; private Thread m_acquire_task; + private SimDevice m_simDevice; + private SimDouble m_simGyroAngleX; + private SimDouble m_simGyroAngleY; + private SimDouble m_simGyroAngleZ; + private SimDouble m_simGyroRateX; + private SimDouble m_simGyroRateY; + private SimDouble m_simGyroRateZ; + private SimDouble m_simAccelX; + private SimDouble m_simAccelY; + private SimDouble m_simAccelZ; + /* CRC-16 Look-Up Table */ int adiscrc[] = new int[] { @@ -227,52 +240,68 @@ public class ADIS16448_IMU implements Gyro, NTSendable { m_acquire_task = new Thread(new AcquireTask(this)); - // Force the IMU reset pin to toggle on startup (doesn't require DS enable) - m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low - Timer.delay(0.01); // Wait 10ms - m_reset_out.close(); - m_reset_in = new DigitalInput(18); // Set MXP DIO8 high - Timer.delay(0.25); // Wait 250ms - - configCalTime(cal_time); - - if (!switchToStandardSPI()) { - return; + m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value); + if (m_simDevice != null) { + m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); + m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); + m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); + m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); + m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); + m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); + m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); + m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); + m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); } - // Set IMU internal decimation to 819.2 SPS - writeRegister(SMPL_PRD, 0x0001); - // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) - writeRegister(MSC_CTRL, 0x0016); - // Disable IMU internal Bartlett filter - writeRegister(SENS_AVG, 0x0400); - // Clear offset registers - writeRegister(XGYRO_OFF, 0x0000); - writeRegister(YGYRO_OFF, 0x0000); - writeRegister(ZGYRO_OFF, 0x0000); + if (m_simDevice == null) { + // Force the IMU reset pin to toggle on startup (doesn't require DS enable) + m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low + Timer.delay(0.01); // Wait 10ms + m_reset_out.close(); + m_reset_in = new DigitalInput(18); // Set MXP DIO8 high + Timer.delay(0.25); // Wait 250ms - // Configure standard SPI - if (!switchToAutoSPI()) { - return; + configCalTime(cal_time); + + if (!switchToStandardSPI()) { + return; + } + + // Set IMU internal decimation to 819.2 SPS + writeRegister(SMPL_PRD, 0x0001); + // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP) + writeRegister(MSC_CTRL, 0x0016); + // Disable IMU internal Bartlett filter + writeRegister(SENS_AVG, 0x0400); + // Clear offset registers + writeRegister(XGYRO_OFF, 0x0000); + writeRegister(YGYRO_OFF, 0x0000); + writeRegister(ZGYRO_OFF, 0x0000); + + // Configure standard SPI + if (!switchToAutoSPI()) { + return; + } + // Notify DS that IMU calibration delay is active + DriverStation.reportWarning( + "ADIS16448 IMU Detected. Starting initial calibration delay.", false); + // Wait for whatever time the user set as the start-up delay + try { + Thread.sleep((long) (m_calibration_time * 1.2 * 1000)); + } catch (InterruptedException e) { + } + // Execute calibration routine + calibrate(); + // Reset accumulated offsets + reset(); + // Tell the acquire loop that we're done starting up + m_start_up_mode = false; + // Let the user know the IMU was initiallized successfully + DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false); + // Drive MXP PWM5 (IMU ready LED) low (active low) + m_status_led = new DigitalOutput(19); } - // Notify DS that IMU calibration delay is active - DriverStation.reportWarning( - "ADIS16448 IMU Detected. Starting initial calibration delay.", false); - // Wait for whatever time the user set as the start-up delay - try { - Thread.sleep((long) (m_calibration_time * 1.2 * 1000)); - } catch (InterruptedException e) { - } - // Execute calibration routine - calibrate(); - // Reset accumulated offsets - reset(); - // Tell the acquire loop that we're done starting up - m_start_up_mode = false; - // Let the user know the IMU was initiallized successfully - DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false); - // Drive MXP PWM5 (IMU ready LED) low (active low) - m_status_led = new DigitalOutput(19); + // Report usage and post data to DS HAL.report(tResourceType.kResourceType_ADIS16448, 0); } @@ -908,11 +937,11 @@ public class ADIS16448_IMU implements Gyro, NTSendable { public synchronized double getRate() { switch (m_yaw_axis) { case kX: - return getGyroInstantX(); + return getGyroRateX(); case kY: - return getGyroInstantY(); + return getGyroRateY(); case kZ: - return getGyroInstantZ(); + return getGyroRateZ(); default: return 0.0; } @@ -925,46 +954,73 @@ public class ADIS16448_IMU implements Gyro, NTSendable { /** @return accumulated gyro angle in the X axis */ public synchronized double getGyroAngleX() { + if (m_simGyroAngleX != null) { + return m_simGyroAngleX.get(); + } return m_integ_gyro_x; } /** @return accumulated gyro angle in the Y axis */ public synchronized double getGyroAngleY() { + if (m_simGyroAngleY != null) { + return m_simGyroAngleY.get(); + } return m_integ_gyro_y; } /** @return accumulated gyro angle in the Z axis */ public synchronized double getGyroAngleZ() { + if (m_simGyroAngleZ != null) { + return m_simGyroAngleZ.get(); + } return m_integ_gyro_z; } /** @return current gyro angle in the X axis */ - public synchronized double getGyroInstantX() { + public synchronized double getGyroRateX() { + if (m_simGyroRateX != null) { + return m_simGyroRateX.get(); + } return m_gyro_x; } /** @return current gyro angle in the Y axis */ - public synchronized double getGyroInstantY() { + public synchronized double getGyroRateY() { + if (m_simGyroRateY != null) { + return m_simGyroRateY.get(); + } return m_gyro_y; } /** @return current gyro angle in the Z axis */ - public synchronized double getGyroInstantZ() { + public synchronized double getGyroRateZ() { + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } return m_gyro_z; } /** @return urrent acceleration in the X axis */ - public synchronized double getAccelInstantX() { + public synchronized double getAccelX() { + if (m_simAccelX != null) { + return m_simAccelX.get(); + } return m_accel_x; } /** @return current acceleration in the Y axis */ - public synchronized double getAccelInstantY() { + public synchronized double getAccelY() { + if (m_simAccelY != null) { + return m_simAccelY.get(); + } return m_accel_y; } /** @return current acceleration in the Z axis */ - public synchronized double getAccelInstantZ() { + public synchronized double getAccelZ() { + if (m_simAccelZ != null) { + return m_simAccelZ.get(); + } return m_accel_z; } @@ -1013,6 +1069,15 @@ public class ADIS16448_IMU implements Gyro, NTSendable { return m_temp; } + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + public int getPort() { + return m_spi_port.value; + } + @Override public void initSendable(NTSendableBuilder builder) { builder.setSmartDashboardType("Gyro"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java index e3a41feeed..890b6e50af 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java @@ -14,6 +14,8 @@ package edu.wpi.first.wpilibj; // import java.lang.FdLibm.Pow; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.wpilibj.interfaces.Gyro; @@ -250,6 +252,17 @@ public class ADIS16470_IMU implements Gyro, NTSendable { private DigitalOutput m_status_led; private Thread m_acquire_task; + private SimDevice m_simDevice; + private SimDouble m_simGyroAngleX; + private SimDouble m_simGyroAngleY; + private SimDouble m_simGyroAngleZ; + private SimDouble m_simGyroRateX; + private SimDouble m_simGyroRateY; + private SimDouble m_simGyroRateZ; + private SimDouble m_simAccelX; + private SimDouble m_simAccelY; + private SimDouble m_simAccelZ; + private static class AcquireTask implements Runnable { private ADIS16470_IMU imu; @@ -279,59 +292,74 @@ public class ADIS16470_IMU implements Gyro, NTSendable { m_acquire_task = new Thread(new AcquireTask(this)); - // Force the IMU reset pin to toggle on startup (doesn't require DS enable) - // Relies on the RIO hardware by default configuring an output as low - // and configuring an input as high Z. The 10k pull-up resistor internal to the - // IMU then forces the reset line high for normal operation. - m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low - Timer.delay(0.01); // Wait 10ms - m_reset_out.close(); - m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high - Timer.delay(0.25); // Wait 250ms for reset to complete - - if (!switchToStandardSPI()) { - return; + m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); + if (m_simDevice != null) { + m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); + m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); + m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); + m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); + m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); + m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); + m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); + m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); + m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); } - // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = - // 400Hz) - writeRegister(DEC_RATE, 4); + if (m_simDevice == null) { + // Force the IMU reset pin to toggle on startup (doesn't require DS enable) + // Relies on the RIO hardware by default configuring an output as low + // and configuring an input as high Z. The 10k pull-up resistor internal to the + // IMU then forces the reset line high for normal operation. + m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low + Timer.delay(0.01); // Wait 10ms + m_reset_out.close(); + m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high + Timer.delay(0.25); // Wait 250ms for reset to complete - // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and - // PoP - writeRegister(MSC_CTRL, 1); + if (!switchToStandardSPI()) { + return; + } - // Configure IMU internal Bartlett filter - writeRegister(FILT_CTRL, 0); + // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = + // 400Hz) + writeRegister(DEC_RATE, 4); - // Configure continuous bias calibration time based on user setting - writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); + // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and + // PoP + writeRegister(MSC_CTRL, 1); - // Notify DS that IMU calibration delay is active - DriverStation.reportWarning( - "ADIS16470 IMU Detected. Starting initial calibration delay.", false); + // Configure IMU internal Bartlett filter + writeRegister(FILT_CTRL, 0); - // Wait for samples to accumulate internal to the IMU (110% of user-defined - // time) - try { - Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); - } catch (InterruptedException e) { + // Configure continuous bias calibration time based on user setting + writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); + + // Notify DS that IMU calibration delay is active + DriverStation.reportWarning( + "ADIS16470 IMU Detected. Starting initial calibration delay.", false); + + // Wait for samples to accumulate internal to the IMU (110% of user-defined + // time) + try { + Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); + } catch (InterruptedException e) { + } + + // Write offset calibration command to IMU + writeRegister(GLOB_CMD, 0x0001); + + // Configure and enable auto SPI + if (!switchToAutoSPI()) { + return; + } + + // Let the user know the IMU was initiallized successfully + DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); + + // Drive "Ready" LED low + m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low } - // Write offset calibration command to IMU - writeRegister(GLOB_CMD, 0x0001); - - // Configure and enable auto SPI - if (!switchToAutoSPI()) { - return; - } - - // Let the user know the IMU was initiallized successfully - DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); - - // Drive "Ready" LED low - m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low - // Report usage and post data to DS HAL.report(tResourceType.kResourceType_ADIS16470, 0); } @@ -890,6 +918,23 @@ public class ADIS16470_IMU implements Gyro, NTSendable { /** {@inheritDoc} */ public synchronized double getAngle() { + switch (m_yaw_axis) { + case kX: + if (m_simGyroAngleX != null) { + return m_simGyroAngleX.get(); + } + break; + case kY: + if (m_simGyroAngleY != null) { + return m_simGyroAngleY.get(); + } + break; + case kZ: + if (m_simGyroAngleZ != null) { + return m_simGyroAngleZ.get(); + } + break; + } return m_integ_angle; } @@ -897,11 +942,11 @@ public class ADIS16470_IMU implements Gyro, NTSendable { public synchronized double getRate() { switch (m_yaw_axis) { case kX: - return m_gyro_x; + return getGyroRateX(); case kY: - return m_gyro_y; + return getGyroRateY(); case kZ: - return m_gyro_z; + return getGyroRateZ(); } return 0.0; } @@ -912,17 +957,26 @@ public class ADIS16470_IMU implements Gyro, NTSendable { } /** @return current gyro angle in the X direction */ - public synchronized double getGyroInstantX() { + public synchronized double getGyroRateX() { + if (m_simGyroRateX != null) { + return m_simGyroRateX.get(); + } return m_gyro_x; } /** @return current gyro angle in the Y axis */ - public synchronized double getGyroInstantY() { + public synchronized double getGyroRateY() { + if (m_simGyroRateY != null) { + return m_simGyroRateY.get(); + } return m_gyro_y; } /** @return current gyro angle in the Z axis */ - public synchronized double getGyroInstantZ() { + public synchronized double getGyroRateZ() { + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } return m_gyro_z; } @@ -961,6 +1015,15 @@ public class ADIS16470_IMU implements Gyro, NTSendable { return m_accelAngleY; } + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + public int getPort() { + return m_spi_port.value; + } + @Override public void initSendable(NTSendableBuilder builder) { builder.setSmartDashboardType("Gyro"); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java new file mode 100644 index 0000000000..149f789b59 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.ADIS16448_IMU; + +/** Class to control a simulated ADIS16448 gyroscope. */ +@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"}) +public class ADIS16448_IMUSim { + private final SimDouble m_simGyroAngleX; + private final SimDouble m_simGyroAngleY; + private final SimDouble m_simGyroAngleZ; + private final SimDouble m_simGyroRateX; + private final SimDouble m_simGyroRateY; + private final SimDouble m_simGyroRateZ; + private final SimDouble m_simAccelX; + private final SimDouble m_simAccelY; + private final SimDouble m_simAccelZ; + + /** + * Constructs from an ADIS16448_IMU object. + * + * @param gyro ADIS16448_IMU to simulate + */ + public ADIS16448_IMUSim(ADIS16448_IMU gyro) { + SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16448" + "[" + gyro.getPort() + "]"); + m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x"); + m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y"); + m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z"); + m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x"); + m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y"); + m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z"); + m_simAccelX = wrappedSimDevice.getDouble("accel_x"); + m_simAccelY = wrappedSimDevice.getDouble("accel_y"); + m_simAccelZ = wrappedSimDevice.getDouble("accel_z"); + } + + /** + * Sets the X axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleX(double angleDegrees) { + m_simGyroAngleX.set(angleDegrees); + } + + /** + * Sets the Y axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleY(double angleDegrees) { + m_simGyroAngleY.set(angleDegrees); + } + + /** + * Sets the Z axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleZ(double angleDegrees) { + m_simGyroAngleZ.set(angleDegrees); + } + + /** + * Sets the X axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateX(double rateDegreesPerSecond) { + m_simGyroRateX.set(rateDegreesPerSecond); + } + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateY(double rateDegreesPerSecond) { + m_simGyroRateY.set(rateDegreesPerSecond); + } + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateZ(double rateDegreesPerSecond) { + m_simGyroRateZ.set(rateDegreesPerSecond); + } + + /** + * Sets the X axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelX(double accelMetersPerSecond) { + m_simAccelX.set(accelMetersPerSecond / 9.81); + } + + /** + * Sets the Y axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelY(double accelMetersPerSecond) { + m_simAccelY.set(accelMetersPerSecond / 9.81); + } + + /** + * Sets the Z axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelZ(double accelMetersPerSecond) { + m_simAccelZ.set(accelMetersPerSecond / 9.81); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java new file mode 100644 index 0000000000..1834ff86e4 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.ADIS16470_IMU; + +/** Class to control a simulated ADIS16470 gyroscope. */ +@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"}) +public class ADIS16470_IMUSim { + private final SimDouble m_simGyroAngleX; + private final SimDouble m_simGyroAngleY; + private final SimDouble m_simGyroAngleZ; + private final SimDouble m_simGyroRateX; + private final SimDouble m_simGyroRateY; + private final SimDouble m_simGyroRateZ; + private final SimDouble m_simAccelX; + private final SimDouble m_simAccelY; + private final SimDouble m_simAccelZ; + + /** + * Constructs from an ADIS16470_IMU object. + * + * @param gyro ADIS16470_IMU to simulate + */ + public ADIS16470_IMUSim(ADIS16470_IMU gyro) { + SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]"); + m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x"); + m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y"); + m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z"); + m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x"); + m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y"); + m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z"); + m_simAccelX = wrappedSimDevice.getDouble("accel_x"); + m_simAccelY = wrappedSimDevice.getDouble("accel_y"); + m_simAccelZ = wrappedSimDevice.getDouble("accel_z"); + } + + /** + * Sets the X axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleX(double angleDegrees) { + m_simGyroAngleX.set(angleDegrees); + } + + /** + * Sets the Y axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleY(double angleDegrees) { + m_simGyroAngleY.set(angleDegrees); + } + + /** + * Sets the Z axis angle (CCW positive). + * + * @param angleDegrees The angle. + */ + public void setGyroAngleZ(double angleDegrees) { + m_simGyroAngleZ.set(angleDegrees); + } + + /** + * Sets the X axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateX(double rateDegreesPerSecond) { + m_simGyroRateX.set(rateDegreesPerSecond); + } + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateY(double rateDegreesPerSecond) { + m_simGyroRateY.set(rateDegreesPerSecond); + } + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setGyroRateZ(double rateDegreesPerSecond) { + m_simGyroRateZ.set(rateDegreesPerSecond); + } + + /** + * Sets the X axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelX(double accelMetersPerSecond) { + m_simAccelX.set(accelMetersPerSecond / 9.81); + } + + /** + * Sets the Y axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelY(double accelMetersPerSecond) { + m_simAccelY.set(accelMetersPerSecond / 9.81); + } + + /** + * Sets the Z axis acceleration. + * + * @param accelMetersPerSecond The acceleration. + */ + public void setAccelZ(double accelMetersPerSecond) { + m_simAccelZ.set(accelMetersPerSecond / 9.81); + } +}