Day before competition changes

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotContoller.java

@@ -86,10 +86,11 @@ public void initialize() {
         driverController = new GamepadEx(gamepad1);
         operatorController = new GamepadEx(gamepad2);
 
+        GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP;
         savedLocation = SavedConfiguration.selectedLocation;
         savedAutonomousRoutine = SavedConfiguration.selectedAuto;
         savedAllianceColor = SavedConfiguration.selectedAlliance;
-        savedAutonomousEndingPosition = SavedConfiguration.pathEndPose;
+        savedAutonomousEndingPosition = SavedConfiguration.finalDrivetrainPose;
 
         teleopInitTime = getRuntime();
         configLocked = false;
@@ -102,10 +103,7 @@ public void initialize() {
         TeleopBindings.configureBindings(driverController, operatorController, drivetrain, intake, transfer, shooter, turret, lift, led);
         TeleopBindings.configureDefaultCommands(driverController, operatorController, drivetrain, intake, transfer, shooter, turret, led);
 
-        schedule(
-                new RunCommand(() -> telemetryManager.update(telemetry)),
-                new RunCommand(led::update)
-        );
+        schedule(new RunCommand(() -> telemetryManager.update(telemetry)));
     }
 
     private void readInputs() {
@@ -158,6 +156,7 @@ public void initialize_loop() {
         }
 
         telemetryManager.update(telemetry);
+        lift.onInitialization();
         led.update();
     }
 
@@ -190,7 +189,6 @@ private void drawLockedUI() {
 
     private void setGlobalSettings() {
         GlobalConstants.allianceColor = savedAllianceColor;
-        GlobalConstants.opModeType = GlobalConstants.OpModeType.TELEOP;
     }
 
     private <T> T cycleRight(T current, T[] values) {
@@ -228,7 +226,7 @@ public void run() {
             telemetryManager.addData(VisionConstants.kSubsystemName + " Estimated Robot Pose Y", visionPose2d.getY());
             telemetryManager.addData(VisionConstants.kSubsystemName + " Estimated Robot Pose θ", visionPose2d.getRotation().getRadians());
 
-            drivetrain.updateWithVision(visionPose2d);
+            //drivetrain.updateWithVision(visionPose2d);
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleopBindings.java

@@ -1,10 +1,13 @@
 package org.firstinspires.ftc.teamcode;
 
+import org.firstinspires.ftc.library.command.Commands;
+import org.firstinspires.ftc.library.command.ConditionalCommand;
 import org.firstinspires.ftc.library.command.InstantCommand;
 import org.firstinspires.ftc.library.command.button.Trigger;
 import org.firstinspires.ftc.library.gamepad.GamepadEx;
 import org.firstinspires.ftc.library.gamepad.GamepadKeys;
 import org.firstinspires.ftc.teamcode.command_factories.IntakeFactory;
+import org.firstinspires.ftc.teamcode.command_factories.LiftFactory;
 import org.firstinspires.ftc.teamcode.command_factories.ShooterFactory;
 import org.firstinspires.ftc.teamcode.command_factories.SuperstructureFactory;
 import org.firstinspires.ftc.teamcode.command_factories.TransferFactory;
@@ -32,8 +35,8 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
 
         if(GlobalConstants.getCurrentDriverType() == GlobalConstants.DriverType.HANISH) {
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
-                    .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
-                    .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenActive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1))
+                    .whenInactive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
                     .whenActive(ShooterFactory.velocitySetpointCommand(shooter, () -> 3500))
@@ -45,12 +48,12 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
             );
 
             driver.getGamepadButton(GamepadKeys.Button.CIRCLE).whenPressed(
-                    ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.smartShootingCommand(intake, transfer, led))
+                    ShooterFactory.velocitySetpointCommand(shooter, () -> 3350).andThen(SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> true))
             );
 
             driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                    .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
