cleanup and add toggleable subsystems

Author: abhobe

src/main/java/frc/robot/Constants/Constants.java

@@ -263,26 +263,16 @@ public static final class ELEVATOR {
             * (3.0 / 2.0)
             / 2.0; // CALCULATE LAST VALUE FOR STAGES IN THE ELEVATOR
     public static final Distance CYCLE_HEIGHT = Inches.of(2.15 * Math.PI); // CALCULATE
+    public static final Distance TOLERANCE = Inches.of(0.1);
     public static final Distance Bhobe_HEIGHT = Inches.of(1);
-
-    public static final NavigableMap<Double, AngleRange> HEIGHT_TO_ANGLE_MAP = new TreeMap<>();
-
-    static {
-      HEIGHT_TO_ANGLE_MAP.put(0.0, new AngleRange(Degrees.of(0.0), Degrees.of(45.0)));
-      HEIGHT_TO_ANGLE_MAP.put(1.0, new AngleRange(Degrees.of(10.0), Degrees.of(40.0)));
-      HEIGHT_TO_ANGLE_MAP.put(2.0, new AngleRange(Degrees.of(20.0), Degrees.of(35.0)));
-      HEIGHT_TO_ANGLE_MAP.put(3.0, new AngleRange(Degrees.of(30.0), Degrees.of(30.0)));
-      HEIGHT_TO_ANGLE_MAP.put(4.0, new AngleRange(Degrees.of(40.0), Degrees.of(25.0)));
-    }
-
     public static final class PROFILE {
-      public static final double kP = 25.0;
-      public static final double kS = 1.0;
+      public static final double kP = 4.0;
+      public static final double kS = 2.0;
     }
 
     // HEIGHT IS MEASURED FROM THE GROUND TO THE TOP OF THE ELEVATOR
     public static final Distance BASE_HEIGHT = Inches.of(35.5);
-    public static final Distance MAX_HEIGHT = Inches.of(83.6);
+    public static final Distance MAX_HEIGHT = Inches.of(84.5);
     public static final Distance MIN_HEIGHT = BASE_HEIGHT;
     public static final Distance STOW_HEIGHT = BASE_HEIGHT;
     public static final Distance MAX_UNLIMITED_HEIGHT = Inches.of(41.0); // AVERAGE

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

@@ -98,6 +98,7 @@ public void disabledPeriodic() {
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
+    System.out.println("=== AUTON ENABLED ===");
     autonomousCommand = robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
@@ -112,6 +113,7 @@ public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
+    System.out.println("=== TELEOP ENABLED ===");
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
     // continue until interrupted by another command, remove

src/main/java/frc/robot/RobotContainer.java

@@ -74,7 +74,7 @@ public static RobotContainer getInstance() {
   public final Elevator elevator;
   public final Hang hang;
   public final Autonomous autonomous;
-  public final Algae algaeDetector;
+  // public final Algae algaeDetector;
   public final PieceCombos pieceCombos;
   public final SafeSubsystems safeties;
   // public final ManipulatorSafeties manipulatorSafeties;
@@ -103,12 +103,11 @@ public RobotContainer() {
     // Logger.autoLog("PDH", PDH);
 
     CachedRobotState.init();
-
     AutonChooser.init();
 
     LiveWindow.disableAllTelemetry();
 
-    DriverStation.silenceJoystickConnectionWarning(true);
+    DriverStation.silenceJoystickConnectionWarning(false);
 
     StatusChecks.Category statusChecks = StatusChecks.under("General");
 
@@ -133,11 +132,11 @@ public RobotContainer() {
     // KinematicsUtils.getTranslation(swerveDrive.getEstimatedSpeeds()).getNorm());
     // intake = new Intake();
     manipulator = new Manipulator();
-    elevator = new Elevator();
+    elevator = Elevator.create();
     safeties = new SafeSubsystems(elevator, manipulator);
     pieceCombos = new PieceCombos(elevator, manipulator, safeties);
     autonomous = new Autonomous(stateController, swerveDrive, manipulator, elevator, pieceCombos);
-    algaeDetector = new Algae();
+    // algaeDetector = new Algae();
     hang = Hang.create();
     // // collisionDetector = new CollisionDetector();
 
@@ -156,27 +155,6 @@ public RobotContainer() {
     statusChecks.timestampAdd("timerChecker", () -> Timer.getFPGATimestamp());
 
     refreshButtonEntry.setBoolean(false);
-
-    // module.configureModule(Constants.SWERVE.CONFIG, Corner.FRONT_LEFT);
-
-    // AprilTags.printConfig(Constants.LIMELIGHT.APRILTAG_CAMERA_POSES);
-
-    // Pathfinding.ensureInitialized();
-
-    // swerveModuleTest = new SwerveModuleTest();
-
-    // new Talon10Test();
-
-    // steerModuleTest = new SteerModuleTest();
-
-    // test = new DriveModuleTest();
-
-    // ChassisSpeeds testSpeeds = new ChassisSpeeds(0, 0, 1);
-
-    // Logger.log("conversionTest/speeds", testSpeeds);
-    // Logger.log("conversionTest/states",
-    // KinematicsUtils.kinematicsFromChassis(Constants.SWERVE.CHASSIS).toSwerveModuleStates(testSpeeds));
-
     Logger.start(Milliseconds.of(20));
   }
 

src/main/java/frc/robot/commands/drive/OperatorFineControls.java

@@ -1,36 +1,36 @@
-package frc.robot.commands.drive;
+// package frc.robot.commands.drive;
 
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.elevator.Elevator;
-import frc.robot.subsystems.manipulator.Manipulator;
+// import edu.wpi.first.wpilibj.XboxController;
+// import edu.wpi.first.wpilibj2.command.Command;
+// import frc.robot.subsystems.elevator.Elevator;
+// import frc.robot.subsystems.manipulator.Manipulator;
 
-public class OperatorFineControls extends Command {
-  private Elevator elevator;
-  private XboxController controller;
-  private Manipulator manipulator;
+// public class OperatorFineControls extends Command {
+//   private Elevator elevator;
+//   private XboxController controller;
+//   private Manipulator manipulator;
 
-  public OperatorFineControls(
-      Elevator elevator, Manipulator manipulator, XboxController controller) {
-    this.elevator = elevator;
-    this.controller = controller;
-    this.manipulator = manipulator;
-  }
+//   public OperatorFineControls(
+//       Elevator elevator, Manipulator manipulator, XboxController controller) {
+//     this.elevator = elevator;
+//     this.controller = controller;
+//     this.manipulator = manipulator;
+//   }
 
-  @Override
-  public void execute() {
-    double elevatorSpeed = controller.getLeftY() * 0.2;
+//   @Override
+//   public void execute() {
+//     double elevatorSpeed = controller.getLeftY() * 0.2;
 
-    if (elevator.getCurrentCommand() == elevator.getDefaultCommand()
-        && Math.abs(controller.getLeftY()) > 0.1) {
-      elevator.move(elevatorSpeed).schedule();
-    }
+//     if (elevator.getCurrentCommand() == elevator.getDefaultCommand()
+//         && Math.abs(controller.getLeftY()) > 0.1) {
+//       elevator.move(elevatorSpeed).schedule();
+//     }
 
-    double manipulatorSpeed = controller.getRightY() * 0.2;
+//     double manipulatorSpeed = controller.getRightY() * 0.2;
 
-    if (manipulator.pivot.getCurrentCommand() == manipulator.pivot.getDefaultCommand()
-        && Math.abs(controller.getRightY()) > 0.1) {
-      manipulator.pivot.move(manipulatorSpeed).schedule();
-    }
-  }
-}
+//     if (manipulator.pivot.getCurrentCommand() == manipulator.pivot.getDefaultCommand()
+//         && Math.abs(controller.getRightY()) > 0.1) {
+//       manipulator.pivot.move(manipulatorSpeed).schedule();
+//     }
+//   }
+// }

src/main/java/frc/robot/subsystems/elevator/DisabledElevator.java

@@ -0,0 +1,91 @@
+package frc.robot.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import com.team6962.lib.utils.CommandUtils;
+
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class DisabledElevator extends SubsystemBase implements Elevator {
+    public Command setHeight(Distance height) {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command stop() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command hold() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+    
+    public Command stow() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command coralL1() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+    
+    public Command coralL2() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command coralL3() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command coralL4() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command coralIntake() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command algaeGround() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command algaeL2() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command algaeL3() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command algaeBarge() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command algaeProcessor() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command rezeroAtBottom() {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Distance getAverageHeight() {
+        return Meters.of(0.0);
+    }
+
+    public Command up   () {
+        return CommandUtils.noneWithRequirements(this);
+    }
+
+    public Command down() {
+        return CommandUtils.noneWithRequirements(this);
+    }   
+
+    public Distance getMaxHeight() {
+        return Meters.of(Double.POSITIVE_INFINITY);
+    }
+
+    public Distance getMinHeight() {
+        return Meters.of(Double.NEGATIVE_INFINITY);
+    }
+}