diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp index 2cf2f73f84..8ea5b414ea 100644 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2015-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -32,7 +32,7 @@ static constexpr int kPIDRegister = 0x0C; ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {} ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) - : m_spi(port), m_simDevice("ADXRS450_Gyro", port) { + : m_spi(port), m_port(port), m_simDevice("ADXRS450_Gyro", port) { if (m_simDevice) { m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0); m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0); @@ -123,3 +123,5 @@ void ADXRS450_Gyro::Calibrate() { m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage()); m_spi.ResetAccumulator(); } + +int ADXRS450_Gyro::GetPort() const { return m_port; } diff --git a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp new file mode 100644 index 0000000000..74a5180bc1 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/ADXRS450_GyroSim.h" + +#include +#include + +#include "frc/ADXRS450_Gyro.h" +#include "frc/simulation/SimDeviceSim.h" + +using namespace frc::sim; + +ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) { + wpi::SmallString<128> fullname; + wpi::raw_svector_ostream os(fullname); + os << "ADXRS450_Gyro" << '[' << gyro.GetPort() << ']'; + frc::sim::SimDeviceSim deviceSim{fullname.c_str()}; + m_simAngle = deviceSim.GetDouble("Angle"); + m_simRate = deviceSim.GetDouble("Rate"); +} + +void ADXRS450_GyroSim::SetAngle(units::degree_t angle) { + m_simAngle.Set(angle.to()); +} + +void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) { + m_simRate.Set(rate.to()); +} diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h index d88b085ad9..309fab35e3 100644 --- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h +++ b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h @@ -92,8 +92,16 @@ class ADXRS450_Gyro : public GyroBase { */ void Calibrate() override; + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + int GetPort() const; + private: SPI m_spi; + SPI::Port m_port; hal::SimDevice m_simDevice; hal::SimDouble m_simAngle; diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h new file mode 100644 index 0000000000..1d2d2c35c7 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include "frc/geometry/Rotation2d.h" + +namespace frc { + +class ADXRS450_Gyro; + +namespace sim { + +/** + * Class to control a simulated ADXRS450 gyroscope. + */ +class ADXRS450_GyroSim { + public: + /** + * Constructs from a ADXRS450_Gyro object. + * + * @param gyro ADXRS450_Gyro to simulate + */ + explicit ADXRS450_GyroSim(const ADXRS450_Gyro& gyro); + + /** + * Sets the angle. + * + * @param angle The angle (clockwise positive). + */ + void SetAngle(units::degree_t angle); + + /** + * Sets the angular rate (clockwise positive). + * + * @param rate The angular rate. + */ + void SetRate(units::degrees_per_second_t rate); + + private: + hal::SimDouble m_simAngle; + hal::SimDouble m_simRate; +}; + +} // namespace sim +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java index 5e4a271f1f..78ab8c4539 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java @@ -45,6 +45,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable private static final int kSNLowRegister = 0x10; private SPI m_spi; + private SPI.Port m_port; private SimDevice m_simDevice; private SimBoolean m_simConnected; @@ -65,6 +66,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable */ public ADXRS450_Gyro(SPI.Port port) { m_spi = new SPI(port); + m_port = port; // simulation m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value); @@ -129,6 +131,15 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable m_spi.resetAccumulator(); } + /** + * Get the SPI port number. + * + * @return The SPI port number. + */ + public int getPort() { + return m_port.value; + } + private boolean calcParity(int value) { boolean parity = false; while (value != 0) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java new file mode 100644 index 0000000000..950efeff64 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; + +/** + * Class to control a simulated ADXRS450 gyroscope. + */ +@SuppressWarnings("TypeName") +public class ADXRS450_GyroSim { + private final SimDouble m_simAngle; + private final SimDouble m_simRate; + + /** + * Constructs from an ADXRS450_Gyro object. + * + * @param gyro ADXRS450_Gyro to simulate + */ + public ADXRS450_GyroSim(ADXRS450_Gyro gyro) { + SimDeviceSim wrappedSimDevice = new SimDeviceSim("ADXRS450_Gyro" + "[" + gyro.getPort() + "]"); + m_simAngle = wrappedSimDevice.getDouble("Angle"); + m_simRate = wrappedSimDevice.getDouble("Rate"); + } + + /** + * Sets the angle in degrees (clockwise positive). + * + * @param angleDegrees The angle. + */ + public void setAngle(double angleDegrees) { + m_simAngle.set(angleDegrees); + } + + /** + * Sets the angular rate in degrees per second (clockwise positive). + * + * @param rateDegreesPerSecond The angular rate. + */ + public void setRate(double rateDegreesPerSecond) { + m_simRate.set(rateDegreesPerSecond); + } +}