From 5d5b61cc7bfc94c5b445ded9f1871e269e1b1465 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Tue, 19 Mar 2024 17:14:48 -0400 Subject: [PATCH 1/3] added auto ferry command Co-authored-by: Lapcas Co-authored-by: alex Co-authored-by: Rahel Arka Co-authored-by: jopy-man Co-authored-by: rzheng287 Co-authored-by: hahafoot Co-authored-by: simon Co-authored-by: urnotkool --- .../commands/conveyor/ConveyorShoot.java | 2 +- .../conveyor/ConveyorShootRoutine.java | 2 +- .../commands/swerve/SwerveDriveAutoFerry.java | 52 +++++++++++++++++++ .../com/stuypulse/robot/constants/Field.java | 23 ++++++++ .../stuypulse/robot/constants/Settings.java | 6 +++ 5 files changed, 83 insertions(+), 2 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java index 22209ede..09d8b4ae 100644 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShoot.java @@ -18,7 +18,7 @@ public class ConveyorShoot extends InstantCommand { public static Command untilDone() { return new ConveyorShoot() - .andThen(new WaitUntilCommand(() -> Shooter.getInstance().noteShot())); + .andThen(new WaitUntilCommand(Shooter.getInstance()::noteShot)); } private final Conveyor conveyor; diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java index bf372641..62ec75fe 100644 --- a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootRoutine.java @@ -16,7 +16,7 @@ public ConveyorShootRoutine() { public ConveyorShootRoutine(double delay) { addCommands( - new InstantCommand(() -> SwerveDrive.getInstance().stop(), SwerveDrive.getInstance()), + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), new ConveyorShoot(), new WaitCommand(delay), new ConveyorStop(), diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java new file mode 100644 index 00000000..6bed2143 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -0,0 +1,52 @@ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.shooter.Shooter; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.input.Gamepad; + +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + + +public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { + + private final SwerveDrive swerve; + private final Odometry odometry; + private final Shooter shooter; + private final Conveyor conveyor; + + public SwerveDriveAutoFerry(Gamepad driver) { + super(driver); + shooter = Shooter.getInstance(); + odometry = Odometry.getInstance(); + swerve = SwerveDrive.getInstance(); + conveyor = Conveyor.getInstance(); + + addRequirements(shooter, odometry, swerve); + } + + @Override + public Rotation2d getTargetAngle() { + return odometry.getPose().getRotation().minus(Field.getOpposingSourcePose().getRotation()); + } + + @Override + public double getDistanceToTarget() { + return odometry.getPose().getTranslation().getDistance(Field.getOpposingSourcePose().getTranslation()); + } + + @Override + public void execute() { + super.execute(); + Pose2d robotPose = odometry.getPose(); + if (robotPose.getX() > Field.getAllianceWingX() && robotPose.getX() < Field.getOpposingWingX()) { + conveyor.toShooter(); + shooter.setTargetSpeeds(Settings.Shooter.FERRY); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 68d32a41..4076ac8a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -9,6 +9,7 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.util.vision.AprilTag; +import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -185,6 +186,28 @@ public static AprilTag getAllianceAmpTag() { return (Robot.isBlue() ? NamedTags.BLUE_AMP : NamedTags.RED_AMP).tag; } + /*** SOURCE ***/ + + public static Pose2d getAllianceSourcePose() { + return (Robot.isBlue() ? NamedTags.BLUE_SOURCE_RIGHT : NamedTags.RED_SOURCE_RIGHT) + .getLocation().toPose2d(); + } + + public static Pose2d getOpposingSourcePose() { + return (Robot.isBlue() ? NamedTags.RED_SOURCE_RIGHT : NamedTags.BLUE_SOURCE_RIGHT) + .getLocation().toPose2d(); + } + + /*** WING ***/ + + public static double getAllianceWingX() { + return 0.0; // xxx: determine this + } + + public static double getOpposingWingX() { + return 0.0; // xxx: determine this + } + /*** TRAP ***/ public static Pose2d[] getAllianceTrapPoses() { diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a489bad9..c72d37ab 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -316,6 +316,12 @@ public interface Shooter { ShooterSpeeds HANDOFF = new ShooterSpeeds(3000, 3000); + // xxx: determine + ShooterSpeeds FERRY = new ShooterSpeeds( + new SmartNumber("Shooter/Ferry Shooter RPM", 5500), + 500, + new SmartNumber("Shooter/Ferry Feeder RPM", 3000)); + double AT_RPM_EPSILON = 125; SmartNumber RPM_CHANGE_RC = new SmartNumber("Shooter/RPM Change RC", 0.2); From dc4a714de6980ada571df7a016818e2b8a8d7f11 Mon Sep 17 00:00:00 2001 From: Naowal Rahman Date: Tue, 19 Mar 2024 17:31:31 -0400 Subject: [PATCH 2/3] add override end method --- .../robot/commands/swerve/SwerveDriveAutoFerry.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index 6bed2143..5792e3bc 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.shooter.Shooter; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; @@ -19,6 +20,7 @@ public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { private final Odometry odometry; private final Shooter shooter; private final Conveyor conveyor; + private final Intake intake; public SwerveDriveAutoFerry(Gamepad driver) { super(driver); @@ -26,6 +28,7 @@ public SwerveDriveAutoFerry(Gamepad driver) { odometry = Odometry.getInstance(); swerve = SwerveDrive.getInstance(); conveyor = Conveyor.getInstance(); + intake = Intake.getInstance(); addRequirements(shooter, odometry, swerve); } @@ -45,8 +48,16 @@ public void execute() { super.execute(); Pose2d robotPose = odometry.getPose(); if (robotPose.getX() > Field.getAllianceWingX() && robotPose.getX() < Field.getOpposingWingX()) { + intake.acquire(); conveyor.toShooter(); shooter.setTargetSpeeds(Settings.Shooter.FERRY); } } + + @Override + public void end(boolean interrupted) { + intake.stop(); + conveyor.stop(); + shooter.stop(); + } } From eb465198af2499f8ad4075dc451e100819fa1261 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Sat, 23 Mar 2024 09:52:19 -0400 Subject: [PATCH 3/3] Fix command, bind to top button --- .../com/stuypulse/robot/RobotContainer.java | 25 +++++++++-------- .../commands/swerve/SwerveDriveAutoFerry.java | 28 +++++++++++++------ .../com/stuypulse/robot/constants/Field.java | 14 +++------- 3 files changed, 37 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 492807bf..e16f8d72 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -185,20 +185,23 @@ private void configureDriverBindings() { .onTrue(new AmperScoreSpeed(-Score.TRAP_SPEED)) .onFalse(new AmperStop()); - driver.getTopButton() - // on command start - .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN))) + // driver.getTopButton() + // // on command start + // .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY) + // .deadlineWith(new LEDSet(LEDInstructions.GREEN))) - .onTrue(new SwerveDriveAutomatic(driver) - // after command end - .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN))) + // .onTrue(new SwerveDriveAutomatic(driver) + // // after command end + // .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) + // .deadlineWith(new LEDSet(LEDInstructions.GREEN))) - .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION)) + // .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION)) - .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) - .deadlineWith(new LEDSet(LEDInstructions.GREEN)))); + // .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY) + // .deadlineWith(new LEDSet(LEDInstructions.GREEN)))); + + driver.getTopButton() + .whileTrue(new SwerveDriveAutoFerry(driver)); driver.getRightButton() .whileTrue(new SwerveDriveToClimb()); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java index 5792e3bc..ec444ac6 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAutoFerry.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.commands.swerve; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.conveyor.Conveyor; import com.stuypulse.robot.subsystems.intake.Intake; @@ -10,13 +11,12 @@ import com.stuypulse.robot.constants.Settings; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { - private final SwerveDrive swerve; private final Odometry odometry; private final Shooter shooter; private final Conveyor conveyor; @@ -24,33 +24,43 @@ public class SwerveDriveAutoFerry extends SwerveDriveDriveAligned { public SwerveDriveAutoFerry(Gamepad driver) { super(driver); + shooter = Shooter.getInstance(); odometry = Odometry.getInstance(); - swerve = SwerveDrive.getInstance(); conveyor = Conveyor.getInstance(); intake = Intake.getInstance(); - addRequirements(shooter, odometry, swerve); + addRequirements(shooter, odometry); + } + + // returns pose of close amp corner + private Translation2d getTargetPose() { + return Robot.isBlue() + ? new Translation2d(0, Field.WIDTH) + : new Translation2d(0, 0); } @Override public Rotation2d getTargetAngle() { - return odometry.getPose().getRotation().minus(Field.getOpposingSourcePose().getRotation()); + return getTargetPose().minus(odometry.getPose().getTranslation()).getAngle(); } @Override public double getDistanceToTarget() { - return odometry.getPose().getTranslation().getDistance(Field.getOpposingSourcePose().getTranslation()); + return getTargetPose().getDistance(odometry.getPose().getTranslation()); } @Override public void execute() { super.execute(); - Pose2d robotPose = odometry.getPose(); - if (robotPose.getX() > Field.getAllianceWingX() && robotPose.getX() < Field.getOpposingWingX()) { + + if (odometry.getPose().getX() < Field.FERRY_SHOT_THRESHOLD_X) { intake.acquire(); conveyor.toShooter(); shooter.setTargetSpeeds(Settings.Shooter.FERRY); + } else { + intake.stop(); + conveyor.stop(); } } @@ -58,6 +68,6 @@ public void execute() { public void end(boolean interrupted) { intake.stop(); conveyor.stop(); - shooter.stop(); + shooter.setTargetSpeeds(Settings.Shooter.PODIUM_SHOT); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 4076ac8a..831bad4f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -198,16 +198,6 @@ public static Pose2d getOpposingSourcePose() { .getLocation().toPose2d(); } - /*** WING ***/ - - public static double getAllianceWingX() { - return 0.0; // xxx: determine this - } - - public static double getOpposingWingX() { - return 0.0; // xxx: determine this - } - /*** TRAP ***/ public static Pose2d[] getAllianceTrapPoses() { @@ -280,6 +270,10 @@ private static boolean pointInTriangle(Translation2d point, Translation2d[] tria double NOTE_BOUNDARY = LENGTH / 2 + Units.inchesToMeters(Settings.LENGTH / 2); + /*** FERRYING ***/ + + double FERRY_SHOT_THRESHOLD_X = 9.0; + /**** EMPTY FIELD POSES ****/ Pose2d EMPTY_FIELD_POSE2D = new Pose2d(new Translation2d(-1, -1), new Rotation2d());