Amp + ferrying changes

simgui.json

@@ -19,6 +19,7 @@
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Autonomous": "String Chooser",
       "/SmartDashboard/Field": "Field2d",
+      "/SmartDashboard/Scheduler": "Scheduler",
       "/SmartDashboard/Visualizers/Climber": "Mechanism2d",
       "/SmartDashboard/Visualizers/Intake": "Mechanism2d",
       "/SmartDashboard/Visualizers/Lift": "Mechanism2d"

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -50,6 +50,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
@@ -148,8 +149,9 @@ private void configureDriverBindings() {
 
         // note to amper and align then score
         driver.getLeftBumper()
-            .whileTrue(new AmpScoreRoutine())
-            .onFalse(new AmperStop())
+            .onTrue(new ConveyorToAmpUnsafe()) // requirements: conveyor, intake
+            .whileTrue(new AmpScoreRoutine())  // requirements: swerve, amper
+            // .onFalse(new ConditionalCommand(new AmperStop(), new InstantCommand(), () -> !Amper.getInstance().hasNote()))
             .onFalse(new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT));
 
         // score speaker no align
@@ -211,7 +213,15 @@ private void configureDriverBindings() {
         //             .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))));
 
         driver.getTopButton()
-            .whileTrue(new SwerveDriveAutoFerry(driver));
+            .onTrue(new ShooterSetRPM(Settings.Shooter.FERRY))
+            .whileTrue(new SwerveDriveToFerry()
+                .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)
+                .andThen(new ShooterWaitForTarget()
+                    .withTimeout(0.5)))
+                .andThen(new ConveyorShoot()))
+            .onFalse(new ConveyorStop())
+            .onFalse(new IntakeStop())
+            .onFalse(new ShooterSetRPM(Settings.Shooter.PODIUM_SHOT));
 
         // climb
         driver.getRightButton()

src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java

@@ -8,6 +8,7 @@
 
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.commands.amper.AmperScore;
+import com.stuypulse.robot.commands.amper.AmperScoreThenStop;
 import com.stuypulse.robot.commands.amper.AmperToHeight;
 import com.stuypulse.robot.commands.conveyor.ConveyorToAmp;
 import com.stuypulse.robot.commands.leds.LEDSet;
@@ -20,6 +21,7 @@
 import com.stuypulse.robot.constants.Settings.Alignment;
 import com.stuypulse.robot.constants.Settings.Amper.Lift;
 import com.stuypulse.robot.constants.Settings.Amper.Score;
+import com.stuypulse.robot.subsystems.amper.Amper;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.vision.AprilTagVision;
 import com.stuypulse.stuylib.math.Vector2D;
@@ -31,6 +33,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 
 public class AmpScoreRoutine extends SequentialCommandGroup {
 
@@ -57,7 +60,7 @@ private static Pose2d getTargetPose(double distanceToWall) {
     public AmpScoreRoutine() {
         addCommands(
             new ParallelCommandGroup(
-                new ConveyorToAmp(),
+                new WaitUntilCommand(() -> Amper.getInstance().hasNote()),
                 new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get()))
                     .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE)
                     .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN))
@@ -75,7 +78,7 @@ public AmpScoreRoutine() {
                     .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE))
             ),
 
-            AmperScore.untilDone(),
+            AmperScoreThenStop.scoreThenStop(),
 
             new WaitCommand(0.25),
 

src/main/java/com/stuypulse/robot/commands/amper/AmperScoreThenStop.java

