fixing thigns?

Iron-Panthers · Nov 11, 2023 · 38d200a · 38d200a
1 parent 527f839
commit 38d200a
Showing 1 changed file with 80 additions and 68 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,6 +5,7 @@
 package frc.robot;
 
 import com.pathplanner.lib.server.PathPlannerServer;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
@@ -23,6 +24,16 @@
 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;
+import frc.robot.autonomous.commands.N2_Engage;
+import frc.robot.autonomous.commands.N3_1ConePlusMobility;
+import frc.robot.autonomous.commands.N3_1ConePlusMobilityEngage;
+import frc.robot.autonomous.commands.N6_1ConePlusEngage;
+import frc.robot.autonomous.commands.N9_1ConePlus2CubeMobility;
+import frc.robot.autonomous.commands.N9_1ConePlusMobility;
+import frc.robot.autonomous.commands.N9_1ConePlusMobilityEngage;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.DriveToPlaceCommand;
@@ -34,6 +45,7 @@
 import frc.robot.commands.HashMapCommand;
 import frc.robot.commands.IntakeModeCommand;
 import frc.robot.commands.RotateVectorDriveCommand;
+import frc.robot.commands.RotateVelocityDriveCommand;
 import frc.robot.commands.ScoreCommand;
 import frc.robot.commands.ScoreCommand.ScoreStep;
 import frc.robot.commands.SetZeroModeCommand;
@@ -56,6 +68,7 @@
 import frc.util.NodeSelectorUtility.NodeType;
 import frc.util.SharedReference;
 import frc.util.Util;
+import frc.util.pathing.AlliancePose2d;
 import frc.util.pathing.RubenManueverGenerator;
 import java.util.HashMap;
 import java.util.List;
@@ -225,14 +238,14 @@ private void configureButtonBindings() {
                 /** percent of fraction power */
                 (anthony.getHID().getAButton() ? .3 : .8);
 
-    // new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
-    //     .whileTrue(
-    //         new RotateVelocityDriveCommand(
-    //             drivebaseSubsystem,
-    //             translationXSupplier,
-    //             translationYSupplier,
-    //             rotationVelocity,
-    //             anthony.rightBumper()));
+    new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
+        .whileTrue(
+            new RotateVelocityDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                rotationVelocity,
+                anthony.rightBumper()));
 
     new Trigger(
             () ->
@@ -539,68 +552,67 @@ public void end(boolean interrupted) {
     //     new N9_1ConePlus2CubeMobility(
     //         4.95, 3, eventMap, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
 
-    // autoSelector.addOption(
-    //     "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem));
-
-    // autoSelector.addOption(
-    //     "Near Substation Mobility [APRILTAG]",
-    //     new MobilityAuto(
-    //         manueverGenerator,
-    //         drivebaseSubsystem,
-    //         intakeSubsystem,
-    //         elevatorSubsystem,
-    //         rgbSubsystem,
-    //         new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0))));
-
-    // autoSelector.addOption(
-    //     "Far Substation Mobility [APRILTAG]",
-    //     new MobilityAuto(
-    //         manueverGenerator,
-    //         drivebaseSubsystem,
-    //         intakeSubsystem,
-    //         elevatorSubsystem,
-    //         rgbSubsystem,
-    //         new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0))));
-
-    // autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem));
+    autoSelector.addOption(
+        "Just Zero Elevator [DOES NOT CALIBRATE]", new SetZeroModeCommand(elevatorSubsystem));
 
-    // autoSelector.addOption(
-    //     "N3 1Cone + Mobility Engage",
-    //     new N3_1ConePlusMobilityEngage(
-    //         5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
-
-    // autoSelector.setDefaultOption(
-    //     "N3 1Cone + Mobility",
-    //     new N3_1ConePlusMobility(
-    //         4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+    autoSelector.addOption(
+        "Near Substation Mobility [APRILTAG]",
+        new MobilityAuto(
+            manueverGenerator,
+            drivebaseSubsystem,
+            intakeSubsystem,
+            elevatorSubsystem,
+            rgbSubsystem,
+            new AlliancePose2d(4.88, 6.05, Rotation2d.fromDegrees(0))));
 
-    // autoSelector.setDefaultOption(
-    //     "N6 1Cone + Engage",
-    //     new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
-
-    // autoSelector.addOption(
-    //     "N9 1Cone + Mobility Engage",
-    //     new N9_1ConePlusMobilityEngage(
-    //         5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
-
-    // autoSelector.addOption(
-    //     "N9 1Cone + Mobility",
-    //     new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem,
-    // drivebaseSubsystem));
-
-    // autoSelector.addOption(
-    //     "Score High Cone [DOES NOT CALIBRATE]",
-    //     new SetZeroModeCommand(
-    //             elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check
-    //         // later
-    //         .raceWith(
-    //             new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)
-    //                 .andThen(
-    //                     new ScoreCommand(
-    //                         intakeSubsystem,
-    //                         elevatorSubsystem,
-    //                         Constants.SCORE_STEP_MAP.get(
-    //                             NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))));
+    autoSelector.addOption(
+        "Far Substation Mobility [APRILTAG]",
+        new MobilityAuto(
+            manueverGenerator,
+            drivebaseSubsystem,
+            intakeSubsystem,
+            elevatorSubsystem,
+            rgbSubsystem,
+            new AlliancePose2d(6, .6, Rotation2d.fromDegrees(0))));
+
+    autoSelector.addOption("N2 Engage", new N2_Engage(5, 3.5, drivebaseSubsystem));
+
+    autoSelector.addOption(
+        "N3 1Cone + Mobility Engage",
+        new N3_1ConePlusMobilityEngage(
+            5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+
+    autoSelector.setDefaultOption(
+        "N3 1Cone + Mobility",
+        new N3_1ConePlusMobility(
+            4.95, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+
+    autoSelector.setDefaultOption(
+        "N6 1Cone + Engage",
+        new N6_1ConePlusEngage(5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+
+    autoSelector.addOption(
+        "N9 1Cone + Mobility Engage",
+        new N9_1ConePlusMobilityEngage(
+            5, 3.5, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+
+    autoSelector.addOption(
+        "N9 1Cone + Mobility",
+        new N9_1ConePlusMobility(4.95, 3, intakeSubsystem, elevatorSubsystem, drivebaseSubsystem));
+
+    autoSelector.addOption(
+        "Score High Cone [DOES NOT CALIBRATE]",
+        new SetZeroModeCommand(
+                elevatorSubsystem) // FIXME pretty sure this shouldn't zero wrist, double check
+            // later
+            .raceWith(
+                new IntakeModeCommand(intakeSubsystem, Modes.INTAKE, () -> false)
+                    .andThen(
+                        new ScoreCommand(
+                            intakeSubsystem,
+                            elevatorSubsystem,
+                            Constants.SCORE_STEP_MAP.get(
+                                NodeSelectorUtility.NodeType.CONE.atHeight(Height.HIGH))))));
 
     driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);