Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Use ADXRS450_GyroSim class in simulation example (#2964)

Browse Source 
This class did not exist when the original example was written. This
also changes the C++ example to use ADXRS450_Gyro for the sake of
consistency.
This commit is contained in:
Prateek Machiraju
2020-12-24 18:42:46 -05:00
committed by GitHub
parent 9962f6fd79
commit 4e34f05238
3 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
View File

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