Merge branch 'fix/build-error' into feat/proximity-sensor

Iron-Panthers · Oct 19, 2023 · dbcce21 · dbcce21
2 parents e031545 + e423466
commit dbcce21
Show file tree

Hide file tree

Showing 18 changed files with 128 additions and 94 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -19,6 +19,7 @@
 import frc.robot.Constants.Drive.Dims;
 import frc.robot.commands.ScoreCommand.ScoreStep;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.NetworkWatchdogSubsystem.IPv4;
 import frc.robot.subsystems.RGBSubsystem.RGBColor;
 import frc.robot.subsystems.VisionSubsystem.TagCountDeviation;
@@ -238,22 +239,28 @@ public static final class Ports {
       Map.of(
           NodeType.CONE.atHeight(Height.HIGH),
           List.of(
-              new ScoreStep(SCORE_HIGH).canWaitHere(),
-              new ScoreStep(SCORE_HIGH, IntakeMode.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CONE.atHeight(Height.MID),
           List.of(
-              new ScoreStep(SCORE_MID).canWaitHere(), new ScoreStep(SCORE_MID, IntakeMode.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CONE.atHeight(Height.LOW),
-          List.of(new ScoreStep(SCORE_LOW).canWaitHere(), new ScoreStep(IntakeMode.OUTTAKE)),
+          List.of(
+              new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(),
+              new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.HIGH),
           List.of(
-              new ScoreStep(new ElevatorState(Elevator.SCORE_HIGH)).canWaitHere(),
-              new ScoreStep(new ArmState(Elevator.SCORE_HIGH), IntakeSubsystem.Modes.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_HIGH, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.MID),
           List.of(
-              new ScoreStep(SCORE_MID).canWaitHere(), new ScoreStep(SCORE_MID, IntakeMode.OUTTAKE)),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID).canWaitHere(),
+              new ScoreStep(Elevator.Setpoints.SCORE_MID, IntakeSubsystem.Modes.OUTTAKE)),
           NodeType.CUBE.atHeight(Height.LOW),
-          List.of(new ScoreStep(SCORE_LOW).canWaitHere(), new ScoreStep(IntakeMode.OUTTAKE)));
+          List.of(
+              new ScoreStep(Elevator.Setpoints.SCORE_LOW).canWaitHere(),
+              new ScoreStep(IntakeSubsystem.Modes.OUTTAKE)));
 
   public static final class Vision {
     public static record VisionSource(String name, Transform3d robotToCamera) {}

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -9,22 +9,21 @@
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.Config;
 import frc.robot.Constants.Drive;
+import frc.robot.Constants.Elevator;
 import frc.robot.autonomous.commands.MobilityAuto;
 import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobility;
 import frc.robot.autonomous.commands.N1_1ConePlus2CubeHybridMobilityEngage;
@@ -58,7 +57,7 @@
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem.ElevatorState;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.robot.subsystems.NetworkWatchdogSubsystem;
 import frc.robot.subsystems.RGBSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
@@ -298,16 +297,16 @@ private void configureButtonBindings() {
     will.x()
         .onTrue(
             new EngageCommand(
-                drivebaseSubsystem, intakeSubsystem, EngageCommand.EngageDirection.GO_BACKWARD));
+                drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD));
 
     // outtake states
     jasonLayer
         .off(jason.leftTrigger())
-        .whileTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE));
+        .whileTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE));
     jasonLayer
         .off(jason.rightTrigger())
-        .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.IntakeMode.OUTTAKE));
-    jasonLayer.off(jason.x()).onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF));
+        .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.OUTTAKE));
+    jasonLayer.off(jason.x()).onTrue(new IntakeModeCommand(intakeSubsystem, Modes.OFF));
 
     // intake presets
     // jasonLayer
@@ -324,7 +323,7 @@ private void configureButtonBindings() {
         .onTrue(
             new ElevatorPositionCommand(
                 elevatorSubsystem, Constants.Elevator.Setpoints.SHELF_INTAKE))
-        .onTrue(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE));
+        .onTrue(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE));
 
     // ground pickup
     jasonLayer
@@ -335,8 +334,8 @@ private void configureButtonBindings() {
                 intakeSubsystem,
                 () ->
                     jason.getHID().getPOV() == 180
-                        ? IntakeSubsystem.IntakeMode.HOLD
-                        : IntakeSubsystem.IntakeMode.INTAKE));
+                        ? IntakeSubsystem.Modes.HOLD
+                        : IntakeSubsystem.Modes.INTAKE));
 
     // scoring
     // jasonLayer
