Skip to content

Commit

Permalink
Additional simplification downstream of managing allianceColor within…
Browse files Browse the repository at this point in the history
… the drivetrain (#78)

* simplify logic and remove allianceColor enum

* spotless

* remove commented lines
  • Loading branch information
nlaverdure authored Feb 3, 2024
1 parent c53d743 commit 1ec335c
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 42 deletions.
15 changes: 5 additions & 10 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
29 changes: 10 additions & 19 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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();

Expand Down Expand Up @@ -101,7 +99,7 @@ private class Autonomous {

private final String filename;

private Autonomous(String filename, AllianceColor alliance) {
private Autonomous(String filename) {
this.filename = filename;
}

Expand All @@ -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:
Expand Down
20 changes: 7 additions & 13 deletions src/main/java/frc/robot/drivetrain/Drivetrain.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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",
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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();
}
}

0 comments on commit 1ec335c

Please sign in to comment.