Merge pull request #58 from FRC2713/add-mode-manager

Add mode manager for easy toggle between competition and demo
FRC2713 · Oct 30, 2024 · ed8bcf3 · ed8bcf3
2 parents 01670d6 + 6dcad9e
commit ed8bcf3
Show file tree

Hide file tree

Showing 9 changed files with 99 additions and 7 deletions.
diff --git a/.Glass/glass.json b/.Glass/glass.json
@@ -1,4 +1,18 @@
 {
+  "NetworkTables": {
+    "Retained Values": {
+      "open": false
+    },
+    "Transitory Values": {
+      "open": false
+    },
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Autonomous Routine": "String Chooser",
+      "/SmartDashboard/Mech2d": "Mechanism2d",
+      "/SmartDashboard/Robot Mode": "String Chooser"
+    }
+  },
   "NetworkTables Info": {
     "visible": true
   },

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -209,6 +209,7 @@ public static final class ElevatorConstants {
     public static final double STARTING_HEIGHT_METERS = Units.inchesToMeters(2);
     public static final boolean SIMULATE_GRAVITY = true;
     public static final int ELEVATOR_CURRENT_LIMIT = 30;
+    public static final int DEMO_ELAVATOR_CURRENT_LIMIT = 20;
     public static final double FLOOR_TO_ELEVATOR_BASE_METRES = Units.inchesToMeters(31.25);
   }
 
