Merge pull request #14 from MillenniumFalcons/autoShootWhenReady

auto shoot and code actually passes gh checks now
MillenniumFalcons · Aug 20, 2024 · 9908161 · 9908161
2 parents e9434b2 + d76df75
commit 9908161
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 17 deletions.
diff --git a/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2024/commands/DrivetrainCommands.java
@@ -60,8 +60,8 @@ public Command driveVisionTeleop(
                             -turnSpeedFunction.getAsDouble() * maxRotationRadpS * triggerSlow;
 
                     if (mode == DriveMode.SHOOT_AT_AMP && enabeld) {
-                        motionXComponent = autoDriveTwist2d.dx;
-                        motionTurnComponent = autoDriveTwist2d.dtheta;
+                        motionXComponent = autoDriveTwist2d.dx + motionXComponent * 0.1;
+                        motionTurnComponent = autoDriveTwist2d.dtheta + motionTurnComponent * 0.1;
 
                         var translation = new Translation2d(motionXComponent, motionYComponent);
 

diff --git a/src/main/java/team3647/frc2024/robot/RobotContainer.java b/src/main/java/team3647/frc2024/robot/RobotContainer.java
@@ -518,7 +518,8 @@ public Command getAutonomousCommand() {
                     autoDrive::flywheelThreshold,
                     targetingUtil.exitVelocity(),
                     autoDrive::isFeed,
-                    autoDrive::swerveAimed);
+                    autoDrive::swerveAimed,
+                    mainController.buttonY);
 
     LEDTriggers ledTriggers = new LEDTriggers(superstructure, autoDrive::getMode);
 

diff --git a/src/main/java/team3647/frc2024/subsystems/Superstructure.java b/src/main/java/team3647/frc2024/subsystems/Superstructure.java
@@ -50,6 +50,8 @@ public class Superstructure {
 
     private final BooleanSupplier feedShot;
 
+    private final BooleanSupplier dontShoot;
+
     private final Trigger front;
 
     private DriveMode wantedShootingMode = DriveMode.SHOOT_STATIONARY;
@@ -68,7 +70,8 @@ public Superstructure(
             DoubleSupplier shooterSpeedThresholdSupplier,
             double shootSpeed,
             BooleanSupplier feedShot,
-            BooleanSupplier swerveAimed) {
+            BooleanSupplier swerveAimed,
+            BooleanSupplier dontShoot) {
         this.intake = intake;
         this.kicker = kicker;
         this.shooterRight = shooterRight;
@@ -83,6 +86,7 @@ public Superstructure(
         this.wrist = wrist;
         this.swerveAimed = swerveAimed;
         this.feedShot = feedShot;
+        this.dontShoot = dontShoot;
 
         intakeCommands = new IntakeCommands(intake);
         kickerCommands = new KickerCommands(kicker);
@@ -222,21 +226,28 @@ public boolean swerveReady() {
         return swerveAimed.getAsBoolean();
     }
 
+    public boolean readyForShot() {
+        return aimedAtSpeaker() && !dontShoot.getAsBoolean();
+    }
+
     public Command shoot() {
         return Commands.parallel(
-                prep(), spinUp()
-                // Commands.sequence(
-                //         // Commands.waitSeconds(2.5),
-                //         Commands.waitUntil(
-                //                         () ->
-                //                                 shooterLeft.velocityReached(30, 2)
-                //                                         && pivot.angleReached(
-                //                                                 pivotAngleSupplier.getAsDouble(),
-                // 5)
-                //                                         && swerveAimed.getAsBoolean())
-                //                 .withTimeout(1.2),
-                //         feed())
-                );
+                        prep(), spinUp()
+                        // Commands.sequence(
+                        //         // Commands.waitSeconds(2.5),
+                        //         Commands.waitUntil(
+                        //                         () ->
+                        //                                 shooterLeft.velocityReached(30, 2)
+                        //                                         && pivot.angleReached(
+                        //
+                        // pivotAngleSupplier.getAsDouble(),
+                        // 5)
+                        //                                         && swerveAimed.getAsBoolean())
+                        //                 .withTimeout(1.2),
+                        //         feed())
+                        )
+                .until(this::readyForShot)
+                .andThen(stowFromShoot());
     }
 
     public Command cleanShoot() {
@@ -372,6 +383,14 @@ public Command stowFromShoot() {
                         .withTimeout(0.1));
     }
 
+    public Command stowNoShoot() {
+        return Commands.parallel(
+                        pivotCommands.setAngle(() -> pivotAngleSupplier.getAsDouble()),
+                        shooterCommands.kill(),
+                        kickerCommands.kill())
+                .withTimeout(0.1);
+    }
+
     public Command stowFromAmpShoot() {
         return Commands.sequence(
                 Commands.parallel(