Add mode manager: demo and competition

Author: luanzeba

src/main/java/frc/robot/Robot.java

@@ -67,6 +67,7 @@
 import frc.robot.subsystems.visionIO.VisionIOSim;
 import frc.robot.util.ChangeDetector;
 import frc.robot.util.MechanismManager;
+import frc.robot.util.ModeManager;
 import frc.robot.util.RedHawkUtil;
 import frc.robot.util.RumbleManager;
 import frc.robot.util.SwerveHeadingController;
@@ -104,10 +105,19 @@ public class Robot extends LoggedRobot {
   private Command autoCommand;
   private final LoggedDashboardChooser<RHRFullRoutine> autoChooser =
       new LoggedDashboardChooser<>("Autonomous Routine");
+  private final LoggedDashboardChooser<RobotMode> modeChooser =
+    new LoggedDashboardChooser<>("Robot Mode");
 
   private ChangeDetector<Optional<Alliance>> allianceChangeDetector;
   private ChangeDetector<RHRFullRoutine> autoChangeDetector;
+  private ChangeDetector<RobotMode> modeChangeDetector;
   private Rotation2d gyroInitial = Rotation2d.fromRadians(0);
+  public static final ModeManager modeManager = new ModeManager(RobotMode.DEMO);
+
+  public enum RobotMode {
+    DEMO,
+    COMPETITION
+  }
 
   @Override
   public void robotInit() {
@@ -199,6 +209,12 @@ public void robotInit() {
               gyroInitial = auto.traj1.getInitialPose().getRotation();
               seedGyroBasedOnAlliance();
             });
+
+    modeChangeDetector =
+        new ChangeDetector<>(
+            (mode) -> {
+              modeManager.setMode(mode);
+            });
   }
 
   public void createDriverBindings() {
@@ -989,6 +1005,7 @@ public void disabledPeriodic() {
     swerveDrive.seed();
     allianceChangeDetector.feed(DriverStation.getAlliance());
     autoChangeDetector.feed(autoChooser.get());
+    modeChangeDetector.feed(modeChooser.get());
   }
 
   @Override
@@ -1062,6 +1079,12 @@ public void buildAutoChooser() {
     autoChooser.addOption("NopeAmp", new NopeAmp());
   }
 
+  public void buildModeChooser() {
+    modeChooser.addDefaultOption(modeManager.getMode().toString(), Robot.modeManager.getMode());
+    modeChooser.addOption("Demo", RobotMode.DEMO);
+    modeChooser.addOption("Competition", RobotMode.COMPETITION);
+  }
+
   public void updatePreMatchDashboardValues() {
     var encoderReadings = swerveDrive.getAbsoluteEncoderAngles();
     SmartDashboard.putNumber("Dashboard/Battery Voltage", RobotController.getBatteryVoltage());

src/main/java/frc/robot/util/ModeManager.java

@@ -0,0 +1,21 @@
+package frc.robot.util;
+
+import frc.robot.Robot.RobotMode;
+
+public class ModeManager 
+{
+    private RobotMode mode;
+    
+    public ModeManager(RobotMode initialMode) {
+        this.mode = initialMode;
+    }
+    
+    public RobotMode getMode() {
+        return this.mode;
+    }
+
+    public void setMode(RobotMode newMode) {
+        this.mode = newMode;
+    }
+}
+

src/main/java/frc/robot/util/MotionHandler.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Robot;
+import frc.robot.Robot.RobotMode;
 import frc.robot.VehicleState;
 import frc.robot.commands.otf.RotateScore;
 import frc.robot.rhr.auto.RHRTrajectoryController;
@@ -52,7 +53,7 @@ public static ChassisSpeeds driveTrajectoryHeadingController(ChassisSpeeds cs) {
    * @return The desired array of desaturated swerveModuleStates.
    */
   public static ChassisSpeeds driveFullControl() {
-    double speedFactor = 1; // Robot.driver.rightBumper().getAsBoolean() ? 0.33 : 1.0;
+    double speedFactor = Robot.modeManager.getMode() == RobotMode.DEMO ? 0.5 : 1; 
 
     double xSpeed =
         MathUtil.applyDeadband(-Robot.driver.getLeftY(), DriveConstants.K_JOYSTICK_TURN_DEADZONE)