From e3be887eab5ef4776a535fe5056bde02783dc76e Mon Sep 17 00:00:00 2001 From: Benjamin Hall Date: Tue, 22 Oct 2024 11:51:05 -0400 Subject: [PATCH] More SwerveWithPathPlanner cleanup --- .../src/main/java/frc/robot/Robot.java | 4 +-- .../main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/CommandSwerveDrivetrain.java | 32 +++++++++++-------- 3 files changed, 22 insertions(+), 17 deletions(-) diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java index 96d3744d..ec5b5c57 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/Robot.java @@ -15,7 +15,7 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; - private final boolean UseLimelight = false; + private final boolean kUseLimelight = false; public Robot() { m_robotContainer = new RobotContainer(); @@ -33,7 +33,7 @@ public void robotPeriodic() { * This example is sufficient to show that vision integration is possible, though exact implementation * of how to use vision should be tuned per-robot and to the team's specification. */ - if (UseLimelight) { + if (kUseLimelight) { var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight"); if (llMeasurement != null) { m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds)); diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java index 88e250c1..d1606a08 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/RobotContainer.java @@ -35,7 +35,8 @@ public class RobotContainer { .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /* Path follower */ private Command runAuto = new PathPlannerAuto("Tests"); diff --git a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 1cc69797..140ed53f 100644 --- a/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/java/SwerveWithPathPlanner/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -45,7 +45,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst private boolean m_hasAppliedOperatorPerspective = false; /* Swerve request to apply during path following */ - private final SwerveRequest.ApplyChassisSpeeds m_applySpeeds = new SwerveRequest.ApplyChassisSpeeds(); + private final SwerveRequest.ApplyChassisSpeeds m_applyRobotSpeeds = new SwerveRequest.ApplyChassisSpeeds(); /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); @@ -126,10 +126,10 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst */ public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { super(drivetrainConstants, modules); - configurePathPlanner(); if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } /** @@ -147,10 +147,10 @@ public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, Sw */ public CommandSwerveDrivetrain(SwerveDrivetrainConstants drivetrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) { super(drivetrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } /** @@ -173,13 +173,13 @@ public CommandSwerveDrivetrain( Matrix odometryStandardDeviation, Matrix visionStandardDeviation, SwerveModuleConstants... modules) { super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - configurePathPlanner(); if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } - private void configurePathPlanner() { + private void configureAutoBuilder() { double driveBaseRadius = 0; for (var moduleLocation : m_moduleLocations) { driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); @@ -193,7 +193,7 @@ private void configurePathPlanner() { () -> this.getState().Speeds, // Supplier of current robot speeds // Consumer of ChassisSpeeds and feedforwards to drive the robot (speeds, feedforwards) -> this.setControl( - m_applySpeeds.withSpeeds(speeds) + m_applyRobotSpeeds.withSpeeds(speeds) .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) ), @@ -242,16 +242,20 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { @Override public void periodic() { - /* Periodically try to apply the operator perspective */ - /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ - /* This allows us to correct the perspective in case the robot code restarts mid-match */ - /* Otherwise, only check and apply the operator perspective if the DS is disabled */ - /* This ensures driving behavior doesn't change until an explicit disable event occurs during testing*/ + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent((allianceColor) -> { + DriverStation.getAlliance().ifPresent(allianceColor -> { setOperatorPerspectiveForward( - allianceColor == Alliance.Red ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); + allianceColor == Alliance.Red + ? kRedAlliancePerspectiveRotation + : kBlueAlliancePerspectiveRotation + ); m_hasAppliedOperatorPerspective = true; }); }