From 1ec335c0ae1ad2cfa3ed22d83eedf373c53fe488 Mon Sep 17 00:00:00 2001 From: Nate Laverdure <41549690+nlaverdure@users.noreply.github.com> Date: Sat, 3 Feb 2024 18:41:13 -0500 Subject: [PATCH] Additional simplification downstream of managing allianceColor within the drivetrain (#78) * simplify logic and remove allianceColor enum * spotless * remove commented lines --- src/main/java/frc/robot/Constants.java | 15 ++++------ src/main/java/frc/robot/RobotContainer.java | 29 +++++++------------ .../java/frc/robot/drivetrain/Drivetrain.java | 20 +++++-------- 3 files changed, 22 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bcc68e2..4443667 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,16 +9,6 @@ public final class Constants { - public static enum AllianceColor { - RED_ALLIANCE, - BLUE_ALLIANCE - } - - public static final int dioAllianceSwitchPort = 10; - - // length is 8 - public static final int[] dioPortNumbers = {11, 12, 13, 18, 19, 20, 21, 22}; - public static final class RobotConstants { public static final double kNominalVoltage = 12.0; public static final double kPeriod = TimedRobot.kDefaultPeriod; @@ -195,6 +185,11 @@ public static final class ArmConstants { } public static final class AutoConstants { + public static final int kAllianceColorSelectorPort = 10; + + // length is 8 + public static final int[] kAutonomousModeSelectorPorts = {11, 12, 13, 18, 19, 20, 21, 22}; + public static final double kMaxSpeedMetersPerSecond = 3.0; public static final double kMaxAccelerationMetersPerSecondSquared = 3.0; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5d51c2..b68189c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.Constants.AllianceColor; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.OIConstants; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.commands.ZorroDrive; @@ -25,19 +25,17 @@ public class RobotContainer { // private XboxController m_operator = new XboxController(OIConstants.kOperatorControllerPort); // digital inputs for autonomous selection - private final DigitalInput[] autonomousModes = new DigitalInput[Constants.dioPortNumbers.length]; - private final DigitalInput allianceSelectionSwitch; + private final DigitalInput[] autonomousModes = + new DigitalInput[AutoConstants.kAutonomousModeSelectorPorts.length]; private Autonomous m_autonomous; public RobotContainer() { - for (int i = 0; i < Constants.dioPortNumbers.length; i++) { - autonomousModes[i] = new DigitalInput(Constants.dioPortNumbers[i]); + for (int i = 0; i < AutoConstants.kAutonomousModeSelectorPorts.length; i++) { + autonomousModes[i] = new DigitalInput(AutoConstants.kAutonomousModeSelectorPorts[i]); } - allianceSelectionSwitch = new DigitalInput(Constants.dioAllianceSwitchPort); - m_swerve.setDefaultCommand(new ZorroDrive(m_swerve, m_driver)); m_swerve.configurePathPlanner(); @@ -101,7 +99,7 @@ private class Autonomous { private final String filename; - private Autonomous(String filename, AllianceColor alliance) { + private Autonomous(String filename) { this.filename = filename; } @@ -114,28 +112,21 @@ private String getFilename() { } } - private void updateAllianceColor() { - m_swerve.setAlliance( - allianceSelectionSwitch.get() ? AllianceColor.RED_ALLIANCE : AllianceColor.BLUE_ALLIANCE); - } - /** Updates the autonomous based on the physical selector switch */ private void updateSelectedAutonomous() { - updateAllianceColor(); - switch (getSelectedAutonomousMode()) { case 0: m_autonomous = m_swerve.getRedAlliance() - ? new Autonomous("R-driveFwd2m", AllianceColor.RED_ALLIANCE) - : new Autonomous("B-driveFwd2m", AllianceColor.BLUE_ALLIANCE); + ? new Autonomous("R-driveFwd2m") + : new Autonomous("B-driveFwd2m"); break; case 1: m_autonomous = m_swerve.getRedAlliance() - ? new Autonomous("R-driveFwd2m", AllianceColor.RED_ALLIANCE) - : new Autonomous("B_SpinForward", AllianceColor.BLUE_ALLIANCE); + ? new Autonomous("R-driveFwd2m") + : new Autonomous("B_SpinForward"); break; case 2: diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 09bf166..0396520 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -14,11 +14,12 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Constants.AllianceColor; +import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotConstants; @@ -29,8 +30,6 @@ public class Drivetrain extends SubsystemBase { private final SwerveDriveKinematics m_kinematics = DriveConstants.kDriveKinematics; - private AllianceColor m_alliance; - private final SwerveModule m_frontLeft = new SwerveModule( "FrontLeft", @@ -73,6 +72,9 @@ public class Drivetrain extends SubsystemBase { private final Field2d m_field = new Field2d(); + private final DigitalInput allianceSelectionSwitch = + new DigitalInput(AutoConstants.kAllianceColorSelectorPort); + public Drivetrain() { m_gyro.reset(); @@ -242,19 +244,11 @@ public void configurePathPlanner() { ); } - public void setAlliance(AllianceColor alliance) { - this.m_alliance = alliance; - } - - public AllianceColor getAlliance() { - return this.m_alliance; - } - public boolean getFieldRotated() { - return m_alliance == AllianceColor.RED_ALLIANCE; + return allianceSelectionSwitch.get(); } public boolean getRedAlliance() { - return m_alliance == AllianceColor.RED_ALLIANCE; + return allianceSelectionSwitch.get(); } }