From f0e1c669eadd23ac0ca9f30f5e66a191f6ba60de Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 12:48:23 -0700 Subject: [PATCH 1/4] Add target lines --- .../java/frc/robot/utils/autoaim/AutoAim.java | 2 ++ .../frc/robot/utils/autoaim/L1Targets.java | 30 +++++++++++++++++++ 2 files changed, 32 insertions(+) create mode 100644 src/main/java/frc/robot/utils/autoaim/L1Targets.java diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index a249680a..78bdd07e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -39,6 +39,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; 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..5d71fbbf --- /dev/null +++ b/src/main/java/frc/robot/utils/autoaim/L1Targets.java @@ -0,0 +1,30 @@ +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; + +public enum L1Targets { + + BLUE_AB(new Rectangle2d(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + BLUE_CD(new Rectangle2d(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + BLUE_EF(new Rectangle2d(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + BLUE_GH(new Rectangle2d(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + BLUE_IJ(new Rectangle2d(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + BLUE_KL(new Rectangle2d(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + + RED_AB(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + RED_CD(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + RED_EF(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + RED_GH(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + RED_IJ(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), + RED_KL(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)); + + private Rectangle2d line; + + private L1Targets(Rectangle2d line) { + this.line = line; + } + +} From f731a09b4fdd64762c2856a90d25b2036671fc8d Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 16:09:20 -0700 Subject: [PATCH 2/4] Initial testing, nonworking --- src/main/java/frc/robot/Robot.java | 38 +++++- .../subsystems/climber/ClimberIOSim.java | 3 +- .../java/frc/robot/utils/autoaim/AutoAim.java | 74 ++++++++++++ .../frc/robot/utils/autoaim/L1Targets.java | 110 ++++++++++++++---- 4 files changed, 201 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c40b48ce..2131d761 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,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; @@ -77,6 +78,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; @@ -119,7 +121,7 @@ private RobotHardware(SwerveConstants swerveConstants) { } } - public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.REPLAY; + public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; // For replay to work properly this should match the hardware used in the log public static final RobotHardware ROBOT_HARDWARE = RobotHardware.KELPIE; @@ -694,7 +696,7 @@ public Robot() { driver .rightBumper() .or(driver.leftBumper()) - .and(() -> superstructure.stateIsCoralAlike()) + .and(() -> superstructure.stateIsCoralAlike() && currentTarget != ReefTarget.L1) .whileTrue( Commands.parallel( AutoAim.autoAimWithIntermediatePose( @@ -714,6 +716,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() .or(driver.leftBumper()) @@ -979,6 +1007,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/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 3f2ffaf4..05fe3ca5 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -68,7 +68,6 @@ public void resetEncoder(double position) { @Override public void setPosition(double position, double vel) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); + setPosition(position); } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 78bdd07e..3ae9b1da 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; @@ -225,6 +226,79 @@ 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( + () -> { + final var diff = swerve.getPose().minus(cachedTarget[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(), cachedTarget[0].getX()) + + vxController.getSetpoint().velocity, + // Use the inputted y velocity target + vyController.calculate( + swerve.getPose().getY(), cachedTarget[0].getY()) + + vyController.getSetpoint().velocity, + headingController.calculate( + swerve.getPose().getRotation().getRadians(), + cachedTarget[0].getRotation().getRadians()) + + headingController.getSetpoint().velocity) + .plus(new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), 0.0)); + 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 index 5d71fbbf..4edb7e76 100644 --- a/src/main/java/frc/robot/utils/autoaim/L1Targets.java +++ b/src/main/java/frc/robot/utils/autoaim/L1Targets.java @@ -4,27 +4,97 @@ 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)), - BLUE_AB(new Rectangle2d(new Pose2d(3.64, 4.03, Rotation2d.fromDegrees(180)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - BLUE_CD(new Rectangle2d(new Pose2d(4.06, 3.31, Rotation2d.fromDegrees(240)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - BLUE_EF(new Rectangle2d(new Pose2d(4.89, 3.31, Rotation2d.fromDegrees(300)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - BLUE_GH(new Rectangle2d(new Pose2d(5.31, 4.03, Rotation2d.fromDegrees(0)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - BLUE_IJ(new Rectangle2d(new Pose2d(4.89, 4.75, Rotation2d.fromDegrees(60)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - BLUE_KL(new Rectangle2d(new Pose2d(4.06, 4.75, Rotation2d.fromDegrees(120)), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - - RED_AB(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_AB.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - RED_CD(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_CD.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - RED_EF(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_EF.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - RED_GH(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_GH.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - RED_IJ(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_IJ.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)), - RED_KL(new Rectangle2d(ChoreoAllianceFlipUtil.flip(BLUE_KL.line.getCenter()), AutoAim.L1_TROUGH_WIDTH_METERS, 0.0)); - - private Rectangle2d line; - - private L1Targets(Rectangle2d line) { - this.line = line; - } - + 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()), + AutoAim.L1_TROUGH_WIDTH_METERS, + 0.0); + } } From 6bffa4d8c8a9029d99d197c030415d3faa093338 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 16:22:54 -0700 Subject: [PATCH 3/4] Make it mostly work --- src/main/java/frc/robot/utils/autoaim/L1Targets.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/L1Targets.java b/src/main/java/frc/robot/utils/autoaim/L1Targets.java index 4edb7e76..d06bbfe9 100644 --- a/src/main/java/frc/robot/utils/autoaim/L1Targets.java +++ b/src/main/java/frc/robot/utils/autoaim/L1Targets.java @@ -94,7 +94,7 @@ 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()), - AutoAim.L1_TROUGH_WIDTH_METERS, - 0.0); + 0.0, + AutoAim.L1_TROUGH_WIDTH_METERS); } } From c89a2a0d1f8ca20d4027c5c4d2cac93622ff4b40 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 29 Mar 2025 16:53:27 -0700 Subject: [PATCH 4/4] Allow driver to move along the line --- .../java/frc/robot/utils/autoaim/AutoAim.java | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 3ae9b1da..8763dc25 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -267,7 +267,16 @@ public static Command alignToLine( .andThen( swerve.driveVelocityFieldRelative( () -> { - final var diff = swerve.getPose().minus(cachedTarget[0]); + 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)) @@ -275,17 +284,20 @@ public static Command alignToLine( ? new ChassisSpeeds() : new ChassisSpeeds( vxController.calculate( - swerve.getPose().getX(), cachedTarget[0].getX()) + swerve.getPose().getX(), target.get().getX()) + vxController.getSetpoint().velocity, // Use the inputted y velocity target vyController.calculate( - swerve.getPose().getY(), cachedTarget[0].getY()) + swerve.getPose().getY(), target.get().getY()) + vyController.getSetpoint().velocity, headingController.calculate( swerve.getPose().getRotation().getRadians(), - cachedTarget[0].getRotation().getRadians()) + target.get().getRotation().getRadians()) + headingController.getSetpoint().velocity) - .plus(new ChassisSpeeds(xVel.getAsDouble(), yVel.getAsDouble(), 0.0)); + .plus( + ChassisSpeeds.fromRobotRelativeSpeeds( + driverReqSpeedsRobotRelative, swerve.getRotation()) + .times(1.2)); if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput( "AutoAim/Target Pose",