From dbe098b51bfd4126f2672906fced025b1d606fec Mon Sep 17 00:00:00 2001 From: Azalea H Date: Sat, 6 Apr 2024 09:37:37 -0700 Subject: [PATCH] Import auto code from other branch Co-Authored-By: Aceius E. <144858100+AceiusRedshift@users.noreply.github.com> --- src/main/java/frc/robot/RobotContainer.java | 3 +- src/main/java/frc/robot/commands/Autos.java | 71 +++++++++++++++++++++ 2 files changed, 73 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b49477d..109b833 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -149,7 +149,8 @@ public RobotContainer() { autoChooser.addOption("1+Forward", Autos.shootStartingAuto(drivetrain, arm, intakeShooter, this)); autoChooser.setDefaultOption("2+Forward", Autos.shoot2StartingAuto(drivetrain, arm, intakeShooter, this)); - + autoChooser.addOption("4 note", + Autos.shoot4StartingAuto(drivetrain, arm, intakeShooter, vision, teamChooser, this)); SmartDashboard.putData("Auto Chooser", autoChooser); teamChooser.setDefaultOption("Blue", Alliance.Blue); diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 2e26be6..04ef8b8 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -29,7 +30,10 @@ */ public final class Autos { + final static double speakerDepth = Units.inchesToMeters(36.125); final static double driveDistanceForNote1 = 1.68; + final static Translation2d rightNotePickup = new Translation2d( + Units.inchesToMeters((144 - speakerDepth) / 2) - (Constants.BOT_WIDTH / 2), Units.inchesToMeters(57)); /** Example static factory for an autonomous command. */ public static Command driveAuto(SwerveDrivetrain drivetrain, Translation2d translation) { @@ -95,6 +99,73 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I shootInSpeaker(drivetrain, arm, shooter, cRobotContainer)); } + public static Command shootSideAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, + Vision vision, SendableChooser alliance, boolean isAmpSide, RobotContainer cRobotContainer) + throws Exception { + // If on red, and going for amp side note (right side) keep translation + // If on red, and going for stage side note (left side) mirror translation y + // If on blue, and going for amp side note (left side) mirror translation y + // If on blue, and going for stage side note (right side) keep translation + // if (!vision.isEnabled()) + // throw new UnsupportedOperationException("This auto requires vision!"); + + final int speakerTagId = (alliance.getSelected() == Alliance.Red) ? 4 : 7; + final Translation2d sideNoteTranslation = new Translation2d(rightNotePickup.getX(), + (alliance.getSelected() == Alliance.Red) == isAmpSide ? rightNotePickup.getY() + : -rightNotePickup.getY()); + return Commands.sequence( + intakeFromFloorStart(arm, shooter), + new AutoDriveTo(drivetrain, sideNoteTranslation), + new AutoDriveTo(drivetrain, new Translation2d(0, sideNoteTranslation.getY())), + intakeFromFloorEnd(arm, shooter), + // new AutoDriveTo(drivetrain, sideNoteTranslation.times(-1)), + new AutoDriveTo(drivetrain, + new Translation2d(-sideNoteTranslation.getX() * 2, -sideNoteTranslation.getY())), + // new FollowTag(drivetrain, vision, speakerTagId, + // new Translation2d(-speakerDepth - Constants.BOT_WIDTH / 2, 0)), + shootInSpeaker(drivetrain, arm, shooter, cRobotContainer)); + } + + public static Command shoot3StartingAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Vision vision, + SendableChooser alliance, RobotContainer cRobotContainer) throws Exception { + // final Optional ally = DriverStation.getAlliance(); + + // // if (!vision.isEnabled()) + // // throw new UnsupportedOperationException("This auto requires vision!"); + // if (ally.isEmpty()) + // throw new Exception("DriverStation.getAlliance is not present. Set the + // alliance in the driver station."); + + // boolean redAlliance = ally.get() == Alliance.Red; + + return Commands.sequence( + shoot2StartingAuto(drivetrain, arm, shooter, cRobotContainer), + shootSideAuto(drivetrain, arm, shooter, vision, alliance, true, cRobotContainer)); + + } + + public static Command shoot4StartingAuto(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter, Vision vision, + SendableChooser alliance, RobotContainer container) { + // final Optional ally = DriverStation.getAlliance(); + + // if (!vision.isEnabled()) + // throw new UnsupportedOperationException("This auto requires vision!"); + // if (ally.isEmpty()) + // throw new Exception("DriverStation.getAlliance is not present. Set the + // alliance in the driver station."); + Command opSequence; + try { + opSequence = Commands.sequence( + shoot2StartingAuto(drivetrain, arm, shooter, container), + shootSideAuto(drivetrain, arm, shooter, vision, alliance, true, container), + shootSideAuto(drivetrain, arm, shooter, vision, alliance, false, container)); + } catch (Exception e) { + opSequence = Commands.sequence(); + } + + return opSequence; + } + public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShooter shooter) { return dropInAmp(drivetrain, arm, shooter, null, null); }