From 32d4e4e9ba9c49bd7efef07e802a745bbb9d8636 Mon Sep 17 00:00:00 2001 From: Elvis Date: Sat, 25 Jan 2025 16:19:50 -0500 Subject: [PATCH 01/14] added driver logic --- src/main/java/frc/robot/Robot.java | 17 ++++++- .../java/frc/robot/subsystems/Swerve.java | 47 ++++++++++++++++++- 2 files changed, 60 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9339d20..ca1787c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,8 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Manipulator; import frc.robot.subsystems.Manipulator.Piece; +import frc.robot.subsystems.Swerve.DesiredLocation; +import frc.robot.subsystems.Swerve.SideOffset; import frc.robot.subsystems.Serializer; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Wristevator; @@ -138,6 +140,9 @@ public Robot(NetworkTableInstance ntInst) { .and(() -> _manipulator.getFeedDirection() == -1) .onTrue(runOnce(() -> _currentPiece = Piece.CORAL)); + + new Trigger(_driverController.leftTrigger().or(_driverController.rightTrigger()).onFalse(runOnce(() -> _swerve.setOffset(SideOffset.NONE)))); + SmartDashboard.putData( "Robot Self Check", sequence( @@ -203,10 +208,18 @@ private void configureDefaultCommands() { } private void configureDriverBindings() { - _driverController.x().whileTrue(_swerve.brake()); - _driverController.a().onTrue(_swerve.toggleFieldOriented()); + _driverController.a().whileTrue(_swerve.brake()); + _driverController.x().onTrue(_swerve.toggleFieldOriented()); _driverController.y().onTrue(_swerve.resetHeading()); + _driverController.povUp().whileTrue(_swerve.driveTo(DesiredLocation.REEF.getPose())); + _driverController.povLeft().whileTrue(_swerve.driveTo(DesiredLocation.HUMAN.getPose())); + _driverController.povRight().whileTrue(_swerve.driveTo(DesiredLocation.PROCESSOR.getPose())); + + _driverController.leftTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.LEFT))); + _driverController.rightTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.RIGHT))); + + // _driverController // .b() // .whileTrue(_swerve.driveTo(new Pose2d(10, 3, Rotation2d.fromDegrees(-150)))); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6a2e3f1..c02a82b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,10 +24,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -47,6 +50,7 @@ import frc.robot.Constants.VisionConstants; import frc.robot.Robot; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.Manipulator.Piece; import frc.robot.utils.HolonomicController; import frc.robot.utils.SysId; import frc.robot.utils.VisionPoseEstimator; @@ -134,6 +138,39 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem, SelfChec private double _lastSimTime = 0; private Notifier _simNotifier; + public static enum DesiredLocation { + HUMAN(new Pose2d(0, 0, Rotation2d.kZero)), + PROCESSOR(new Pose2d(0, 0, Rotation2d.kZero)), + REEF(new Pose2d(0, 0, Rotation2d.kZero)); + + Pose2d _pose; + + private DesiredLocation (Pose2d pose){ + _pose = pose; + } + + public Pose2d getPose() { + return _pose; } + } + + public static enum SideOffset { + NONE(new Transform2d(Translation2d.kZero, Rotation2d.kZero)), + LEFT(new Transform2d(5, 3, Rotation2d.kZero)), + RIGHT(new Transform2d(3, 5, Rotation2d.kZero)); + + private final Transform2d _offset; + + private SideOffset(Transform2d offset){ + _offset = offset; + } + + public Transform2d getOffset() { + return _offset; + } + } + + private SideOffset _sideOffset = SideOffset.NONE; + @Logged(name = "Driver Chassis Speeds") private final ChassisSpeeds _driverChassisSpeeds = new ChassisSpeeds(); @@ -341,6 +378,11 @@ public Command resetHeading() { }); } + /** What offset the desried location should use */ + public void setOffset(SideOffset offset) { + _sideOffset = offset; + } + /** * Creates a new Command that drives the chassis. * @@ -430,7 +472,8 @@ public void followTrajectory(SwerveSample sample) { /** Drives the robot in a straight line to some given goal pose. */ public Command driveTo(Pose2d goalPose) { return run(() -> { - ChassisSpeeds speeds = _poseController.calculate(getPose(), goalPose); + Pose2d offsetGoalPose = goalPose.plus(_sideOffset.getOffset()); + ChassisSpeeds speeds = _poseController.calculate(getPose(), offsetGoalPose); setControl(_fieldSpeedsRequest.withSpeeds(speeds)); }) @@ -438,7 +481,7 @@ public Command driveTo(Pose2d goalPose) { () -> _poseController.reset( getPose(), - goalPose, + goalPose.plus(_sideOffset.getOffset()), ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getHeading()))) .until(_poseController::atGoal) .withName("Drive To"); From 01754dfdf556838bfb502402690bdabe9926b8cf Mon Sep 17 00:00:00 2001 From: Elvis Date: Sat, 25 Jan 2025 16:22:35 -0500 Subject: [PATCH 02/14] spotless --- src/main/java/frc/robot/Robot.java | 12 +++++++----- src/main/java/frc/robot/subsystems/Swerve.java | 10 +++++----- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca1787c..dcbe6fd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,10 +41,10 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Manipulator; import frc.robot.subsystems.Manipulator.Piece; -import frc.robot.subsystems.Swerve.DesiredLocation; -import frc.robot.subsystems.Swerve.SideOffset; import frc.robot.subsystems.Serializer; import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.Swerve.DesiredLocation; +import frc.robot.subsystems.Swerve.SideOffset; import frc.robot.subsystems.Wristevator; /** @@ -140,8 +140,11 @@ public Robot(NetworkTableInstance ntInst) { .and(() -> _manipulator.getFeedDirection() == -1) .onTrue(runOnce(() -> _currentPiece = Piece.CORAL)); - - new Trigger(_driverController.leftTrigger().or(_driverController.rightTrigger()).onFalse(runOnce(() -> _swerve.setOffset(SideOffset.NONE)))); + new Trigger( + _driverController + .leftTrigger() + .or(_driverController.rightTrigger()) + .onFalse(runOnce(() -> _swerve.setOffset(SideOffset.NONE)))); SmartDashboard.putData( "Robot Self Check", @@ -219,7 +222,6 @@ private void configureDriverBindings() { _driverController.leftTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.LEFT))); _driverController.rightTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.RIGHT))); - // _driverController // .b() // .whileTrue(_swerve.driveTo(new Pose2d(10, 3, Rotation2d.fromDegrees(-150)))); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c02a82b..24e47ff 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -50,7 +50,6 @@ import frc.robot.Constants.VisionConstants; import frc.robot.Robot; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.subsystems.Manipulator.Piece; import frc.robot.utils.HolonomicController; import frc.robot.utils.SysId; import frc.robot.utils.VisionPoseEstimator; @@ -145,12 +144,13 @@ public static enum DesiredLocation { Pose2d _pose; - private DesiredLocation (Pose2d pose){ + private DesiredLocation(Pose2d pose) { _pose = pose; } public Pose2d getPose() { - return _pose; } + return _pose; + } } public static enum SideOffset { @@ -160,7 +160,7 @@ public static enum SideOffset { private final Transform2d _offset; - private SideOffset(Transform2d offset){ + private SideOffset(Transform2d offset) { _offset = offset; } @@ -380,7 +380,7 @@ public Command resetHeading() { /** What offset the desried location should use */ public void setOffset(SideOffset offset) { - _sideOffset = offset; + _sideOffset = offset; } /** From d22d5b02de54bb8b0a729e50ff100518ab1b844a Mon Sep 17 00:00:00 2001 From: Elvis Osmanov Date: Sun, 26 Jan 2025 21:48:12 -0500 Subject: [PATCH 03/14] added logic for reef offsets --- src/main/java/frc/robot/Constants.java | 25 ++++++++ src/main/java/frc/robot/Robot.java | 9 +-- .../java/frc/robot/subsystems/Swerve.java | 62 +++++++++++++------ src/test/java/frc/robot/SwerveTest.java | 2 +- 4 files changed, 74 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 11762b9..129ec60 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,8 +8,10 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.AngularAccelerationUnit; import edu.wpi.first.units.AngularVelocityUnit; @@ -24,6 +26,7 @@ import edu.wpi.first.units.measure.Per; import frc.robot.generated.TunerConstants; import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimatorConstants; +import java.util.HashMap; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -46,6 +49,28 @@ public static class Ports { public static class FieldConstants { public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + public static final Translation2d reefCenter = + new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters)); + public static final Translation2d reefAlignCenter = + new Translation2d(Inches.of(101.3).in(Meters), Inches.of(158.5).in(Meters)); + public static final Translation2d reefAlignLeft = + new Translation2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters)); + public static final Translation2d reefAlignRight = + new Translation2d(Inches.of(101.3).in(Meters), Inches.of(146.5).in(Meters)); + + // Use these rotations to find the goal postion and rotations + public static final HashMap aprilTagAlignment = + new HashMap<>() { + { + put(17, new Rotation2d(Degrees.of(60))); + put(18, Rotation2d.kZero); + put(19, new Rotation2d(Degrees.of(300))); + put(20, new Rotation2d(Degrees.of(240))); + put(21, Rotation2d.k180deg); + put(22, new Rotation2d(Degrees.of(120))); + } + }; } public static class VisionConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dcbe6fd..daac249 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -43,7 +43,6 @@ import frc.robot.subsystems.Manipulator.Piece; import frc.robot.subsystems.Serializer; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Swerve.DesiredLocation; import frc.robot.subsystems.Swerve.SideOffset; import frc.robot.subsystems.Wristevator; @@ -215,9 +214,11 @@ private void configureDriverBindings() { _driverController.x().onTrue(_swerve.toggleFieldOriented()); _driverController.y().onTrue(_swerve.resetHeading()); - _driverController.povUp().whileTrue(_swerve.driveTo(DesiredLocation.REEF.getPose())); - _driverController.povLeft().whileTrue(_swerve.driveTo(DesiredLocation.HUMAN.getPose())); - _driverController.povRight().whileTrue(_swerve.driveTo(DesiredLocation.PROCESSOR.getPose())); + _driverController.povUp().whileTrue(_swerve.driveToPose(_swerve::getReefOffset)); + // _driverController.povLeft().whileTrue(_swerve.driveToPose(() -> + // DesiredLocation.HUMAN.getPose().plus(_swerve.getOffset()))); + // _driverController.povRight().whileTrue(_swerve.driveToPose(() -> + // DesiredLocation.PROCESSOR.getPose().plus(_swerve.getOffset()))); _driverController.leftTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.LEFT))); _driverController.rightTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.RIGHT))); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 24e47ff..f1aecc4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,8 +24,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -58,6 +56,7 @@ import java.util.HashSet; import java.util.List; import java.util.Set; +import java.util.function.Supplier; import org.photonvision.simulation.VisionSystemSim; @Logged(strategy = Strategy.OPT_IN) @@ -154,19 +153,9 @@ public Pose2d getPose() { } public static enum SideOffset { - NONE(new Transform2d(Translation2d.kZero, Rotation2d.kZero)), - LEFT(new Transform2d(5, 3, Rotation2d.kZero)), - RIGHT(new Transform2d(3, 5, Rotation2d.kZero)); - - private final Transform2d _offset; - - private SideOffset(Transform2d offset) { - _offset = offset; - } - - public Transform2d getOffset() { - return _offset; - } + NONE, + LEFT, + RIGHT } private SideOffset _sideOffset = SideOffset.NONE; @@ -383,6 +372,42 @@ public void setOffset(SideOffset offset) { _sideOffset = offset; } + /** Return the current side reef offset */ + public Pose2d getReefOffset() { + Pose2d offset = new Pose2d(); + + // The value in the hashmap should be the apriltag that we are currently moving towards + Rotation2d rotation = FieldConstants.aprilTagAlignment.get(18); + + switch (_sideOffset) { + case NONE: + offset = + new Pose2d( + FieldConstants.reefAlignCenter.rotateAround(FieldConstants.reefCenter, rotation), + rotation); + break; + + case LEFT: + offset = + new Pose2d( + FieldConstants.reefAlignLeft.rotateAround(FieldConstants.reefCenter, rotation), + rotation); + break; + + case RIGHT: + offset = + new Pose2d( + FieldConstants.reefAlignRight.rotateAround(FieldConstants.reefCenter, rotation), + rotation); + break; + + default: + break; + } + + return offset; + } + /** * Creates a new Command that drives the chassis. * @@ -470,10 +495,9 @@ public void followTrajectory(SwerveSample sample) { } /** Drives the robot in a straight line to some given goal pose. */ - public Command driveTo(Pose2d goalPose) { + public Command driveToPose(Supplier goalPose) { return run(() -> { - Pose2d offsetGoalPose = goalPose.plus(_sideOffset.getOffset()); - ChassisSpeeds speeds = _poseController.calculate(getPose(), offsetGoalPose); + ChassisSpeeds speeds = _poseController.calculate(getPose(), goalPose.get()); setControl(_fieldSpeedsRequest.withSpeeds(speeds)); }) @@ -481,7 +505,7 @@ public Command driveTo(Pose2d goalPose) { () -> _poseController.reset( getPose(), - goalPose.plus(_sideOffset.getOffset()), + goalPose.get(), ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getHeading()))) .until(_poseController::atGoal) .withName("Drive To"); diff --git a/src/test/java/frc/robot/SwerveTest.java b/src/test/java/frc/robot/SwerveTest.java index 78dc6a3..672d30a 100644 --- a/src/test/java/frc/robot/SwerveTest.java +++ b/src/test/java/frc/robot/SwerveTest.java @@ -30,7 +30,7 @@ public void driveTo() { // TODO: ts don't work var goal = new Pose2d(0.5, 0, Rotation2d.fromDegrees(5)); - runToCompletion(_swerve.driveTo(goal)); + runToCompletion(_swerve.driveToPose(() -> goal)); assertEquals(goal.getX(), _swerve.getPose().getX(), 0.2); assertEquals(goal.getY(), _swerve.getPose().getY(), 0.2); From b4ff667ae7a2a8c23f0b0bc5fc8654414a849cad Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Mon, 3 Feb 2025 19:04:46 -0500 Subject: [PATCH 04/14] pose alignment start --- src/main/java/frc/robot/utils/AlignPoses.java | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 src/main/java/frc/robot/utils/AlignPoses.java diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java new file mode 100644 index 0000000..77abd13 --- /dev/null +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils; + +import edu.wpi.first.math.geometry.Pose2d; + +/** Poses for alignment. */ +public class AlignPoses { + private final Pose2d _left; + private final Pose2d _center; + private final Pose2d _right; + + public Pose2d getLeft() { + return _left; + } + + public Pose2d getCenter() { + return _center; + } + + public Pose2d getRight() { + return _right; + } + + /** Setup align poses for three positions */ + public AlignPoses(Pose2d left, Pose2d center, Pose2d right) { + _left = left; + _center = center; + _right = right; + } + + /** Setup align pose for center */ + public AlignPoses(Pose2d center) { + this(center, center, center); + } +} From ff36de8092e162730d9101672d3d6bf16540d924 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Tue, 4 Feb 2025 19:08:47 -0500 Subject: [PATCH 05/14] it works [SwerveBase] renamed driveToPose to driveTo --- src/main/java/frc/robot/Constants.java | 28 ++---- src/main/java/frc/robot/Robot.java | 37 ++++---- .../java/frc/robot/subsystems/Swerve.java | 93 ++++++++----------- src/main/java/frc/robot/utils/AlignPoses.java | 35 +++++++ src/test/java/frc/robot/SwerveTest.java | 2 +- 5 files changed, 101 insertions(+), 94 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 129ec60..05e74b7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -25,8 +26,8 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Per; import frc.robot.generated.TunerConstants; +import frc.robot.utils.AlignPoses; import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimatorConstants; -import java.util.HashMap; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -52,25 +53,12 @@ public static class FieldConstants { public static final Translation2d reefCenter = new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters)); - public static final Translation2d reefAlignCenter = - new Translation2d(Inches.of(101.3).in(Meters), Inches.of(158.5).in(Meters)); - public static final Translation2d reefAlignLeft = - new Translation2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters)); - public static final Translation2d reefAlignRight = - new Translation2d(Inches.of(101.3).in(Meters), Inches.of(146.5).in(Meters)); - - // Use these rotations to find the goal postion and rotations - public static final HashMap aprilTagAlignment = - new HashMap<>() { - { - put(17, new Rotation2d(Degrees.of(60))); - put(18, Rotation2d.kZero); - put(19, new Rotation2d(Degrees.of(300))); - put(20, new Rotation2d(Degrees.of(240))); - put(21, Rotation2d.k180deg); - put(22, new Rotation2d(Degrees.of(120))); - } - }; + + public static final AlignPoses reef = + new AlignPoses( + new Pose2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters), Rotation2d.kZero), + new Pose2d(Inches.of(101.3).in(Meters), Inches.of(158.5).in(Meters), Rotation2d.kZero), + new Pose2d(Inches.of(101.3).in(Meters), Inches.of(146.5).in(Meters), Rotation2d.kZero)); } public static class VisionConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index daac249..7ab6148 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.FaultLogger; import frc.lib.InputStream; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ManipulatorConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.SwerveConstants; @@ -43,8 +44,9 @@ import frc.robot.subsystems.Manipulator.Piece; import frc.robot.subsystems.Serializer; import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Swerve.SideOffset; import frc.robot.subsystems.Wristevator; +import frc.robot.utils.AlignPoses; +import frc.robot.utils.AlignPoses.AlignSide; /** * The methods in this class are called automatically corresponding to each mode, as described in @@ -139,12 +141,6 @@ public Robot(NetworkTableInstance ntInst) { .and(() -> _manipulator.getFeedDirection() == -1) .onTrue(runOnce(() -> _currentPiece = Piece.CORAL)); - new Trigger( - _driverController - .leftTrigger() - .or(_driverController.rightTrigger()) - .onFalse(runOnce(() -> _swerve.setOffset(SideOffset.NONE)))); - SmartDashboard.putData( "Robot Self Check", sequence( @@ -209,23 +205,26 @@ private void configureDefaultCommands() { .scale(WristevatorConstants.maxWristSpeed.in(RadiansPerSecond)))); } + private void alignmentTriggers(Trigger button, AlignPoses poses) { + button + .and(_driverController.leftTrigger().and(_driverController.rightTrigger().negate())) + .whileTrue(_swerve.alignTo(poses, AlignSide.LEFT)); + + button + .and(_driverController.leftTrigger().and(_driverController.rightTrigger()).negate()) + .whileTrue(_swerve.alignTo(poses, AlignSide.CENTER)); + + button + .and(_driverController.rightTrigger().and(_driverController.leftTrigger().negate())) + .whileTrue(_swerve.alignTo(poses, AlignSide.RIGHT)); + } + private void configureDriverBindings() { _driverController.a().whileTrue(_swerve.brake()); _driverController.x().onTrue(_swerve.toggleFieldOriented()); _driverController.y().onTrue(_swerve.resetHeading()); - _driverController.povUp().whileTrue(_swerve.driveToPose(_swerve::getReefOffset)); - // _driverController.povLeft().whileTrue(_swerve.driveToPose(() -> - // DesiredLocation.HUMAN.getPose().plus(_swerve.getOffset()))); - // _driverController.povRight().whileTrue(_swerve.driveToPose(() -> - // DesiredLocation.PROCESSOR.getPose().plus(_swerve.getOffset()))); - - _driverController.leftTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.LEFT))); - _driverController.rightTrigger().whileTrue(run(() -> _swerve.setOffset(SideOffset.RIGHT))); - - // _driverController - // .b() - // .whileTrue(_swerve.driveTo(new Pose2d(10, 3, Rotation2d.fromDegrees(-150)))); + alignmentTriggers(_driverController.povDown(), FieldConstants.reef); } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f1aecc4..853773d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -28,12 +28,12 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.FaultLogger; @@ -48,6 +48,8 @@ import frc.robot.Constants.VisionConstants; import frc.robot.Robot; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.utils.AlignPoses; +import frc.robot.utils.AlignPoses.AlignSide; import frc.robot.utils.HolonomicController; import frc.robot.utils.SysId; import frc.robot.utils.VisionPoseEstimator; @@ -56,7 +58,6 @@ import java.util.HashSet; import java.util.List; import java.util.Set; -import java.util.function.Supplier; import org.photonvision.simulation.VisionSystemSim; @Logged(strategy = Strategy.OPT_IN) @@ -152,14 +153,6 @@ public Pose2d getPose() { } } - public static enum SideOffset { - NONE, - LEFT, - RIGHT - } - - private SideOffset _sideOffset = SideOffset.NONE; - @Logged(name = "Driver Chassis Speeds") private final ChassisSpeeds _driverChassisSpeeds = new ChassisSpeeds(); @@ -172,6 +165,8 @@ public static enum SideOffset { @Logged(name = "Ignore Vision Estimates") private boolean _ignoreVisionEstimates = true; // for sim for now + private AlignPoses _alignGoal = new AlignPoses(Pose2d.kZero); + private HolonomicController _poseController = new HolonomicController(); @Logged(name = VisionConstants.blueArducamName) @@ -367,47 +362,6 @@ public Command resetHeading() { }); } - /** What offset the desried location should use */ - public void setOffset(SideOffset offset) { - _sideOffset = offset; - } - - /** Return the current side reef offset */ - public Pose2d getReefOffset() { - Pose2d offset = new Pose2d(); - - // The value in the hashmap should be the apriltag that we are currently moving towards - Rotation2d rotation = FieldConstants.aprilTagAlignment.get(18); - - switch (_sideOffset) { - case NONE: - offset = - new Pose2d( - FieldConstants.reefAlignCenter.rotateAround(FieldConstants.reefCenter, rotation), - rotation); - break; - - case LEFT: - offset = - new Pose2d( - FieldConstants.reefAlignLeft.rotateAround(FieldConstants.reefCenter, rotation), - rotation); - break; - - case RIGHT: - offset = - new Pose2d( - FieldConstants.reefAlignRight.rotateAround(FieldConstants.reefCenter, rotation), - rotation); - break; - - default: - break; - } - - return offset; - } - /** * Creates a new Command that drives the chassis. * @@ -494,10 +448,41 @@ public void followTrajectory(SwerveSample sample) { .withWheelForceFeedforwardsY(sample.moduleForcesY())); } + /** + * Aligns to a {@link AlignPoses} to the correct side. + * + * @return Drive to the correct pose. + */ + public Command alignTo(AlignPoses alignGoal, AlignSide side) { + return runOnce( + () -> { + Pose2d pose = getPose(); + + if (alignGoal == FieldConstants.reef) { + double minDistance = Double.MAX_VALUE; + + for (int i = 0; i < 6; i++) { + var rotated = + alignGoal.rotateAround( + FieldConstants.reefCenter, Rotation2d.fromDegrees(60).times(i)); + + if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) { + _alignGoal = rotated; + minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm(); + } + } + } + + DogLog.log("Auto/Align Pose", _alignGoal.getPose(side)); + }) + .andThen(new DeferredCommand(() -> driveTo(_alignGoal.getPose(side)), Set.of(this))) + .withName("Align To"); + } + /** Drives the robot in a straight line to some given goal pose. */ - public Command driveToPose(Supplier goalPose) { + public Command driveTo(Pose2d goalPose) { return run(() -> { - ChassisSpeeds speeds = _poseController.calculate(getPose(), goalPose.get()); + ChassisSpeeds speeds = _poseController.calculate(getPose(), goalPose); setControl(_fieldSpeedsRequest.withSpeeds(speeds)); }) @@ -505,7 +490,7 @@ public Command driveToPose(Supplier goalPose) { () -> _poseController.reset( getPose(), - goalPose.get(), + goalPose, ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getHeading()))) .until(_poseController::atGoal) .withName("Drive To"); diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java index 77abd13..40c81ad 100644 --- a/src/main/java/frc/robot/utils/AlignPoses.java +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -5,6 +5,8 @@ package frc.robot.utils; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; /** Poses for alignment. */ public class AlignPoses { @@ -12,6 +14,12 @@ public class AlignPoses { private final Pose2d _center; private final Pose2d _right; + public static enum AlignSide { + LEFT, + CENTER, + RIGHT + } + public Pose2d getLeft() { return _left; } @@ -35,4 +43,31 @@ public AlignPoses(Pose2d left, Pose2d center, Pose2d right) { public AlignPoses(Pose2d center) { this(center, center, center); } + + /** Get pose depending on side. */ + public Pose2d getPose(AlignSide side) { + switch (side) { + case LEFT: + return getLeft(); + + case CENTER: + return getCenter(); + + case RIGHT: + return getRight(); + + default: + return Pose2d.kZero; + } + } + + /** Rotates all the poses around a point. */ + public AlignPoses rotateAround(Translation2d point, Rotation2d rot) { + return new AlignPoses( + new Pose2d(_left.getTranslation().rotateAround(point, rot), _left.getRotation().plus(rot)), + new Pose2d( + _center.getTranslation().rotateAround(point, rot), _center.getRotation().plus(rot)), + new Pose2d( + _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot))); + } } diff --git a/src/test/java/frc/robot/SwerveTest.java b/src/test/java/frc/robot/SwerveTest.java index 672d30a..78dc6a3 100644 --- a/src/test/java/frc/robot/SwerveTest.java +++ b/src/test/java/frc/robot/SwerveTest.java @@ -30,7 +30,7 @@ public void driveTo() { // TODO: ts don't work var goal = new Pose2d(0.5, 0, Rotation2d.fromDegrees(5)); - runToCompletion(_swerve.driveToPose(() -> goal)); + runToCompletion(_swerve.driveTo(goal)); assertEquals(goal.getX(), _swerve.getPose().getX(), 0.2); assertEquals(goal.getY(), _swerve.getPose().getY(), 0.2); From 0d4c7de6a2f3d0cf60e5ad4b3f26f3c2760d7dd6 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich Date: Wed, 5 Feb 2025 14:07:11 -0500 Subject: [PATCH 06/14] fixed some driver logic align to command --- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/subsystems/Swerve.java | 5 ++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7ab6148..aeffa06 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -211,7 +211,8 @@ private void alignmentTriggers(Trigger button, AlignPoses poses) { .whileTrue(_swerve.alignTo(poses, AlignSide.LEFT)); button - .and(_driverController.leftTrigger().and(_driverController.rightTrigger()).negate()) + .and( + _driverController.leftTrigger().negate().and(_driverController.rightTrigger().negate())) .whileTrue(_swerve.alignTo(poses, AlignSide.CENTER)); button diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 853773d..ac53ddf 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -5,7 +5,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.wpilibj2.command.Commands.*; +import static edu.wpi.first.wpilibj2.command.Commands.sequence; import choreo.trajectory.SwerveSample; import com.ctre.phoenix6.SignalLogger; @@ -33,7 +33,6 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.DeferredCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.lib.FaultLogger; @@ -475,7 +474,7 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { DogLog.log("Auto/Align Pose", _alignGoal.getPose(side)); }) - .andThen(new DeferredCommand(() -> driveTo(_alignGoal.getPose(side)), Set.of(this))) + .andThen(defer(() -> driveTo(_alignGoal.getPose(side)))) .withName("Align To"); } From f1703e353e07e98e45971adf267c632ba5404269 Mon Sep 17 00:00:00 2001 From: Elvis Date: Wed, 5 Feb 2025 17:56:27 -0500 Subject: [PATCH 07/14] reef red alliance --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/subsystems/Swerve.java | 18 +++++++++++++++--- src/main/java/frc/robot/utils/AlignPoses.java | 9 +++++++++ 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 05e74b7..4cedabc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -54,6 +54,8 @@ public static class FieldConstants { public static final Translation2d reefCenter = new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters)); + public static final Distance reefDistance = Meters.of(8.57); + public static final AlignPoses reef = new AlignPoses( new Pose2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters), Rotation2d.kZero), diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ac53ddf..66064e6 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -56,6 +57,7 @@ import java.util.ArrayList; import java.util.HashSet; import java.util.List; +import java.util.Optional; import java.util.Set; import org.photonvision.simulation.VisionSystemSim; @@ -457,13 +459,23 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { () -> { Pose2d pose = getPose(); + Optional alliance = DriverStation.getAlliance(); + var reefCenter = + alliance.get() == Alliance.Red + ? FieldConstants.reefCenter.plus( + new Translation2d(FieldConstants.reefDistance.in(Meters), 0)) + : FieldConstants.reefCenter; + if (alignGoal == FieldConstants.reef) { double minDistance = Double.MAX_VALUE; + var goal = + alliance.get() == Alliance.Red + ? alignGoal.offset(FieldConstants.reefDistance.in(Meters)) + : alignGoal; + for (int i = 0; i < 6; i++) { - var rotated = - alignGoal.rotateAround( - FieldConstants.reefCenter, Rotation2d.fromDegrees(60).times(i)); + var rotated = goal.rotateAround(reefCenter, Rotation2d.fromDegrees(60).times(i)); if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) { _alignGoal = rotated; diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java index 40c81ad..2b7021f 100644 --- a/src/main/java/frc/robot/utils/AlignPoses.java +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; /** Poses for alignment. */ @@ -70,4 +71,12 @@ public AlignPoses rotateAround(Translation2d point, Rotation2d rot) { new Pose2d( _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot))); } + + /** Offsets the poses by a given x */ + public AlignPoses offset(double x) { // can overload for y value if needed + return new AlignPoses( + _left.plus(new Transform2d(x, 0, Rotation2d.kZero)), + _center.plus(new Transform2d(x, 0, Rotation2d.kZero)), + _right.plus(new Transform2d(x, 0, Rotation2d.kZero))); + } } From a44e2baa21ab0708a67bc105ce82e82150c836bf Mon Sep 17 00:00:00 2001 From: Elvis Date: Wed, 5 Feb 2025 17:56:59 -0500 Subject: [PATCH 08/14] spotless --- src/main/java/frc/robot/utils/AlignPoses.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java index 2b7021f..614cb60 100644 --- a/src/main/java/frc/robot/utils/AlignPoses.java +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -71,7 +71,7 @@ public AlignPoses rotateAround(Translation2d point, Rotation2d rot) { new Pose2d( _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot))); } - + /** Offsets the poses by a given x */ public AlignPoses offset(double x) { // can overload for y value if needed return new AlignPoses( From d081f670ba99ebd6caf45358c88bf0e5e290d2fb Mon Sep 17 00:00:00 2001 From: Elvis Date: Wed, 5 Feb 2025 19:09:15 -0500 Subject: [PATCH 09/14] starting logic for human station alignment --- src/main/java/frc/robot/Constants.java | 18 ++++++++++++++++++ src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/subsystems/Swerve.java | 15 +++++++++++++++ 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4cedabc..c31cd11 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,11 +56,29 @@ public static class FieldConstants { public static final Distance reefDistance = Meters.of(8.57); + public static final Translation2d humanCenter = + new Translation2d(Inches.of(47.93).in(Meter), Inches.of(158.28).in(Meters)); + public static final AlignPoses reef = new AlignPoses( new Pose2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters), Rotation2d.kZero), new Pose2d(Inches.of(101.3).in(Meters), Inches.of(158.5).in(Meters), Rotation2d.kZero), new Pose2d(Inches.of(101.3).in(Meters), Inches.of(146.5).in(Meters), Rotation2d.kZero)); + + public static final AlignPoses human = + new AlignPoses( + new Pose2d( + Inches.of(23.14).in(Meters), + Inches.of(264.66).in(Meters), + new Rotation2d(Degrees.of(126))), + new Pose2d( + Inches.of(41.17).in(Meters), + Inches.of(282.69).in(Meters), + new Rotation2d(Degrees.of(126))), + new Pose2d( + Inches.of(64.61).in(Meters), + Inches.of(294.86).in(Meters), + new Rotation2d(Degrees.of(126)))); } public static class VisionConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aeffa06..5d7a254 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -225,7 +225,8 @@ private void configureDriverBindings() { _driverController.x().onTrue(_swerve.toggleFieldOriented()); _driverController.y().onTrue(_swerve.resetHeading()); - alignmentTriggers(_driverController.povDown(), FieldConstants.reef); + alignmentTriggers(_driverController.povUp(), FieldConstants.reef); + alignmentTriggers(_driverController.povLeft(), FieldConstants.human); } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 66064e6..ced2a5e 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -484,6 +484,21 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { } } + if (alignGoal == FieldConstants.human) { + double minDistance = Double.MAX_VALUE; + + for (int i = 0; i < 2; i++) { + var rotated = + alignGoal.rotateAround( + FieldConstants.humanCenter, Rotation2d.fromDegrees(180).times(i)); + + if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) { + _alignGoal = rotated; + minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm(); + } + } + } + DogLog.log("Auto/Align Pose", _alignGoal.getPose(side)); }) .andThen(defer(() -> driveTo(_alignGoal.getPose(side)))) From 51152bea369a8930c955bf2b8fc300712b5f902f Mon Sep 17 00:00:00 2001 From: Elvis Date: Thu, 6 Feb 2025 15:44:47 -0500 Subject: [PATCH 10/14] changed alliance to rotate around the center of the field --- src/main/java/frc/robot/Constants.java | 3 ++- src/main/java/frc/robot/Robot.java | 8 ++++---- src/main/java/frc/robot/subsystems/Swerve.java | 15 +++++++-------- src/main/java/frc/robot/utils/AlignPoses.java | 9 --------- 4 files changed, 13 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c31cd11..60b8fce 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -54,7 +54,8 @@ public static class FieldConstants { public static final Translation2d reefCenter = new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters)); - public static final Distance reefDistance = Meters.of(8.57); + public static final Translation2d fieldCenter = + new Translation2d(fieldLayout.getFieldLength() / 2, fieldLayout.getFieldWidth() / 2); public static final Translation2d humanCenter = new Translation2d(Inches.of(47.93).in(Meter), Inches.of(158.28).in(Meters)); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5d7a254..f89658f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -222,11 +222,11 @@ private void alignmentTriggers(Trigger button, AlignPoses poses) { private void configureDriverBindings() { _driverController.a().whileTrue(_swerve.brake()); - _driverController.x().onTrue(_swerve.toggleFieldOriented()); - _driverController.y().onTrue(_swerve.resetHeading()); + _driverController.povUp().onTrue(_swerve.toggleFieldOriented()); + _driverController.povDown().onTrue(_swerve.resetHeading()); - alignmentTriggers(_driverController.povUp(), FieldConstants.reef); - alignmentTriggers(_driverController.povLeft(), FieldConstants.human); + alignmentTriggers(_driverController.x(), FieldConstants.reef); + alignmentTriggers(_driverController.y(), FieldConstants.human); } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ced2a5e..4c137ce 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,7 +24,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -458,22 +457,22 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { return runOnce( () -> { Pose2d pose = getPose(); - Optional alliance = DriverStation.getAlliance(); - var reefCenter = - alliance.get() == Alliance.Red - ? FieldConstants.reefCenter.plus( - new Translation2d(FieldConstants.reefDistance.in(Meters), 0)) - : FieldConstants.reefCenter; if (alignGoal == FieldConstants.reef) { double minDistance = Double.MAX_VALUE; var goal = alliance.get() == Alliance.Red - ? alignGoal.offset(FieldConstants.reefDistance.in(Meters)) + ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg) : alignGoal; + var reefCenter = + alliance.get() == Alliance.Red + ? FieldConstants.reefCenter.rotateAround( + FieldConstants.fieldCenter, Rotation2d.k180deg) + : FieldConstants.reefCenter; + for (int i = 0; i < 6; i++) { var rotated = goal.rotateAround(reefCenter, Rotation2d.fromDegrees(60).times(i)); diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java index 614cb60..40c81ad 100644 --- a/src/main/java/frc/robot/utils/AlignPoses.java +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; /** Poses for alignment. */ @@ -71,12 +70,4 @@ public AlignPoses rotateAround(Translation2d point, Rotation2d rot) { new Pose2d( _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot))); } - - /** Offsets the poses by a given x */ - public AlignPoses offset(double x) { // can overload for y value if needed - return new AlignPoses( - _left.plus(new Transform2d(x, 0, Rotation2d.kZero)), - _center.plus(new Transform2d(x, 0, Rotation2d.kZero)), - _right.plus(new Transform2d(x, 0, Rotation2d.kZero))); - } } From 96c02c89137b66b43a69502035795a39b05f47f0 Mon Sep 17 00:00:00 2001 From: Elvis Date: Thu, 6 Feb 2025 19:02:15 -0500 Subject: [PATCH 11/14] finished all align poses for both alliances --- src/main/java/frc/robot/Constants.java | 5 +++++ src/main/java/frc/robot/Robot.java | 1 + .../java/frc/robot/subsystems/Swerve.java | 21 ++++++++++++++++--- src/main/java/frc/robot/utils/AlignPoses.java | 8 +++++++ 4 files changed, 32 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60b8fce..ab08b65 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -80,6 +80,11 @@ public static class FieldConstants { Inches.of(64.61).in(Meters), Inches.of(294.86).in(Meters), new Rotation2d(Degrees.of(126)))); + + public static final AlignPoses processor = + new AlignPoses( + new Pose2d( + Inches.of(233.7).in(Meters), Inches.of(16.2).in(Meters), Rotation2d.kCW_90deg)); } public static class VisionConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f89658f..c9d3d65 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -227,6 +227,7 @@ private void configureDriverBindings() { alignmentTriggers(_driverController.x(), FieldConstants.reef); alignmentTriggers(_driverController.y(), FieldConstants.human); + alignmentTriggers(_driverController.b(), FieldConstants.processor); } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 4c137ce..d958b24 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -487,9 +487,17 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { double minDistance = Double.MAX_VALUE; for (int i = 0; i < 2; i++) { - var rotated = - alignGoal.rotateAround( - FieldConstants.humanCenter, Rotation2d.fromDegrees(180).times(i)); + AlignPoses rotated = alignGoal.offset(0, -6.26 * i); + + rotated = + alliance.get() == Alliance.Red + ? rotated.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg) + : rotated; + + rotated = + rotated.rotateAround( + rotated.getCenter().getTranslation(), + Rotation2d.fromDegrees(106).times(i)); if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) { _alignGoal = rotated; @@ -498,6 +506,13 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { } } + if (alignGoal == FieldConstants.processor) { + _alignGoal = + alliance.get() == Alliance.Red + ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg) + : alignGoal; + } + DogLog.log("Auto/Align Pose", _alignGoal.getPose(side)); }) .andThen(defer(() -> driveTo(_alignGoal.getPose(side)))) diff --git a/src/main/java/frc/robot/utils/AlignPoses.java b/src/main/java/frc/robot/utils/AlignPoses.java index 40c81ad..2ba630e 100644 --- a/src/main/java/frc/robot/utils/AlignPoses.java +++ b/src/main/java/frc/robot/utils/AlignPoses.java @@ -70,4 +70,12 @@ public AlignPoses rotateAround(Translation2d point, Rotation2d rot) { new Pose2d( _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot))); } + + /** Offset the points by a transformation */ + public AlignPoses offset(double x, double y) { + return new AlignPoses( + new Pose2d(_left.getX() + x, _left.getY() + y, _left.getRotation()), + new Pose2d(_center.getX() + x, _center.getY() + y, _center.getRotation()), + new Pose2d(_right.getX() + x, _right.getY() + y, _right.getRotation())); + } } From 779edca2b6c41c73daa06d471305b9cbce6bb51c Mon Sep 17 00:00:00 2001 From: Elvis Date: Fri, 7 Feb 2025 16:39:34 -0500 Subject: [PATCH 12/14] added cage alignment --- src/main/java/frc/robot/Constants.java | 9 +++++++++ src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/subsystems/Swerve.java | 7 +++++++ 3 files changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ab08b65..861371c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -85,6 +85,15 @@ public static class FieldConstants { new AlignPoses( new Pose2d( Inches.of(233.7).in(Meters), Inches.of(16.2).in(Meters), Rotation2d.kCW_90deg)); + + public static final AlignPoses cage = + new AlignPoses( + new Pose2d( + Inches.of(324.95).in(Meters), Inches.of(285.84).in(Meters), Rotation2d.kCCW_90deg), + new Pose2d( + Inches.of(324.95).in(Meters), Inches.of(241.89).in(Meters), Rotation2d.kCCW_90deg), + new Pose2d( + Inches.of(324.95).in(Meters), Inches.of(200.16).in(Meters), Rotation2d.kCCW_90deg)); } public static class VisionConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c9d3d65..5f4c765 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -228,6 +228,7 @@ private void configureDriverBindings() { alignmentTriggers(_driverController.x(), FieldConstants.reef); alignmentTriggers(_driverController.y(), FieldConstants.human); alignmentTriggers(_driverController.b(), FieldConstants.processor); + alignmentTriggers(_driverController.start(), FieldConstants.cage); } private void configureOperatorBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d958b24..bd144e9 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -513,6 +513,13 @@ public Command alignTo(AlignPoses alignGoal, AlignSide side) { : alignGoal; } + if (alignGoal == FieldConstants.cage) { + _alignGoal = + alliance.get() == Alliance.Red + ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg) + : alignGoal; + } + DogLog.log("Auto/Align Pose", _alignGoal.getPose(side)); }) .andThen(defer(() -> driveTo(_alignGoal.getPose(side)))) From 38c5cf44c8a46ce5e8ee55b1e89e6b9418fe1086 Mon Sep 17 00:00:00 2001 From: Peter Gutkovich <60079012+PGgit08@users.noreply.github.com> Date: Fri, 7 Feb 2025 22:48:08 -0500 Subject: [PATCH 13/14] Update Swerve.java --- src/main/java/frc/robot/subsystems/Swerve.java | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index bd144e9..f8c3a12 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -137,22 +137,6 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem, SelfChec private double _lastSimTime = 0; private Notifier _simNotifier; - public static enum DesiredLocation { - HUMAN(new Pose2d(0, 0, Rotation2d.kZero)), - PROCESSOR(new Pose2d(0, 0, Rotation2d.kZero)), - REEF(new Pose2d(0, 0, Rotation2d.kZero)); - - Pose2d _pose; - - private DesiredLocation(Pose2d pose) { - _pose = pose; - } - - public Pose2d getPose() { - return _pose; - } - } - @Logged(name = "Driver Chassis Speeds") private final ChassisSpeeds _driverChassisSpeeds = new ChassisSpeeds(); From d671966e86a87dc5dab973676c7a86ffe407cf8b Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Fri, 7 Feb 2025 22:51:03 -0500 Subject: [PATCH 14/14] spotless --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 258f3c0..828414c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,9 +10,9 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8fcc07f..ba1d873 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,7 +32,6 @@ import frc.lib.FaultLogger; import frc.lib.InputStream; import frc.robot.Constants.FieldConstants; -import frc.robot.Constants.ManipulatorConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.WristevatorConstants;