@@ -242,6 +243,7 @@ public static final class SuperStructure {
 
   public static final class ShooterConstants {
     public static final double GEARING = 1;
+    public static final double MAX_DEMO_RPM = 1000;
     public static final double RADIUS_METERS = Units.inchesToMeters(2);
     public static final double MASS_KG = 0.83461;
     public static final double MOI = 0.0001;

diff --git a/src/main/java/frc/robot/Robot.java b/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,13 @@ public void robotInit() {
               gyroInitial = auto.traj1.getInitialPose().getRotation();
               seedGyroBasedOnAlliance();
             });
+
+    modeChangeDetector =
+        new ChangeDetector<>(
+            (mode) -> {
+              modeManager.setMode(mode);
+              elevator.getIO().setCurrentLimits();
+            });
   }
 
   public void createDriverBindings() {
@@ -989,6 +1006,7 @@ public void disabledPeriodic() {
     swerveDrive.seed();
     allianceChangeDetector.feed(DriverStation.getAlliance());
     autoChangeDetector.feed(autoChooser.get());
+    modeChangeDetector.feed(modeChooser.get());
   }
 
   @Override
@@ -1062,6 +1080,11 @@ public void buildAutoChooser() {
     autoChooser.addOption("NopeAmp", new NopeAmp());
   }
 
+  public void buildModeChooser() {
+    modeChooser.addDefaultOption("Competition", RobotMode.COMPETITION);
+    modeChooser.addOption("Demo", RobotMode.DEMO);
+  }
+
   public void updatePreMatchDashboardValues() {
     var encoderReadings = swerveDrive.getAbsoluteEncoderAngles();
     SmartDashboard.putNumber("Dashboard/Battery Voltage", RobotController.getBatteryVoltage());
@@ -1108,6 +1131,7 @@ public void seedGyroBasedOnAlliance() {
   public void driverStationConnected() {
 
     buildAutoChooser();
+    buildModeChooser();
     if (autoChooser.get() != null) {
       gyroInitial = autoChooser.get().traj1.getInitialPose().getRotation();
     }

diff --git a/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIO.java
@@ -27,4 +27,6 @@ public static class ElevatorInputs {
   public void setVoltage(double volts);
 
   public void setTargetHeight(double heightInches);
+
+  public void setCurrentLimits();
 }
diff --git a/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSim.java
@@ -68,4 +68,10 @@ public void setVoltage(double volts) {
   public void setTargetHeight(double heightInches) {
     heightControllerRight.setSetpoint(heightInches);
   }
+
+  @Override
+  public void setCurrentLimits() {
+    // TODO
+    return;
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSparks.java b/src/main/java/frc/robot/subsystems/elevatorIO/ElevatorIOSparks.java
@@ -9,6 +9,8 @@
 import edu.wpi.first.wpilibj.RobotController;
 import frc.robot.Constants;
 import frc.robot.Constants.ElevatorConstants;
+import frc.robot.Robot;
+import frc.robot.Robot.RobotMode;
 
 public class ElevatorIOSparks implements ElevatorIO {
   private CANSparkMax left, right;
@@ -22,9 +24,7 @@ public ElevatorIOSparks() {
 
     left.setIdleMode(IdleMode.kBrake);
     right.setIdleMode(IdleMode.kBrake);
-
-    left.setSmartCurrentLimit(Constants.ElevatorConstants.ELEVATOR_CURRENT_LIMIT);
-    right.setSmartCurrentLimit(Constants.ElevatorConstants.ELEVATOR_CURRENT_LIMIT);
+    this.setCurrentLimits();
 
     left.getEncoder().setPositionConversionFactor(1 / 20.0 * Math.PI * 1.7567);
     right.getEncoder().setPositionConversionFactor(1 / 20.0 * Math.PI * 1.7567);
@@ -95,4 +95,15 @@ public void setTargetHeight(double heightInches) {
             ElevatorConstants.ELEVATOR_GAINS.getKG(),
             ArbFFUnits.kVoltage);
   }
+
+  @Override
+  public void setCurrentLimits() {
+    if (Robot.modeManager.getMode() == RobotMode.DEMO) {
+      left.setSmartCurrentLimit(Constants.ElevatorConstants.DEMO_ELAVATOR_CURRENT_LIMIT);
+      right.setSmartCurrentLimit(Constants.ElevatorConstants.DEMO_ELAVATOR_CURRENT_LIMIT);
+    } else {
+      left.setSmartCurrentLimit(Constants.ElevatorConstants.ELEVATOR_CURRENT_LIMIT);
+      right.setSmartCurrentLimit(Constants.ElevatorConstants.ELEVATOR_CURRENT_LIMIT);
+    }
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java b/src/main/java/frc/robot/subsystems/shooterIO/Shooter.java
@@ -3,7 +3,9 @@
 import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
 import frc.robot.Robot;
+import frc.robot.Robot.RobotMode;
 import frc.robot.commands.otf.RotateScore;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
@@ -159,9 +161,20 @@ public void periodic() {
    * @param differentialRpm
    */
   private void setShooterRpms(double leftRPM, double rightRPM, double differentialRpm) {
-    IO.setMotorSetPoint(
-        shooterState.leftRpm.getAsDouble() + differentialRpm,
-        shooterState.rightRpm.getAsDouble() - differentialRpm);
+    double final_value_left = shooterState.leftRpm.getAsDouble() + differentialRpm;
+    double final_value_right = shooterState.rightRpm.getAsDouble() - differentialRpm;
+
+    if (Robot.modeManager.getMode() == RobotMode.DEMO) {
+      final_value_left =
+          Math.min(
+              final_value_left, Constants.ShooterConstants.MAX_DEMO_RPM); // sas its a max of 100
+      final_value_right = Math.min(final_value_right, Constants.ShooterConstants.MAX_DEMO_RPM);
+      final_value_left =
+          Math.max(final_value_left, -Constants.ShooterConstants.MAX_DEMO_RPM); // says min of -100
+      final_value_right = Math.max(final_value_right, -Constants.ShooterConstants.MAX_DEMO_RPM);
+    }
+
+    IO.setMotorSetPoint(final_value_left, final_value_right);
   }
 
   @AutoLogOutput(key = "Shooter/isAtTarget")

diff --git a/src/main/java/frc/robot/util/ModeManager.java b/src/main/java/frc/robot/util/ModeManager.java
@@ -0,0 +1,19 @@
+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;
+  }
+}
diff --git a/src/main/java/frc/robot/util/MotionHandler.java b/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.2 : 1;
 
     double xSpeed =
         MathUtil.applyDeadband(-Robot.driver.getLeftY(), DriveConstants.K_JOYSTICK_TURN_DEADZONE)