diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 241386b2..ce4aa7ad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -25,6 +25,7 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; @@ -79,6 +80,7 @@ import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.CageTargets; import frc.robot.utils.autoaim.CoralTargets; +import frc.robot.utils.autoaim.L1Targets; import java.util.HashMap; import java.util.Optional; import java.util.Set; @@ -805,7 +807,7 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike()) + .and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -825,6 +827,32 @@ public Robot() { AutoAim.INITIAL_REEF_KEEPOFF_DISTANCE_METERS, 0.0, Rotation2d.kZero)), Commands.waitUntil(() -> AutoAim.isInToleranceCoral(swerve.getPose())) .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + + driver + .rightBumper() + .or(driver.leftBumper()) + .and(() -> superstructure.stateIsCoralAlike() && currentTarget == ReefTarget.L1) + .whileTrue( + Commands.parallel( + AutoAim.alignToLine( + swerve, + () -> + modifyJoystick(driver.getLeftY()) + * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + () -> + modifyJoystick(driver.getLeftX()) + * ROBOT_HARDWARE.swerveConstants.getMaxLinearSpeed(), + () -> L1Targets.getNearestLine(swerve.getPose())), + Commands.waitUntil( + () -> + AutoAim.isInTolerance( + swerve.getPose(), + new Pose2d( + L1Targets.getNearestLine(swerve.getPose()) + .nearest(swerve.getPose().getTranslation()), + L1Targets.getNearestLine(swerve.getPose()).getRotation()))) + .andThen(driver.rumbleCmd(1.0, 1.0).withTimeout(0.75).asProxy()))); + driver .rightBumper() .and( @@ -1093,6 +1121,12 @@ public Robot() { Stream.of(CageTargets.values()) .map((target) -> target.getLocation()) .toArray(Pose2d[]::new)); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Targets/L1", + Stream.of(L1Targets.values()) + .map((target) -> L1Targets.getRobotTargetLine(target.line)) + .toArray(Rectangle2d[]::new)); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( "AutoAim/Targets/Algae", diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 078fa79b..f989da2e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -39,6 +40,8 @@ public class AutoAim { public static Pose2d RED_PROCESSOR_POS = ChoreoAllianceFlipUtil.flip(BLUE_PROCESSOR_POS); public static List PROCESSOR_POSES = List.of(BLUE_PROCESSOR_POS, RED_PROCESSOR_POS); + public static final double L1_TROUGH_WIDTH_METERS = 0.935; + public static final double TRANSLATION_TOLERANCE_METERS = Units.inchesToMeters(2.0); public static final double ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(2.0); public static final double VELOCITY_TOLERANCE_METERSPERSECOND = 0.5; @@ -223,6 +226,91 @@ public static Command translateToXCoord( })); } + public static Command alignToLine( + SwerveSubsystem swerve, + DoubleSupplier xVel, + DoubleSupplier yVel, + Supplier line) { + final Pose2d cachedTarget[] = {new Pose2d()}; + final ProfiledPIDController headingController = + // assume we can accelerate to max in 2/3 of a second + new ProfiledPIDController( + Robot.ROBOT_HARDWARE.swerveConstants.getHeadingVelocityKP(), + 0.0, + 0.0, + new TrapezoidProfile.Constraints(MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCELERATION)); + headingController.enableContinuousInput(-Math.PI, Math.PI); + final ProfiledPIDController vxController = + new ProfiledPIDController( + 10.0, + 0.01, + 0.02, + new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION)); + final ProfiledPIDController vyController = + new ProfiledPIDController( + 10.0, + 0.01, + 0.02, + new TrapezoidProfile.Constraints(MAX_AUTOAIM_SPEED, MAX_AUTOAIM_ACCELERATION)); + return Commands.runOnce( + () -> { + cachedTarget[0] = + new Pose2d( + line.get().nearest(swerve.getPose().getTranslation()), + line.get().getRotation()); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("AutoAim/Cached Target", cachedTarget[0]); + headingController.reset(swerve.getPose().getRotation().getRadians(), 0.0); + vxController.reset(swerve.getPose().getX(), 0.0); + vyController.reset(swerve.getPose().getY(), 0.0); + }) + .andThen( + swerve.driveVelocityFieldRelative( + () -> { + Supplier target = + () -> + new Pose2d( + line.get().nearest(swerve.getPose().getTranslation()), + line.get().getRotation()); + final var diff = swerve.getPose().minus(target.get()); + var driverReqSpeedsRobotRelative = + ChassisSpeeds.fromFieldRelativeSpeeds( + xVel.getAsDouble(), yVel.getAsDouble(), 0.0, swerve.getRotation()); + driverReqSpeedsRobotRelative.vxMetersPerSecond = 0; + final var speeds = + MathUtil.isNear(0.0, diff.getX(), Units.inchesToMeters(0.25)) + && MathUtil.isNear(0.0, diff.getY(), Units.inchesToMeters(0.25)) + && MathUtil.isNear(0.0, diff.getRotation().getDegrees(), 0.5) + ? new ChassisSpeeds() + : new ChassisSpeeds( + vxController.calculate( + swerve.getPose().getX(), target.get().getX()) + + vxController.getSetpoint().velocity, + // Use the inputted y velocity target + vyController.calculate( + swerve.getPose().getY(), target.get().getY()) + + vyController.getSetpoint().velocity, + headingController.calculate( + swerve.getPose().getRotation().getRadians(), + target.get().getRotation().getRadians()) + + headingController.getSetpoint().velocity) + .plus( + ChassisSpeeds.fromRobotRelativeSpeeds( + driverReqSpeedsRobotRelative, swerve.getRotation()) + .times(1.2)); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "AutoAim/Target Pose", + new Pose2d( + vxController.getSetpoint().position, + vyController.getSetpoint().position, + Rotation2d.fromRadians(headingController.getSetpoint().position))); + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput("AutoAim/Target Speeds", speeds); + return speeds; + })); + } + public static Command approachAlgae( SwerveSubsystem swerve, Supplier target, double approachSpeed) { // This feels like a horrible way of getting around lambda final requirements diff --git a/src/main/java/frc/robot/utils/autoaim/L1Targets.java b/src/main/java/frc/robot/utils/autoaim/L1Targets.java new file mode 100644 index 00000000..d06bbfe9 --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/L1Targets.java @@ -0,0 +1,100 @@ +package frc.robot.utils.autoaim; + +import choreo.util.ChoreoAllianceFlipUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import frc.robot.Robot; +import java.util.Arrays; +import java.util.List; + +public enum L1Targets { + BLUE_AB( + new Rectangle2d( + new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_CD( + new Rectangle2d( + new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_EF( + new Rectangle2d( + new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_GH( + new Rectangle2d( + new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_IJ( + new Rectangle2d( + new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), 0.0, AutoAim.L1_TROUGH_WIDTH_METERS)), + BLUE_KL( + new Rectangle2d( + new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + + RED_AB( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_CD( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_EF( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_GH( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_IJ( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)), + RED_KL( + new Rectangle2d( + ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS)); + + public Rectangle2d line; + + private L1Targets(Rectangle2d line) { + this.line = line; + } + + private static final List transformedLines = + Arrays.stream(values()) + .map( + (L1Targets targets) -> { + return L1Targets.getRobotTargetLine(targets.line); + }) + .toList(); + + public static Rectangle2d getRobotTargetLine(Rectangle2d original) { + return original.transformBy( + new Transform2d( + Robot.ROBOT_HARDWARE.swerveConstants.getBumperLength() / 2, + 0, + Rotation2d.fromDegrees(180.0))); + } + + public static Rectangle2d getNearestLine(Pose2d pose) { + // It feels like there should be a better way to do this + return new Rectangle2d( + pose.nearest(transformedLines.stream().map(line -> line.getCenter()).toList()), + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS); + } +}