From c69a532efe8944958447291ac78b6b5456fde3c0 Mon Sep 17 00:00:00 2001 From: Saarth Date: Sat, 28 Mar 2026 13:37:35 -0400 Subject: [PATCH 1/2] fixed and added swerve methods --- .../java/frc/robot/subsystems/Swerve.java | 72 +++++++++++++++---- 1 file changed, 58 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index beda904b..86f994d1 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.controller.PIDController; 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.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; @@ -400,22 +401,65 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) } /** - * robot goes to specified pose - * @param destination - * @return + * @param desPose Posd2d to move to + * @return a Command that makes the robot move to the desired Pose2d */ - public Command toPose(Pose2d destination) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + public Command roboToPose(Pose2d desPose, double tolerance) { + return Commands.runOnce(() -> { + Pose2d curPose = getState().Pose; + double xSpeed = m_pathXController.calculate(curPose.getX(), desPose.getX()); + double ySpeed = m_pathYController.calculate(curPose.getY(), desPose.getY()); + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + } - double xSpeed = m_pathXController.calculate(curPose.getX(), destination.getX()); - double ySpeed = m_pathYController.calculate(curPose.getY(), destination.getY()); - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destination.getRotation().getRadians()); + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) + && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + } - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - } - ); + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); + } + + /** + * @param desRotation Rotation2d to turn to + * @return a Command that makes the robot turn to the desired Rotation2d + */ + public Command roboToRotation(Rotation2d desRotation, double tolerance) { + return Commands.runOnce(() -> { + Rotation2d curRotation = getState().Pose.getRotation(); + double thetaSpeed = m_pathThetaController.calculate(curRotation.getRadians(), desRotation.getRadians()); + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); + } + + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + } + + /** + * @param desTranslation Translation2d to go to + * @return a Command that makes the robot go to the desired Translation2d + */ + public Command roboToTranslation(Translation2d desTranslation, double tolerance) { + return Commands.runOnce(() -> { + Translation2d curTranslation = getState().Pose.getTranslation(); + double xSpeed = m_pathXController.calculate(curTranslation.getX(), desTranslation.getX()); + double ySpeed = m_pathYController.calculate(curTranslation.getY(), desTranslation.getY()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + } + + public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) { + return Math.hypot( + desTranslation.getMeasureX().minus(curTranslation.getMeasureX()).baseUnitMagnitude(), + desTranslation.getMeasureY().minus(curTranslation.getMeasureY()).baseUnitMagnitude() + ) <= tolerance; } /** @@ -426,7 +470,7 @@ public Command swerveToObject() { Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - return toPose(destination); + return roboToPose(destination, 0.1); } public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { From 8d2b3661e6fb8c91ac2bc36f31ee21b90d875f07 Mon Sep 17 00:00:00 2001 From: Saarth Date: Mon, 30 Mar 2026 22:22:04 -0400 Subject: [PATCH 2/2] IM LOSING MY MIND everything finally works i think i lost a few braincells today --- src/main/java/frc/robot/Robot.java | 14 +- .../java/frc/robot/subsystems/Swerve.java | 122 ++++++++++++++---- 2 files changed, 107 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a85ede05..30812702 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import static frc.robot.Constants.RobotK.*; import static frc.robot.Constants.ShooterK; import java.util.Optional; +import java.util.function.Supplier; + import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -19,6 +21,9 @@ import choreo.auto.AutoFactory; 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; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DataLogManager; @@ -154,6 +159,8 @@ public class Robot extends TimedRobot { // private final Tracer m_periodicTracer = new Tracer(); private final Command m_preheaterCommand; + private Supplier supp_robotPose = () -> m_drivetrain.getState().Pose; + /* CONSTRUCTOR */ public Robot() { configureBindings(); @@ -286,6 +293,11 @@ private void configureBindings() { m_manipulator.povUp().onTrue(m_intake.setIntakeFlapServoCmd(IntakeK.kIntakeFlapDeployPos)); m_manipulator.povDown().onTrue(m_intake.setIntakeFlapServoCmd(0)); + m_driver.povUp().whileTrue(m_drivetrain.translateRobot(1, 0, 0.1)); + m_driver.povLeft().whileTrue(m_drivetrain.translateRobot(0, 1, 0.1)); + m_driver.povDown().whileTrue(m_drivetrain.translateRobot(-1, 0, 0.1)); + m_driver.povRight().whileTrue(m_drivetrain.translateRobot(0, -1, 0.1)); + // snapshot on each shoot press trg_shoot.onTrue(WaltCamera.takeSnapshotCmd()); @@ -372,7 +384,7 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); // m_periodicTracer.addEpoch("CommandScheduler"); - log_robotPose.accept(m_drivetrain.getState().Pose); + log_robotPose.accept(supp_robotPose.get()); for (var camera : WaltCamera.AllCameras) { Optional estimatedPoseOptional = camera.getEstimatedGlobalPose(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 86f994d1..031fab9b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import java.util.Optional; +import java.util.Set; import java.util.function.Supplier; import org.photonvision.targeting.PhotonTrackedTarget; @@ -24,6 +25,7 @@ import edu.wpi.first.math.controller.PIDController; 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; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; @@ -402,17 +404,20 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) /** * @param desPose Posd2d to move to + * @param tolerance how close to pose until stopping * @return a Command that makes the robot move to the desired Pose2d */ public Command roboToPose(Pose2d desPose, double tolerance) { - return Commands.runOnce(() -> { - Pose2d curPose = getState().Pose; - double xSpeed = m_pathXController.calculate(curPose.getX(), desPose.getX()); - double ySpeed = m_pathYController.calculate(curPose.getY(), desPose.getY()); - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance))) - .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + return Commands.defer(() -> { + Pose2d initPose = getState().Pose; + return Commands.runOnce(() -> { + double xSpeed = m_pathXController.calculate(initPose.getX(), desPose.getX()); + double ySpeed = m_pathYController.calculate(initPose.getY(), desPose.getY()); + double thetaSpeed = m_pathThetaController.calculate(initPose.getRotation().getRadians(), desPose.getRotation().getRadians()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + }, Set.of(this)); } public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { @@ -425,34 +430,41 @@ public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTole } /** - * @param desRotation Rotation2d to turn to - * @return a Command that makes the robot turn to the desired Rotation2d + * @param xTranslation meters in the x direction to translate by + * @param yTranslation meters in the y direction to translate by + * @param translationTolerance how close to desired translations until stopping + * @param rotationRads radians to rotate robot + * @param rotationTolerance how close to desired turn until stopping + * @return a Command that makes the robot go to the desired Translation2d and rotates the robot the specified number of radians */ - public Command roboToRotation(Rotation2d desRotation, double tolerance) { - return Commands.runOnce(() -> { - Rotation2d curRotation = getState().Pose.getRotation(); - double thetaSpeed = m_pathThetaController.calculate(curRotation.getRadians(), desRotation.getRadians()); - setControl(swreq_drive.withRotationalRate(thetaSpeed)); - }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance))) - .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); - } - - public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { - return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + public Command translateAndRotateRobot(double xTranslation, double yTranslation, double translationTolerance, double rotationRads, double rotationTolerance) { + return Commands.defer(() -> { + Pose2d initPose = getState().Pose; + return Commands.runOnce(() -> { + double xSpeed = m_pathXController.calculate(initPose.getX(), initPose.getX() + xTranslation) / 2; + double ySpeed = m_pathYController.calculate(initPose.getY(), initPose.getY() + yTranslation) / 2; + double thetaSpeed = m_pathThetaController.calculate(initPose.getRotation().getRadians(), initPose.getRotation().getRadians() + rotationRads) / 2; + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, initPose.plus(new Transform2d(xTranslation, yTranslation, new Rotation2d(Radians.of(rotationRads)))), translationTolerance, rotationTolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + }, Set.of(this)); } /** * @param desTranslation Translation2d to go to + * @param tolerance how close to translation until stopping * @return a Command that makes the robot go to the desired Translation2d */ public Command roboToTranslation(Translation2d desTranslation, double tolerance) { - return Commands.runOnce(() -> { - Translation2d curTranslation = getState().Pose.getTranslation(); - double xSpeed = m_pathXController.calculate(curTranslation.getX(), desTranslation.getX()); - double ySpeed = m_pathYController.calculate(curTranslation.getY(), desTranslation.getY()); - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); - }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, tolerance))) - .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + return Commands.defer(() -> { + Translation2d initTranslation = getState().Pose.getTranslation(); + return Commands.runOnce(() -> { + double xSpeed = m_pathXController.calculate(initTranslation.getX(), desTranslation.getX()) / 2; + double ySpeed = m_pathYController.calculate(initTranslation.getY(), desTranslation.getY()) / 2; + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + }, Set.of(this)); } public boolean isNearTranslation(Translation2d curTranslation, Translation2d desTranslation, double tolerance) { @@ -462,6 +474,60 @@ public boolean isNearTranslation(Translation2d curTranslation, Translation2d des ) <= tolerance; } + /** + * @param xTranslation meters in the x direction to translate by + * @param yTranslation meters in the y direction to translate by + * @param tolerance how close to desired translations until stopping + * @return a Command that translates the robot by the specified x and y distances + */ + public Command translateRobot(double xTranslation, double yTranslation, double tolerance) { + return Commands.defer(() -> { + Translation2d initTranslation = getState().Pose.getTranslation(); + return Commands.runOnce(() -> { + double xSpeed = m_pathXController.calculate(initTranslation.getX(), initTranslation.getX() + xTranslation) / 2; + double ySpeed = m_pathYController.calculate(initTranslation.getY(), initTranslation.getY() + yTranslation) / 2; + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), initTranslation.plus(new Translation2d(xTranslation, yTranslation)), tolerance))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + }, Set.of(this)); + } + + /** + * @param desRotation Rotation2d to turn to + * @param tolerance how close to rotation until stopping + * @return a Command that makes the robot turn to the desired Rotation2d + */ + public Command roboToRotation(Rotation2d desRotation, double tolerance) { + return Commands.defer(() -> { + Rotation2d initRotation = getState().Pose.getRotation(); + return Commands.runOnce(() -> { + double thetaSpeed = m_pathThetaController.calculate(initRotation.getRadians(), desRotation.getRadians()) / 2; + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, tolerance))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); + }, Set.of(this)); + } + + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + } + + /** + * @param rotationRads radians to rotate robot + * @param tolerance how close to desired turn until stopping + * @return a Command that rotates the robot the specified number of radians + */ + public Command rotateRobot(double rotationRads, double tolerance) { + return Commands.defer(() -> { + Rotation2d initRotation = getState().Pose.getRotation(); + return Commands.runOnce(() -> { + double thetaSpeed = m_pathThetaController.calculate(initRotation.getRadians(), initRotation.getRadians() + rotationRads) / 2; + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), initRotation.plus(new Rotation2d(Radians.of(rotationRads))), tolerance))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); + }, Set.of(this)); + } + /** * robot goes to detected target */