@@ -417,21 +416,22 @@ private void setupAutonomousCommands() {
     final List<ScoreStep> drivingCubeOuttake =
         List.of(
             new ScoreStep(new ElevatorState(35.0, Constants.Elevator.MIN_HEIGHT)).canWaitHere(),
-            new ScoreStep(IntakeMode.OUTTAKE));
+            new ScoreStep(Modes.OUTTAKE));
     final boolean[] intakeLow = {false};
+    // FIXME go through each auto and make sure that we dont use a leftover event marker from Simba
     final Map<String, Command> eventMap =
         Map.of(
-            "stow arm",
+            "stow elevator",
+            new ElevatorPositionCommand(elevatorSubsystem, Elevator.Setpoints.STOWED),
             "zero everything",
+            new SetZeroModeCommand(elevatorSubsystem, true),
             "intake",
-            /*new GroundPickupCommand(
-            intakeSubsystem,
-            elevatorSubsystem,
-            () ->
-                intakeLow[0]
-                    ? IntakeSubsystem.IntakeMode.INTAKE_LOW
-                    : IntakeSubsystem.IntakeMode.INTAKE),*/
-            "squeeze intake",
+            new ElevatorPositionCommand( // edited so that it works with elevator - chooses between
+                // ground or shelf intake
+                elevatorSubsystem,
+                intakeLow[0] ? Elevator.Setpoints.GROUND_INTAKE : Elevator.Setpoints.SHELF_INTAKE),
+            // squeeze intake isn't relevant to offseason bot - leaving here just in case
+            /*"squeeze intake",
             new CommandBase() {
               private double lastTime = Timer.getFPGATimestamp();
 
@@ -450,7 +450,7 @@ public boolean isFinished() {
               public void end(boolean interrupted) {
                 intakeLow[0] = false;
               }
-            },
+            },*/
             "stage outtake",
             new ScoreCommand(
                 intakeSubsystem, elevatorSubsystem, drivingCubeOuttake.subList(0, 1), 1),
@@ -472,7 +472,7 @@ public void end(boolean interrupted) {
                 .andThen(
                     new ElevatorPositionCommand(
                             elevatorSubsystem, Constants.Elevator.Setpoints.STOWED)
-                        .andThen(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF))),
+                        .andThen(new IntakeModeCommand(intakeSubsystem, Modes.OFF))),
             "armbat preload",
             new ElevatorPositionCommand(elevatorSubsystem, 30, 0)
                 .andThen(
@@ -495,7 +495,8 @@ public void end(boolean interrupted) {
             4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
     autoSelector.addOption(
-        "Just Zero Arm [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem));
+        "Just Zero Elevator [DOES NOT CALIBRATE]",
+        new SetZeroModeCommand(elevatorSubsystem, false));
 
     autoSelector.addOption(
         "Near Substation Mobility [APRILTAG]",
@@ -544,8 +545,10 @@ public void end(boolean interrupted) {
 
     autoSelector.addOption(
         "Score High Cone [DOES NOT CALIBRATE]",
-        new SetZeroModeCommand(elevatorSubsystem)
-            .raceWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE))
+        new SetZeroModeCommand(
+                elevatorSubsystem,
+                false) // FIXME pretty sure this shouldn't zero wrist, double check later
+            .raceWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE))
             .andThen(
                 new ScoreCommand(
                     intakeSubsystem,

diff --git a/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java b/src/main/java/frc/robot/autonomous/commands/MobilityAuto.java
@@ -12,7 +12,7 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.robot.subsystems.RGBSubsystem;
 import frc.util.pathing.AlliancePose2d;
 import frc.util.pathing.RubenManueverGenerator;
@@ -40,7 +40,7 @@ public MobilityAuto(
                 Optional.of(rgbSubsystem),
                 Optional.empty())
             .alongWith(new SetZeroModeCommand(elevatorSubsystem))
-            .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.HOLD))
+            .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.HOLD))
             .andThen(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem)));
   }
 }
diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobility.java
@@ -14,7 +14,7 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.util.pathing.LoadMirrorPath;
 import java.util.Map;
 import java.util.function.Supplier;
@@ -39,7 +39,7 @@ public N1_1ConePlus2CubeHybridMobility(
             new FollowTrajectoryCommand(path, true, drivebaseSubsystem),
             path.get().getMarkers(),
             eventMap),
-        new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF)
+        new IntakeModeCommand(intakeSubsystem, Modes.OFF)
             .alongWith(new SetZeroModeCommand(elevatorSubsystem)));
   }
 }
diff --git a/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N1_1ConePlus2CubeHybridMobilityEngage.java
@@ -15,7 +15,7 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.util.pathing.LoadMirrorPath;
 import java.util.Map;
 import java.util.function.Supplier;
