alliance, auto selectors publish booleanEvents when changed

TripleHelixProgramming · Oct 19, 2024 · 018e9c0 · 018e9c0
1 parent bc53932
commit 018e9c0
Show file tree

Hide file tree

Showing 6 changed files with 118 additions and 87 deletions.
diff --git 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
@@ -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
@@ -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<AutoOption> m_autoOptions;
+  private EventLoop m_loop;
 
-  private ChoreoAuto m_autonomous;
-
-  public AutoSelector(int[] ports, AllianceSelector allianceSelector, List<AutoOption> autoOptions) {
+  public AutoSelector(
+      int[] ports, AllianceSelector allianceSelector, List<AutoOption> 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
@@ -2,35 +2,33 @@
 
 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;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 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<AutoOption> 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,25 +94,22 @@ 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
   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
@@ -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
@@ -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 {