diff --git a/src/main/java/frc/lib/AllianceSelector.java b/src/main/java/frc/lib/AllianceSelector.java index 59c2607..4ae2b3f 100644 --- a/src/main/java/frc/lib/AllianceSelector.java +++ b/src/main/java/frc/lib/AllianceSelector.java @@ -1,30 +1,43 @@ package frc.lib; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.AutoConstants.AllianceColor; -import java.util.function.BooleanSupplier; - -public class AllianceSelector extends SubsystemBase{ +public class AllianceSelector { + private AllianceColor m_currentColor; private DigitalInput m_allianceSelectionSwitch; - + private EventLoop m_loop; + public AllianceSelector(int port) { this.m_allianceSelectionSwitch = new DigitalInput(port); } - public BooleanSupplier fieldRotatedSupplier() { - return () -> m_allianceSelectionSwitch.get(); + private boolean updateAlliance() { + AllianceColor m_newColor = + m_allianceSelectionSwitch.get() ? AllianceColor.Red : AllianceColor.Blue; + if (m_newColor == m_currentColor) return false; + else { + m_currentColor = m_newColor; + return true; + } + } + + public BooleanEvent changedAlliance = new BooleanEvent(m_loop, () -> updateAlliance()); + + public boolean fieldRotated() { + return m_currentColor.equals(AllianceColor.Red); } public AllianceColor getAllianceColor() { - return m_allianceSelectionSwitch.get() ? AllianceColor.Red : AllianceColor.Blue; + return m_currentColor; } - @Override - public void periodic() { + public void disabledPeriodic() { + m_loop.poll(); SmartDashboard.putString("Alliance Color", getAllianceColor().name()); } } diff --git a/src/main/java/frc/lib/AutoOption.java b/src/main/java/frc/lib/AutoOption.java new file mode 100644 index 0000000..e6897de --- /dev/null +++ b/src/main/java/frc/lib/AutoOption.java @@ -0,0 +1,34 @@ +package frc.lib; + +import frc.robot.Constants.AutoConstants.AllianceColor; +import frc.robot.autos.ChoreoAuto; + +public class AutoOption { + private AllianceColor m_color; + private int m_option; + private ChoreoAuto m_auto; + + public AutoOption(AllianceColor color, int option, ChoreoAuto auto) { + this.m_color = color; + this.m_option = option; + this.m_auto = auto; + } + + public AutoOption(AllianceColor color, int option) { + this.m_color = color; + this.m_option = option; + this.m_auto = null; + } + + public AllianceColor getColor() { + return this.m_color; + } + + public int getOption() { + return this.m_option; + } + + public ChoreoAuto getChoreoAuto() { + return this.m_auto; + } +} diff --git a/src/main/java/frc/lib/AutoSelector.java b/src/main/java/frc/lib/AutoSelector.java index 28cd0de..565d464 100644 --- a/src/main/java/frc/lib/AutoSelector.java +++ b/src/main/java/frc/lib/AutoSelector.java @@ -1,36 +1,33 @@ package frc.lib; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.event.BooleanEvent; +import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.AutoConstants.AllianceColor; -import frc.robot.Robot.AutoOption; import frc.robot.autos.ChoreoAuto; - import java.util.List; public class AutoSelector { + private ChoreoAuto m_currentAuto; private DigitalInput[] m_switchPositions; private AllianceSelector m_allianceSelector; private List m_autoOptions; + private EventLoop m_loop; - private ChoreoAuto m_autonomous; - - public AutoSelector(int[] ports, AllianceSelector allianceSelector, List autoOptions) { + public AutoSelector( + int[] ports, AllianceSelector allianceSelector, List autoOptions) { + this.m_allianceSelector = allianceSelector; + this.m_autoOptions = autoOptions; m_switchPositions = new DigitalInput[ports.length]; + for (int i = 0; i < ports.length; i++) { m_switchPositions[i] = new DigitalInput(ports[i]); } - - this.m_allianceSelector = allianceSelector; - this.m_autoOptions = autoOptions; } - /** - * @return Index in array of Digital Inputs corresponding to selected auto mode - */ private int getSwitchPosition() { for (int i = 0; i < m_switchPositions.length; i++) { if (!m_switchPositions[i].get()) { @@ -39,33 +36,46 @@ private int getSwitchPosition() { } return 0; // failure of the physical switch } - - public ChoreoAuto getSelectedAutonomous() { + + private ChoreoAuto findMatchingOption() { int switchPosition = getSwitchPosition(); AllianceColor color = m_allianceSelector.getAllianceColor(); for (int i = 0; i < m_autoOptions.size(); i++) { if (m_autoOptions.get(i).getColor() == color) if (m_autoOptions.get(i).getOption() == switchPosition) { - return m_autoOptions.get(i).getChoreoAuto(); - } + return m_autoOptions.get(i).getChoreoAuto(); + } + } + return null; + } + + private boolean updateAuto() { + ChoreoAuto m_newAuto = findMatchingOption(); + if (m_newAuto == m_currentAuto) return false; + else { + m_currentAuto = m_newAuto; + return true; } - return null; } - /** - * @return The Command that runs the selected autonomous mode - */ - public Command getAutonomousCommand() { - getSelectedAutonomous(); - if (m_autonomous != null) return m_autonomous; - else return null; + public BooleanEvent changedAuto = new BooleanEvent(m_loop, () -> updateAuto()); + + public void scheduleAuto() { + if (m_currentAuto != null) m_currentAuto.schedule(); + } + + public void cancelAuto() { + if (m_currentAuto != null) m_currentAuto.cancel(); } - public void periodic() { + public void disabledPeriodic() { + m_loop.poll(); + + m_allianceSelector.changedAlliance.ifHigh(() -> updateAuto()); - if (m_autonomous != null) { - SmartDashboard.putString("Auto", m_autonomous.getName()); + if (m_currentAuto != null) { + SmartDashboard.putString("Auto", m_currentAuto.getName()); } else { SmartDashboard.putString("Auto", "Null"); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 103a2b0..b8e343e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,8 +2,6 @@ package frc.robot; -import java.util.List; - import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; @@ -11,26 +9,26 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.lib.AllianceSelector; +import frc.lib.AutoOption; import frc.lib.AutoSelector; import frc.lib.ControllerPatroller; import frc.lib.SendableZorroController; -import frc.robot.Constants.AutoConstants.AllianceColor; import frc.robot.Constants.AutoConstants; +import frc.robot.Constants.AutoConstants.AllianceColor; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.OIConstants; import frc.robot.Constants.OIConstants.Zorro; -import frc.robot.autos.ChoreoAuto; +import frc.robot.autos.ExampleAuto; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.commands.ZorroDriveCommand; +import java.util.List; public class Robot extends TimedRobot { - private Command m_autonomousCommand; private List m_autoOptions; private final PowerDistribution m_powerDistribution = new PowerDistribution(1, ModuleType.kRev); @@ -49,9 +47,11 @@ public Robot() { configureAutoOptions(); m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort); - m_autoSelector = new AutoSelector(AutoConstants.kAutonomousModeSelectorPorts, m_allianceSelector, m_autoOptions); + m_autoSelector = + new AutoSelector( + AutoConstants.kAutonomousModeSelectorPorts, m_allianceSelector, m_autoOptions); - m_swerve = new Drivetrain(m_allianceSelector.fieldRotatedSupplier()); + m_swerve = new Drivetrain(m_allianceSelector::fieldRotated); // Create a button on Smart Dashboard to reset the encoders. SmartDashboard.putData( @@ -94,15 +94,14 @@ public void disabledPeriodic() { configureButtonBindings(); } } + + m_allianceSelector.disabledPeriodic(); + m_autoSelector.disabledPeriodic(); } @Override public void autonomousInit() { - m_autonomousCommand = m_autoSelector.getAutonomousCommand(); - - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } + m_autoSelector.scheduleAuto(); } @Override @@ -110,9 +109,7 @@ public void autonomousPeriodic() {} @Override public void teleopInit() { - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + m_autoSelector.cancelAuto(); } @Override @@ -161,40 +158,10 @@ private void configureDriverButtonBindings() { private void configureOperatorButtonBindings() {} private void configureAutoOptions() { - m_autoOptions.add(new AutoOption(AllianceColor.Red, 1)); + m_autoOptions.add(new AutoOption(AllianceColor.Red, 1, new ExampleAuto())); m_autoOptions.add(new AutoOption(AllianceColor.Blue, 1)); } - public class AutoOption { - private AllianceColor m_color; - private int m_option; - private ChoreoAuto m_auto; - - public AutoOption(AllianceColor color, int option, ChoreoAuto auto) { - this.m_color = color; - this.m_option = option; - this.m_auto = auto; - } - - public AutoOption(AllianceColor color, int option) { - this.m_color = color; - this.m_option = option; - this.m_auto = null; - } - - public AllianceColor getColor() { - return this.m_color; - } - - public int getOption() { - return this.m_option; - } - - public ChoreoAuto getChoreoAuto() { - return this.m_auto; - } - } - /** * Gets the current drawn from the Power Distribution Hub by a CAN motor controller, assuming that * (PDH port number + 10) = CAN ID diff --git a/src/main/java/frc/robot/autos/ExampleAuto.java b/src/main/java/frc/robot/autos/ExampleAuto.java new file mode 100644 index 0000000..d2886a2 --- /dev/null +++ b/src/main/java/frc/robot/autos/ExampleAuto.java @@ -0,0 +1,9 @@ +package frc.robot.autos; + +public class ExampleAuto extends ChoreoAuto { + + @Override + public String getName() { + return "Example Auto"; + } +} diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index cb88049..b4b8383 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -3,9 +3,6 @@ package frc.robot.drivetrain; import choreo.trajectory.SwerveSample; - -import java.util.function.BooleanSupplier; - import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -25,6 +22,7 @@ import frc.robot.Constants.AutoConstants.TranslationControllerGains; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotConstants; +import java.util.function.BooleanSupplier; /** Constructs a swerve drive style drivetrain. */ public class Drivetrain extends SubsystemBase {