@@ -39,8 +39,8 @@ public N1_1ConePlus2CubeHybridMobilityEngage(
             path.get().getMarkers(),
             eventMap),
         new EngageCommand(
-                drivebaseSubsystem, intakeSubsystem, EngageCommand.EngageDirection.GO_BACKWARD)
-            .alongWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.OFF))
+                drivebaseSubsystem, elevatorSubsystem, EngageCommand.EngageDirection.GO_BACKWARD)
+            .alongWith(new IntakeModeCommand(intakeSubsystem, Modes.OFF))
             .alongWith(new SetZeroModeCommand(elevatorSubsystem)));
   }
 }
diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobility.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
 import frc.robot.commands.ElevatorPositionCommand;
 import frc.robot.commands.FollowTrajectoryCommand;
 import frc.robot.commands.IntakeModeCommand;
@@ -35,7 +36,7 @@ public N3_1ConePlusMobility(
 
     addCommands(
         new SetZeroModeCommand(elevatorSubsystem)
-            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)),
+            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)),
         new ScoreCommand(
             intakeSubsystem,
             elevatorSubsystem,

diff --git a/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java b/src/main/java/frc/robot/autonomous/commands/N3_1ConePlusMobilityEngage.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
 import frc.robot.commands.ElevatorPositionCommand;
 import frc.robot.commands.EngageCommand;
 import frc.robot.commands.FollowTrajectoryCommand;
@@ -17,7 +18,7 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.util.NodeSelectorUtility.Height;
 import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.pathing.LoadMirrorPath;
@@ -40,7 +41,7 @@ public N3_1ConePlusMobilityEngage(
 
     addCommands(
         new SetZeroModeCommand(elevatorSubsystem)
-            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)),
+            .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)),
         new ScoreCommand(
             intakeSubsystem,
             elevatorSubsystem,

diff --git a/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java b/src/main/java/frc/robot/autonomous/commands/N6_1ConePlusEngage.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
 import frc.robot.commands.ElevatorPositionCommand;
 import frc.robot.commands.EngageCommand;
 import frc.robot.commands.FollowTrajectoryCommand;
@@ -17,7 +18,7 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.util.NodeSelectorUtility.Height;
 import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.pathing.LoadMirrorPath;
@@ -38,7 +39,7 @@ public N6_1ConePlusEngage(
 
     addCommands(
         new SetZeroModeCommand(elevatorSubsystem)
-            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)),
+            .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)),
         new ScoreCommand(
             intakeSubsystem,
             elevatorSubsystem,

diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlus2CubeMobility.java
@@ -13,17 +13,16 @@
 import frc.robot.commands.IntakeModeCommand;
 import frc.robot.commands.ScoreCommand;
 import frc.robot.commands.SetZeroModeCommand;
-import frc.robot.commands.ZeroIntakeCommand;
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
-import frc.robot.subsystems.intakeSubsystem;
+import frc.robot.subsystems.IntakeSubsystem.Modes;
 import frc.util.NodeSelectorUtility.Height;
 import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.pathing.LoadMirrorPath;
 import java.util.List;
 import java.util.Map;
+import java.util.Optional;
 import java.util.function.Supplier;
 
 public class N9_1ConePlus2CubeMobility extends SequentialCommandGroup {
@@ -41,8 +40,10 @@ public N9_1ConePlus2CubeMobility(
 
     addCommands(
         (new SetZeroModeCommand(elevatorSubsystem)
-                .alongWith(new ZeroIntakeCommand(intakeSubsystem)))
-            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)),
+                .alongWith(
+                    new SetZeroModeCommand(
+                        elevatorSubsystem, Optional.of(true), Optional.of(false))))
+            .deadlineWith(new IntakeModeCommand(intakeSubsystem, Modes.INTAKE)),
         new ScoreCommand(
             intakeSubsystem,
             elevatorSubsystem,

diff --git a/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java b/src/main/java/frc/robot/autonomous/commands/N9_1ConePlusMobility.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
+import frc.robot.Constants.Elevator;
 import frc.robot.commands.ElevatorPositionCommand;
 import frc.robot.commands.FollowTrajectoryCommand;
 import frc.robot.commands.IntakeModeCommand;
@@ -16,7 +17,6 @@
 import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.intakeSubsystem;
 import frc.util.NodeSelectorUtility.Height;
 import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.pathing.LoadMirrorPath;
@@ -37,7 +37,7 @@ public N9_1ConePlusMobility(
 
     addCommands(
         new SetZeroModeCommand(elevatorSubsystem)
-            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeMode.INTAKE)),
+            .deadlineWith(new IntakeModeCommand(intakeSubsystem, IntakeSubsystem.Modes.INTAKE)),
         new ScoreCommand(
             intakeSubsystem,
             elevatorSubsystem,