Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add simulation support to ADIS classes (#3857)

Browse Source
This commit is contained in:
Tyler Veness
2022-01-03 11:44:12 -08:00
committed by GitHub
parent c137569f91
commit 831052f118
12 changed files with 1077 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View File

@@ -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<double>(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<double>(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.
*

164
wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp
View File

@@ -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<uint16_t>(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<uint16_t>(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.
*

59
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp Normal file
View File

@@ -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 <frc/ADIS16448_IMU.h>
#include <frc/simulation/SimDeviceSim.h>
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);
}

59
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp Normal file
View File

@@ -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 <frc/ADIS16470_IMU.h>
#include <frc/simulation/SimDeviceSim.h>
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);
}

36
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
View File

@@ -23,16 +23,12 @@
#include <memory>
#include <thread>
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
// 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;

31
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View File

@@ -23,6 +23,7 @@
#include <memory>
#include <thread>
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
@@ -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;

106
wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h Normal file
View File

@@ -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 <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
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

106
wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h Normal file
View File

@@ -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 <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
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

167
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

@@ -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");

163
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

@@ -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");

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java Normal file
View File

@@ -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);
}
}

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java Normal file
View File

@@ -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);
}
}