-                    .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenPressed(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> -1))
+                    .whenReleased(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             driver.getGamepadButton(GamepadKeys.Button.DPAD_UP)
                     .whenActive(() -> shooter.setHoodPosition(shooter.getHoodTargetPosition() + 0.001));
@@ -60,15 +63,19 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
                     .whenReleased(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
         } else {
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5)
-                    .whenActive(IntakeFactory.openLoopSetpointCommand(intake, () -> 1))
-                    .whenInactive(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenActive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 1))
+                    .whenInactive(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             new Trigger(() -> driver.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
-                    .whenActive(SuperstructureFactory.smartShootingCommand(intake, transfer, led));
+                    .whenActive(new ConditionalCommand(
+                            SuperstructureFactory.rapidFireCommand(intake, transfer, shooter, led, () -> false),
+                            SuperstructureFactory.controlledShootCommand(intake, transfer, shooter, led, () -> false),
+                            drivetrain::isRobotinCloseZone
+                    ));
 
             driver.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
-                    .whenPressed(IntakeFactory.openLoopSetpointCommand(intake, () -> -1))
-                    .whenReleased(IntakeFactory.openLoopSetpointCommand(intake, () -> 0));
+                    .whenPressed(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> -1))
+                    .whenReleased(IntakeFactory.setUnevenOpenLoopSetpointCommand(intake, () -> 0));
 
             driver.getGamepadButton(GamepadKeys.Button.RIGHT_STICK_BUTTON).toggleWhenPressed(
                     TransferFactory.engageBlocker(transfer, () -> TransferConstants.blockerAllowPosition),
@@ -117,13 +124,26 @@ public static void configureBindings(GamepadEx driver, GamepadEx operator, Drive
         operator.getGamepadButton(GamepadKeys.Button.SQUARE)
                 .whenPressed(TransferFactory.runKickerCycle(transfer));
 
+        operator.getGamepadButton(GamepadKeys.Button.TRIANGLE)
+                .whenPressed(LiftFactory.resetLiftToZeroCommand(lift));
+
+        //operator.getGamepadButton(GamepadKeys.Button.CIRCLE)
+        //        .whenPressed(new InstantCommand(() -> shooter.)
+
         new Trigger(() -> operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5)
                 .whenActive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0.75))
                 .whenInactive(ShooterFactory.openLoopSetpointCommand(shooter, () -> 0));
 
         new Trigger(transfer::doesTransferContainAnyBalls)
                 .whenActive(new InstantCommand(() -> shooter.setFlywheelCommanded(true)))
                 .whenInactive(new InstantCommand(() -> shooter.setFlywheelCommanded(false)));
+
+        new Trigger(transfer::doesTransferContainAllBalls)
+                .whenActive(Commands.sequence(
+                        new InstantCommand(() -> driver.gamepad.rumble(1.0, 1.0, 200)),
+                        Commands.waitMillis(350),
+                        new InstantCommand(() -> driver.gamepad.rumble(1.0, 1.0, 200))
+                ));
     }
 
     public static void configureDefaultCommands(GamepadEx driver, GamepadEx operator, Drivetrain drivetrain, Intake intake, Transfer transfer, Shooter shooter, Turret turret, LED led) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto.java

@@ -8,5 +8,6 @@ public enum Auto {
     SIX_BALL,
     SIX_BALL_PICKUP,
     NINE_BALL,
-    NINE_BALL_PICKUP
+    NINE_BALL_PICKUP,
+    TWELVE_BALL
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/AutoChooser.java

@@ -27,7 +27,7 @@ public AutoChooser(Drivetrain drivetrain, Intake intake, Transfer transfer, Turr
         routines.add(new AutoRoutine(Location.CLOSE, Auto.LEAVE, () -> autoFactory.initializeCloseLeave(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
         routines.add(new AutoRoutine(Location.CLOSE, Auto.NINE_BALL, () -> autoFactory.initializeCloseNineBall(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
         routines.add(new AutoRoutine(Location.CLOSE, Auto.NINE_BALL_PICKUP, () -> autoFactory.initializeCloseNineBallPickup(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
-
+        routines.add(new AutoRoutine(Location.CLOSE, Auto.TWELVE_BALL, () -> autoFactory.initializeCloseTwelveBall(GlobalConstants.allianceColor, Location.CLOSE.getPose())));
 
         routines.add(new AutoRoutine(Location.FAR, Auto.IDLE, () -> autoFactory.initializeIdle(GlobalConstants.allianceColor, Location.FAR.getPose())));
         routines.add(new AutoRoutine(Location.FAR, Auto.LEAVE, () -> autoFactory.initializeFarLeave(GlobalConstants.allianceColor, Location.FAR.getPose())));