@@ -0,0 +1,37 @@
+
+package com.stuypulse.robot.commands.amper;
+
+import com.stuypulse.robot.subsystems.amper.Amper;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class AmperScoreThenStop extends InstantCommand {
+
+    private final Amper amper;
+
+    public static Command scoreThenStop() {
+        return new AmperScoreThenStop();
+    }
+
+    public AmperScoreThenStop() {
+        amper = Amper.getInstance();
+        addRequirements(amper);
+    }
+
+    @Override
+    public void initialize() {
+        amper.amp();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return !amper.hasNote();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        amper.stopRoller();
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorToAmpUnsafe.java

@@ -0,0 +1,65 @@
+/************************ PROJECT IZZI *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.conveyor;
+
+import com.stuypulse.robot.commands.amper.AmperToHeight;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Amper.Lift;
+import com.stuypulse.robot.subsystems.amper.Amper;
+import com.stuypulse.robot.subsystems.conveyor.Conveyor;
+import com.stuypulse.robot.subsystems.intake.Intake;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ConveyorToAmpUnsafe extends Command {
+    public static Command withCheckLift() {
+        return AmperToHeight.untilBelow(Lift.MIN_HEIGHT, Lift.MAX_HEIGHT_ERROR)
+            .andThen(new ConveyorToAmp());
+    }
+
+    private final Conveyor conveyor;
+    private final Shooter shooter;
+    private final Intake intake;
+    private final Amper amper;
+
+    public ConveyorToAmpUnsafe() {
+        conveyor = Conveyor.getInstance();
+        shooter = Shooter.getInstance();
+        intake = Intake.getInstance();
+        amper = Amper.getInstance();
+
+        addRequirements(conveyor, intake);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.setTargetSpeeds(Settings.Shooter.HANDOFF);
+    }
+
+    @Override
+    public void execute() {
+        if (shooter.atTargetSpeeds()) {
+            conveyor.toAmp();
+            intake.acquire();
+            amper.fromConveyor();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return amper.hasNote();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        conveyor.stop();
+        // shooter.stop();
+        intake.stop();
+        amper.stopRoller();
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToFerry.java

@@ -0,0 +1,41 @@
+/************************ PROJECT IZZI *************************/
+/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
+/* Use of this source code is governed by an MIT-style license */
+/* that can be found in the repository LICENSE file.           */
+/***************************************************************/
+
+package com.stuypulse.robot.commands.swerve;
+
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class SwerveDriveToFerry extends SwerveDriveToPose {
+
+    public SwerveDriveToFerry() {
+        super(() -> {
+            Pose2d robot = Odometry.getInstance().getPose();
+
+            Translation2d target = Robot.isBlue()
+                ? new Translation2d(0, Field.WIDTH - 1.5)
+                : new Translation2d(0, 1.5);
+
+            Rotation2d targetAngle = new Translation2d(Field.CONST_FERRY_X, robot.getY())
+                .minus(target)
+                .getAngle();
+
+            // use robot's y, put in x and rotation
+
+            return new Pose2d(
+                Field.CONST_FERRY_X,
+                Robot.isBlue() ? Math.max(robot.getY(), Field.FERRY_CUTOFF) : Math.min(robot.getY(), Field.WIDTH - Field.FERRY_CUTOFF),
+                targetAngle
+            );
+        });
+    }
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java

@@ -13,7 +13,6 @@
 import com.stuypulse.stuylib.streams.booleans.BStream;
 import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
 import com.stuypulse.stuylib.streams.numbers.IStream;
-import com.stuypulse.stuylib.streams.numbers.filters.Derivative;
 import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
 import com.pathplanner.lib.util.PIDConstants;
 import com.stuypulse.robot.constants.Field;

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -282,6 +282,9 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria
     double FERRY_SHOT_MIN_X = 6.0;
     double FERRY_SHOT_MIN_FAR_X = 8.5;
 
+    double CONST_FERRY_X = (FERRY_SHOT_MAX_X + FERRY_SHOT_MIN_FAR_X) / 2.0;
+    double FERRY_CUTOFF = 1.8;
+
     /**** EMPTY FIELD POSES ****/
 
     Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d());

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -400,11 +400,12 @@ public interface Alignment {
         SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1);
         SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5);
 
+        SmartNumber FERRY_SHOT_DISTANCE = new SmartNumber("Shooter/Ferry Shot Distance", 7.0);
         SmartNumber PODIUM_SHOT_DISTANCE = new SmartNumber("Shooter/Podium Distance", 2.85); // 2.75 in lab
         double PODIUM_SHOT_MAX_ANGLE = 80;
 
         SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5));
-        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5));
+        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry
 
         SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0));
         SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0));