diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29ce4f2..18c56fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -302,6 +302,8 @@ private void configureBindings() { // While the right trigger is held, we will shoot into the hub or ferry. Binding A to the shaking of the shooter. gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()) .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_)); + + gamepad_.a().whileTrue(RobotCommands.shootHubInCorner(shooter_, hopper_, intake_, drivebase_)); gamepad_.rightBumper() .whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed())); diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index ca69ef0..670675f 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -1,6 +1,8 @@ package frc.robot.commands.robot; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.Set; @@ -9,6 +11,7 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Distance; @@ -53,6 +56,56 @@ public static Command shootHub(Shooter shooter, Hopper hopper, IntakeSubsystem i ); } + public static Command shootHubInCorner(Shooter shooter, Hopper hopper, IntakeSubsystem intake, Drive drive) { + return Commands.defer(() -> { + Supplier cornerPose = () -> { + Pose2d rightTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueCornerDepot + : ShooterConstants.Positions.redCornerDepot; + + Pose2d leftTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueCornerOutpost + : ShooterConstants.Positions.redCornerOutpost; + + Pose2d target = + drive.getPose().getY() < ShooterConstants.Positions.centerLineY + ? rightTarget + : leftTarget; + return target; + }; + + Supplier immdPose = () -> { + Pose2d rightTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueCornerDepotWaypoint + : ShooterConstants.Positions.redCornerDepotWaypoint; + + Pose2d leftTarget = + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue + ? ShooterConstants.Positions.blueCornerOutpostWaypoint + : ShooterConstants.Positions.redCornerOutpostWaypoint; + + Pose2d target = + drive.getPose().getY() < ShooterConstants.Positions.centerLineY + ? rightTarget + : leftTarget; + return target; + }; + + Logger.recordOutput("Shooter/CornerDriveTarget", cornerPose.get()); + + if(Meters.of(drive.getPose().getTranslation().getDistance(cornerPose.get().getTranslation())).lt(ShooterConstants.Positions.distFromCornerToShootInCorner)) { + return DriveCommands.simplePathCommand(drive, cornerPose.get(), immdPose.get(), MetersPerSecond.of(3.0), MetersPerSecondPerSecond.of(3.0)) + .andThen(shootHub(shooter, hopper, intake, drive)); + } + + return shootHub(shooter, hopper, intake, drive); + + }, Set.of(shooter, hopper, intake, drive)); + } + /** * Shoots into the hub, without aiming. This should not be used in most situations, * and only when you need additional flexibility like trying to squeeze diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 48d4e5f..d13f7ea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -5,7 +5,10 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Inches; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -49,69 +52,68 @@ public class ShooterConstants { public static final Angle hoodMaxAngle = Degrees.of(75.0) ; public static final Angle hoodMinAngle = Degrees.of(0.0) ; - public class FerryPositions{ - public static final Translation2d blueOutpostTarget= new Translation2d(2.135, 1.639); - public static final Translation2d redOutpostTarget= new Translation2d(14.0,6.0); + public class PID { + // shooter + public static final double shooterkP = 0.5; + public static final double shooterkI = 0.0; + public static final double shooterkD = 0.0; + public static final double shooterkV = 0.132; + public static final double shooterkA = 0.0; + public static final double shooterkG = 0.0; + public static final double shooterkS = 0.0; } - public class PID { - // shooter - public static final double shooterkP = 0.5; - public static final double shooterkI = 0.0; - public static final double shooterkD = 0.0; - public static final double shooterkV = 0.132; - public static final double shooterkA = 0.0; - public static final double shooterkG = 0.0; - public static final double shooterkS = 0.0; - } + public class MotionMagic { - public class MotionMagic { + // shooter + public static final double shooterkMaxVelocity = 1000.0; + public static final double shooterkMaxAcceleration = 3000.0; + public static final double shooterkJerk = 0.0; + } - // shooter - public static final double shooterkMaxVelocity = 1000.0; - public static final double shooterkMaxAcceleration = 3000.0; - public static final double shooterkJerk = 0.0; - } + public class SoftwareLimits { + public static final double hoodMaxAngle = 0.0; + public static final double hoodMinAngle = 0.0; + } - public class SoftwareLimits { - public static final double hoodMaxAngle = 0.0; - public static final double hoodMinAngle = 0.0; - } + public class Positions { + // Field Positions + public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); + public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); - public class Positions { - // Field Positions - public static final Translation2d blueHubPose = new Translation2d(4.5974,4.034536); - public static final Translation2d redHubPose = new Translation2d(11.938,4.034536); + public static final Distance allianceZone = Meters.of(4.5974); - public static final Distance allianceZone = Meters.of(4.5974); + public static final Distance blueAllianceWall = Meters.of(0); + public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); - public static final Distance blueAllianceWall = Meters.of(0); - public static final Distance redAllianceWall = Meters.of(FieldConstants.layout.getFieldLength()); + public static final Translation2d blueTargetLeft = new Translation2d(1,6); + public static final Translation2d blueTargetRight = new Translation2d(1,2); - public static final Translation2d blueTargetLeft = new Translation2d(1,6); - public static final Translation2d blueTargetRight = new Translation2d(1,2); + public static final Translation2d redTargetLeft = new Translation2d(15,6); + public static final Translation2d redTargetRight = new Translation2d(15,2); - public static final Translation2d redTargetLeft = new Translation2d(15,6); - public static final Translation2d redTargetRight = new Translation2d(15,2); + public static final Translation2d blueBumpTargetLeft = new Translation2d(4, FieldConstants.layout.getFieldWidth() - 2.5); + public static final Translation2d blueBumpTargetRight = new Translation2d(4,2.5); - public static final Translation2d blueBumpTargetLeft = new Translation2d(4, FieldConstants.layout.getFieldWidth() - 2.5); - public static final Translation2d blueBumpTargetRight = new Translation2d(4,2.5); + public static final Translation2d redBumpTargetLeft = new Translation2d(FieldConstants.layout.getFieldLength() - 4, FieldConstants.layout.getFieldWidth() - 2.5); + public static final Translation2d redBumpTargetRight = new Translation2d(FieldConstants.layout.getFieldLength() - 4,2.5); - public static final Translation2d redBumpTargetLeft = new Translation2d(FieldConstants.layout.getFieldLength() - 4, FieldConstants.layout.getFieldWidth() - 2.5); - public static final Translation2d redBumpTargetRight = new Translation2d(FieldConstants.layout.getFieldLength() - 4,2.5); + public static final double centerLineY = 4.034536; - public static final double centerLineY = 4.034536; + public static final Pose2d blueCornerDepot = new Pose2d(Inches.of(23.09), Inches.of(23.429), new Rotation2d(Degrees.of(40.417))); + public static final Pose2d blueCornerDepotWaypoint = new Pose2d(Inches.of(34.511), Inches.of(33.155), new Rotation2d(Degrees.of(40.417))); + public static final Pose2d blueCornerOutpost = new Pose2d(Inches.of(23.09), Inches.of(294.261), new Rotation2d(Degrees.of(-40.417))); + public static final Pose2d blueCornerOutpostWaypoint = new Pose2d(Inches.of(34.511), Inches.of(284.535), new Rotation2d(Degrees.of(-40.417))); + public static final Pose2d redCornerDepot = new Pose2d(Inches.of(628.13), Inches.of(23.429), new Rotation2d(Degrees.of(180.0 - 40.417))); + public static final Pose2d redCornerDepotWaypoint = new Pose2d(Inches.of(616.709), Inches.of(33.155), new Rotation2d(Degrees.of(180.0 - 40.417))); + public static final Pose2d redCornerOutpost = new Pose2d(Inches.of(628.13), Inches.of(294.261), new Rotation2d(Degrees.of(180.0 + 40.417))); + public static final Pose2d redCornerOutpostWaypoint = new Pose2d(Inches.of(616.709), Inches.of(284.535), new Rotation2d(Degrees.of(180.0 + 40.417))); + + public static final Distance distFromCornerToShootInCorner = Meters.of(2.0); } public class HoodPWMs { public static final int hoodLeftPWMPort = 2; public static final int hoodRightPWMPort = 0; } - - public class ferryPositions{ - public static final Translation2d blueOutpostFerryTarget= new Translation2d(2.136,1.935); //ferry target for the blue outpost - public static final Translation2d redOutpostFerryTarget= new Translation2d(14.0,6.0); //ferry target for the red outpost - public static final Translation2d blueDepotFerryTarget= new Translation2d(2.0,6.0); //ferry target for the blue depot - public static final Translation2d redDepotFerryTarget= new Translation2d(14.0,2.0); //ferry target for the red depot - } }