Added amp assist

Robocubs · Jul 22, 2024 · 4f9f0ad · 4f9f0ad
1 parent c3f0f32
commit 4f9f0ad
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 13 deletions.
diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java
@@ -365,16 +365,6 @@ private void setupControllerBindings() {
                                 : Constants.Drive.kFastKinematicLimits),
                 mDrive::useDriveAssist));
 
-        // mDrive.setDefaultCommand(driveWithJoysticks(
-        //         mDrive,
-        //         mDrive::getFieldRelativeHeading,
-        //         () -> -mDriverController.getLeftY(),
-        //         () -> -mDriverController.getLeftX(),
-        //         () -> -mDriverController.getRightX(),
-        //         () -> mDriverController.getHID().getRightBumper()
-        //                 ? Constants.Drive.kSlowKinematicLimits
-        //                 : Constants.Drive.kFastKinematicLimits));
-
         mIndexer.setDefaultCommand(IntakeCommands.idleIndexer(mIndexer));
 
         mIntake.setDefaultCommand(IntakeCommands.idleIntake(mIntake, mRobotState));
@@ -408,7 +398,7 @@ private void setupControllerBindings() {
         // Long Pass
         mDriverController
                 .leftBumper()
-                .and(() -> mRobotState.getScoringMode().equals(ScoringMode.SPEAKER))
+                .and(() -> mRobotState.isSpeakerMode())
                 .whileTrue(DriveCommands.passLong(
                         mDrive,
                         mShooter,
@@ -428,7 +418,7 @@ private void setupControllerBindings() {
                         () -> -mDriverController.getLeftY(),
                         () -> -mDriverController.getLeftX()));
 
-        // Pass Low
+        // Low Pass
         mDriverController
                 .x()
                 .whileTrue(ShootCommands.passLow(
@@ -439,9 +429,21 @@ private void setupControllerBindings() {
                         () -> -mDriverController.getLeftY(),
                         () -> -mDriverController.getLeftX()));
 
-        // Drive to Amp
+        // Drive With Amp Assist
         mDriverController
                 .leftBumper()
+                .and(() -> mRobotState.isAmpMode())
+                .whileTrue(DriveCommands.driveWithAmpAssist(
+                        mDrive,
+                        () -> -mDriverController.getLeftY(),
+                        () -> -mDriverController.getLeftX(),
+                        mRobotState::getHeading,
+                        () -> Rotation2d.fromRadians(0.05),
+                        Constants.Drive.kFastSmoothKinematicLimits));
+
+        // Drive to Amp
+        mDriverController
+                .povUp()
                 .and(() -> mRobotState.getScoringMode().equals(ScoringMode.AMP))
                 .and(() -> mRobotState.inWing() || mRobotState.getPose2d().getY() > 6.4)
                 .whileTrue(DriveCommands.driveToAmp(
@@ -451,6 +453,7 @@ private void setupControllerBindings() {
                         Constants.Drive.kMediumTrapezoidalKinematicLimits,
                         false));
 
+        // Drive to Opposite Amp
         mDriverController
                 .povDown()
                 .and(() -> mRobotState.getScoringMode().equals(ScoringMode.AMP))

diff --git a/src/main/java/com/team1701/robot/commands/DriveCommands.java b/src/main/java/com/team1701/robot/commands/DriveCommands.java
@@ -55,6 +55,28 @@ public static Command driveWithJoysticks(
                 .withName("DriveWithJoysticks");
     }
 
+    public static Command driveWithAmpAssist(
+            Drive drive,
+            DoubleSupplier throttle,
+            DoubleSupplier strafe,
+            Supplier<Rotation2d> robotHeadingSupplier,
+            Supplier<Rotation2d> headingTolerance,
+            KinematicLimits kinematicLimits) {
+        return new RotateToFieldHeading(
+                drive,
+                () -> calculateDriveWithJoysticksVelocities(
+                                throttle.getAsDouble(),
+                                strafe.getAsDouble(),
+                                drive.getFieldRelativeHeading(),
+                                kinematicLimits.maxDriveVelocity())
+                        .rotateBy(robotHeadingSupplier.get()),
+                () -> Rotation2d.fromDegrees(90),
+                robotHeadingSupplier,
+                headingTolerance,
+                kinematicLimits,
+                false);
+    }
+
     public static Translation2d calculateDriveWithJoysticksVelocities(
             double throttle, double strafe, Rotation2d heading, double maxVelocity) {
         var translationSign = Configuration.isBlueAlliance() ? 1.0 : -1.0;