[wpilib] Document simulation APIs (#3079)

- Remove sim checkstyle suppression
- Add [[nodiscard]] to C++ register callback functions
- Add a couple of missing sim functions

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: Starlight220 <yotamshlomi@gmail.com>
This commit is contained in:
Peter Johnson
2021-01-11 21:55:45 -08:00
committed by GitHub
parent 26584ff145
commit 9c3b51ca0f
64 changed files with 4516 additions and 199 deletions

View File

@@ -75,6 +75,7 @@ public class Drivetrain {
new DifferentialDrivetrainSim(
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
/** Subsystem constructor. */
public Drivetrain() {
// Set the distance per pulse for the drive encoders. We can simply use the
// distance traveled for one rotation of the wheel divided by the encoder
@@ -89,6 +90,7 @@ public class Drivetrain {
SmartDashboard.putData("Field", m_fieldSim);
}
/** Sets speeds to the drivetrain motors. */
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
@@ -101,15 +103,24 @@ public class Drivetrain {
m_rightGroup.setVoltage(rightOutput + rightFeedforward);
}
/**
* Controls the robot using arcade drive.
*
* @param xSpeed the speed for the x axis
* @param rot the rotation
*/
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double rot) {
setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
}
/** Update robot odometry. */
public void updateOdometry() {
m_odometry.update(
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
}
/** Resets robot odometry. */
public void resetOdometry(Pose2d pose) {
m_leftEncoder.reset();
m_rightEncoder.reset();
@@ -117,10 +128,12 @@ public class Drivetrain {
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
}
/** Check the current robot pose. */
public Pose2d getPose() {
return m_odometry.getPoseMeters();
}
/** Update our simulation. This should be run every robot loop in simulation. */
public void simulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated positions and velocities to our
@@ -138,6 +151,7 @@ public class Drivetrain {
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
}
/** Update odometry - this should be run every robot loop. */
public void periodic() {
updateOdometry();
m_fieldSim.setRobotPose(m_odometry.getPoseMeters());

View File

@@ -8,6 +8,7 @@ import static edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;

View File

@@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.RobotController;
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.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
@@ -28,14 +28,14 @@ public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
private final SpeedControllerGroup m_leftMotors =
new SpeedControllerGroup(
new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port),
new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port));
new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
// The motors on the right side of the drive.
private final SpeedControllerGroup m_rightMotors =
new SpeedControllerGroup(
new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port),
new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port));
new PWMVictorSPX(DriveConstants.kRightMotor1Port),
new PWMVictorSPX(DriveConstants.kRightMotor2Port));
// The robot's drive
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
@@ -43,16 +43,16 @@ public class DriveSubsystem extends SubsystemBase {
// The left-side drive encoder
private final Encoder m_leftEncoder =
new Encoder(
Constants.DriveConstants.kLeftEncoderPorts[0],
Constants.DriveConstants.kLeftEncoderPorts[1],
Constants.DriveConstants.kLeftEncoderReversed);
DriveConstants.kLeftEncoderPorts[0],
DriveConstants.kLeftEncoderPorts[1],
DriveConstants.kLeftEncoderReversed);
// The right-side drive encoder
private final Encoder m_rightEncoder =
new Encoder(
Constants.DriveConstants.kRightEncoderPorts[0],
Constants.DriveConstants.kRightEncoderPorts[1],
Constants.DriveConstants.kRightEncoderReversed);
DriveConstants.kRightEncoderPorts[0],
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
@@ -71,8 +71,8 @@ public class DriveSubsystem extends SubsystemBase {
/** Creates a new DriveSubsystem. */
public DriveSubsystem() {
// Sets the distance per pulse for the encoders
m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
resetEncoders();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
@@ -81,11 +81,11 @@ public class DriveSubsystem extends SubsystemBase {
// This class simulates our drivetrain's motion around the field.
m_drivetrainSimulator =
new DifferentialDrivetrainSim(
Constants.DriveConstants.kDrivetrainPlant,
Constants.DriveConstants.kDriveGearbox,
Constants.DriveConstants.kDriveGearing,
Constants.DriveConstants.kTrackwidthMeters,
Constants.DriveConstants.kWheelDiameterMeters / 2.0,
DriveConstants.kDrivetrainPlant,
DriveConstants.kDriveGearbox,
DriveConstants.kDriveGearing,
DriveConstants.kTrackwidthMeters,
DriveConstants.kWheelDiameterMeters / 2.0,
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
// The encoder and gyro angle sims let us set simulated sensor readings
@@ -246,7 +246,6 @@ public class DriveSubsystem extends SubsystemBase {
* @return the robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(m_gyro.getAngle(), 360)
* (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0);
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
}