diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp index 1835100f43..550d19469a 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -52,8 +52,7 @@ void DriveSubsystem::SimulationPeriodic() { m_drivetrainSimulator.GetRightPosition().to()); m_rightEncoderSim.SetRate( m_drivetrainSimulator.GetRightVelocity().to()); - m_gyroAngleSim.SetAngle( - -m_drivetrainSimulator.GetHeading().Degrees().to()); + m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees()); } units::ampere_t DriveSubsystem::GetCurrentDraw() const { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h index fec15e6996..43289e3d2f 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h @@ -7,14 +7,14 @@ #pragma once -#include +#include #include #include #include #include #include #include -#include +#include #include #include #include @@ -154,7 +154,7 @@ class DriveSubsystem : public frc2::SubsystemBase { DriveConstants::kRightEncoderPorts[1]}; // The gyro sensor - frc::AnalogGyro m_gyro{0}; + frc::ADXRS450_Gyro m_gyro; // Odometry class for tracking robot pose frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()}; @@ -170,7 +170,7 @@ class DriveSubsystem : public frc2::SubsystemBase { frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; - frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro}; + frc::sim::ADXRS450_GyroSim m_gyroSim{m_gyro}; // The Field2d class shows the field in the sim GUI. frc::Field2d m_fieldSim; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java index d014ca35ae..7af0b125e4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -7,24 +7,21 @@ package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems; -import edu.wpi.first.hal.SimDouble; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.simulation.EncoderSim; -import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -57,7 +54,7 @@ public class DriveSubsystem extends SubsystemBase { Constants.DriveConstants.kRightEncoderReversed); // The gyro sensor - private final Gyro m_gyro = new ADXRS450_Gyro(); + private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro(); // Odometry class for tracking robot pose private final DifferentialDriveOdometry m_odometry; @@ -68,7 +65,7 @@ public class DriveSubsystem extends SubsystemBase { private EncoderSim m_rightEncoderSim; // The Field2d class shows the field in the sim GUI private Field2d m_fieldSim; - private SimDouble m_gyroAngleSim; + private ADXRS450_GyroSim m_gyroSim; /** * Creates a new DriveSubsystem. @@ -94,9 +91,7 @@ public class DriveSubsystem extends SubsystemBase { // The encoder and gyro angle sims let us set simulated sensor readings m_leftEncoderSim = new EncoderSim(m_leftEncoder); m_rightEncoderSim = new EncoderSim(m_rightEncoder); - m_gyroAngleSim = - new SimDeviceSim("ADXRS450_Gyro" + "[" + SPI.Port.kOnboardCS0.value + "]") - .getDouble("Angle"); + m_gyroSim = new ADXRS450_GyroSim(m_gyro); // the Field2d class lets us visualize our robot in the simulation GUI. m_fieldSim = new Field2d(); @@ -126,7 +121,7 @@ public class DriveSubsystem extends SubsystemBase { m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); - m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees()); + m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); } /**