From 6b8a1ecf930ef3b8936f9355b26ac4e93f92fa15 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Wed, 11 Mar 2026 20:31:43 -0400 Subject: [PATCH 01/23] initial commit, DOES NOT WORK (yet) --- src/main/java/frc/robot/Robot.java | 17 +++++--- .../robot/autons/WaltSimpleAutonFactory.java | 2 +- .../frc/robot/subsystems/Superstructure.java | 14 +++++-- .../java/frc/robot/subsystems/Swerve.java | 40 +++++++++++++++++++ 4 files changed, 63 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e39a4404..75588df7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -62,14 +62,14 @@ public class Robot extends TimedRobot { /* CLASS VARIABLES */ //---CONSTANTS private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed - private final AngularVelocity kMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity + private final AngularVelocity kDriverMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(kMaxTranslationSpeed.times(0.1)).withRotationalDeadband(kMaxAngularRate.times(0.1)) // Add a 10% deadband + .withDeadband(kMaxTranslationSpeed.times(0.1)).withRotationalDeadband(kDriverMaxAngularRate.times(0.1)) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors // private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); // private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); @@ -89,7 +89,7 @@ public class Robot extends TimedRobot { private final Indexer m_indexer = new Indexer(); // private final WaltVisualSim m_visualSim; - private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter); + private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter, m_drivetrain); //---AUTONS private Command m_autonomousCommand; @@ -121,6 +121,7 @@ public class Robot extends TimedRobot { private Trigger trg_passRight = m_manipulator.povRight().and(trg_driverOverride.negate()).and(trg_passLeft.negate()); private Trigger trg_shimmy = m_manipulator.leftBumper(); + private Trigger trg_swerveShimmy = m_driver.povLeft(); //---OVERRIDE TRIGGERS private Trigger trg_deployIntakeOverride = trg_manipOverride.and(m_manipulator.rightTrigger()); @@ -192,7 +193,7 @@ private Command driveCommand(double speedMultiplier) { var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); - var driverYawRate = kMaxAngularRate.times(-m_driver.getRightX()); + var driverYawRate = kDriverMaxAngularRate.times(-m_driver.getRightX()); log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); @@ -307,7 +308,11 @@ private void configureBindings() { m_superstructure.emergencyBarf() ); - trg_shimmy.whileTrue(m_superstructure.shimmy()); + trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); + + trg_swerveShimmy.whileTrue(m_superstructure.swerveShimmy()); + + trg_unjam.whileTrue( m_superstructure.unjamCmd() @@ -535,7 +540,7 @@ public void testInit() { m_drivetrain.applyRequest(() -> drive.withVelocityX(0) .withVelocityY(0) - .withRotationalRate(kMaxAngularRate) + .withRotationalRate(kDriverMaxAngularRate) ), Commands.waitSeconds(2.5), m_drivetrain.xBrake(), diff --git a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index 40274843..e19c9527 100644 --- a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java @@ -94,7 +94,7 @@ private Command preload_oneSweep(boolean isLeft) { logState(4.1), Commands.waitSeconds(6), logState(4.2), - m_superstructure.shimmy() + m_superstructure.intakeArmShimmy() ) ) ).withName(trajName); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3965ff42..eaec4fa4 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.IndexerK; +import frc.robot.Constants.IntakeK; +import frc.robot.Constants.ShooterK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; import frc.util.WaltLogger; @@ -28,6 +30,7 @@ public class Superstructure extends SubsystemBase { private final Intake m_intake; private final Indexer m_indexer; private final Shooter m_shooter; + private final Swerve m_swerve; /* LOGGERS */ private HashSet m_activeCommands = new HashSet<>(); @@ -37,10 +40,11 @@ public class Superstructure extends SubsystemBase { private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); /* CONSTRUCTOR */ - public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { + public Superstructure(Intake intake, Indexer indexer, Shooter shooter, Swerve swerve) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; + m_swerve = swerve; } /* BUTTON BIND SEQUENCES */ @@ -152,7 +156,7 @@ public Command activateOuttake(AngularVelocity RPS) { .onlyWhile(() -> m_shooter.isShooterSpunUp()) .andThen(Commands.waitUntil(() -> m_shooter.isShooterSpunUp())) .repeatedly(), - // m_intake.shimmy(), + // m_intake.\my(), logCommand ).finallyDo( () -> deactivateOuttake() @@ -194,10 +198,14 @@ public Command emergencyBarf() { ); } - public Command shimmy() { + public Command intakeArmShimmy() { return m_intake.shimmy(); } + public Command swerveShimmy() { + return m_swerve.swerveShimmy(); + } + /** * Initiates passing by activating intake and outtake. */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f2f5c88b..408af892 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.function.BooleanSupplier; import java.util.function.Supplier; import org.photonvision.targeting.PhotonTrackedTarget; @@ -28,6 +29,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -39,6 +42,10 @@ import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.vision.Detection; +import frc.util.WaltLogger; +import frc.util.WaltLogger.BooleanLogger; +import frc.util.WaltLogger.DoubleLogger; + /** * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class * and implements Subsystem so it can easily be used in command-based projects. @@ -48,6 +55,7 @@ */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.004; // 4 ms + private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3); private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -74,6 +82,12 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final Detection detection = new Detection(); + private final BooleanLogger log_swerveShimmyCCW = WaltLogger.logBoolean("Swerve", "swerveShimmyCCW"); + private final DoubleLogger log_swerveShimmyYawRate = WaltLogger.logDouble("Swerve", "swerveShimmyYawRate"); + + private double m_swerveShimmyYawRate; + private boolean m_swerveShimmyCCW; + private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -266,6 +280,9 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + + log_swerveShimmyCCW.accept(m_swerveShimmyCCW); + log_swerveShimmyYawRate.accept(m_swerveShimmyYawRate); } private void startSimThread() { @@ -363,6 +380,29 @@ public Command xBrake() { } + + public Command swerveShimmyRotate(boolean CCW) { + return this.applyRequest(() -> { + var yawRate = CCW ? kSwerveShimmyAngularRate : kSwerveShimmyAngularRate.unaryMinus(); + + m_swerveShimmyCCW = CCW; + m_swerveShimmyYawRate = yawRate.magnitude(); + + return swreq_drive + .withRotationalRate(yawRate); // Drive counterclockwise with negative X (left) + } + ); + } + + public Command swerveShimmy() { + return Commands.repeatingSequence( + this.swerveShimmyRotate(true), + Commands.waitSeconds(0.4), + this.swerveShimmyRotate(false), + Commands.waitSeconds(0.4) + ).finallyDo(() -> this.xBrake()); + } + /** * Creates a new auto factory for this drivetrain. * From ab04d2b78dd4457a43b8322595aa58d12638662f Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Wed, 11 Mar 2026 21:38:57 -0400 Subject: [PATCH 02/23] fixed swerveshimmy, will fine tuner later --- src/main/java/frc/robot/Robot.java | 2 -- .../java/frc/robot/subsystems/Swerve.java | 26 +++++++------------ 2 files changed, 10 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 75588df7..a4b2acad 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -312,8 +312,6 @@ private void configureBindings() { trg_swerveShimmy.whileTrue(m_superstructure.swerveShimmy()); - - trg_unjam.whileTrue( m_superstructure.unjamCmd() ); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 408af892..629b29e2 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -85,9 +85,6 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final BooleanLogger log_swerveShimmyCCW = WaltLogger.logBoolean("Swerve", "swerveShimmyCCW"); private final DoubleLogger log_swerveShimmyYawRate = WaltLogger.logDouble("Swerve", "swerveShimmyYawRate"); - private double m_swerveShimmyYawRate; - private boolean m_swerveShimmyCCW; - private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -280,9 +277,10 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + } - log_swerveShimmyCCW.accept(m_swerveShimmyCCW); - log_swerveShimmyYawRate.accept(m_swerveShimmyYawRate); + @Override + public void simulationPeriodic() { } private void startSimThread() { @@ -379,14 +377,12 @@ public Command xBrake() { return runOnce(() -> setControl(stopReq)); } + public Command swerveShimmyRotate(boolean ccw) { + return applyRequest(() -> { + var yawRate = kSwerveShimmyAngularRate.times(ccw ? 1 : -1); - - public Command swerveShimmyRotate(boolean CCW) { - return this.applyRequest(() -> { - var yawRate = CCW ? kSwerveShimmyAngularRate : kSwerveShimmyAngularRate.unaryMinus(); - - m_swerveShimmyCCW = CCW; - m_swerveShimmyYawRate = yawRate.magnitude(); + log_swerveShimmyCCW.accept(ccw); + log_swerveShimmyYawRate.accept(yawRate.magnitude()); return swreq_drive .withRotationalRate(yawRate); // Drive counterclockwise with negative X (left) @@ -396,10 +392,8 @@ public Command swerveShimmyRotate(boolean CCW) { public Command swerveShimmy() { return Commands.repeatingSequence( - this.swerveShimmyRotate(true), - Commands.waitSeconds(0.4), - this.swerveShimmyRotate(false), - Commands.waitSeconds(0.4) + swerveShimmyRotate(true).withTimeout(0.4), + swerveShimmyRotate(false).withTimeout(0.4) ).finallyDo(() -> this.xBrake()); } From 01636924dd3468529a4d7cd6e774e8c3bf3421e9 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 19:14:19 -0400 Subject: [PATCH 03/23] tune shimmy but now we cant swerve while shimmy --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/subsystems/Swerve.java | 17 ++++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a4b2acad..428f7e10 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -312,6 +312,8 @@ private void configureBindings() { trg_swerveShimmy.whileTrue(m_superstructure.swerveShimmy()); + trg_swerveShimmy.onFalse(Commands.print("Robot swerveShimmy stopped")); + trg_unjam.whileTrue( m_superstructure.unjamCmd() ); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 629b29e2..ce156acc 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -55,7 +55,7 @@ */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.004; // 4 ms - private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3); + private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 2); private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -377,11 +377,11 @@ public Command xBrake() { return runOnce(() -> setControl(stopReq)); } - public Command swerveShimmyRotate(boolean ccw) { + public Command swerveShimmyRotate(boolean CCW) { return applyRequest(() -> { - var yawRate = kSwerveShimmyAngularRate.times(ccw ? 1 : -1); + var yawRate = kSwerveShimmyAngularRate.times(CCW ? 1 : -1); - log_swerveShimmyCCW.accept(ccw); + log_swerveShimmyCCW.accept(CCW); log_swerveShimmyYawRate.accept(yawRate.magnitude()); return swreq_drive @@ -392,9 +392,12 @@ public Command swerveShimmyRotate(boolean ccw) { public Command swerveShimmy() { return Commands.repeatingSequence( - swerveShimmyRotate(true).withTimeout(0.4), - swerveShimmyRotate(false).withTimeout(0.4) - ).finallyDo(() -> this.xBrake()); + Commands.print("Robot is now rotating counter clockwise"), + swerveShimmyRotate(true).withTimeout(0.113), + Commands.print("Robot is now rotating clockwise"), + swerveShimmyRotate(false).withTimeout(0.1), + Commands.print("Robot swerveShimmy cycle ended") + ); } /** From 44f99c1c513446e78d6fb8c539b40081e1d05bbb Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 19:48:01 -0400 Subject: [PATCH 04/23] Fix swerveShimmying and driving at the same time --- src/main/java/frc/robot/Robot.java | 55 +++++++++++++++++-- .../frc/robot/subsystems/Superstructure.java | 4 -- .../java/frc/robot/subsystems/Swerve.java | 23 -------- 3 files changed, 51 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 428f7e10..61e03de4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -63,6 +63,7 @@ public class Robot extends TimedRobot { //---CONSTANTS private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed private final AngularVelocity kDriverMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity + private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 2); private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); @@ -179,7 +180,7 @@ public Robot() { /* COMMANDS */ /** * - * @param speedMultiplier how much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing + * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing * @return swerve drive command */ private Command driveCommand(double speedMultiplier) { @@ -190,7 +191,7 @@ private Command driveCommand(double speedMultiplier) { LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : kMaxTranslationSpeed); - + var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); var driverYawRate = kDriverMaxAngularRate.times(-m_driver.getRightX()); @@ -198,7 +199,7 @@ private Command driveCommand(double speedMultiplier) { log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); log_stickDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); - + return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) @@ -206,6 +207,52 @@ private Command driveCommand(double speedMultiplier) { } ); } + /** + * Returns a modified swerveRequest which uses swerveShimmy's rotational value instead of the driver's stick's. + * Does not move the robot, only returns a swerveRequest. + * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing + * @param shimmyCCW If the shimmy direction is CCW + * @return A swerveRequest with shimmying rotational values + */ + private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean shimmyCCW) { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + // Drivetrain will execute this command periodically + return m_drivetrain.applyRequest(() -> { + LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? + kMaxTranslationSpeed.times(speedMultiplier) : + kMaxTranslationSpeed); + + var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); + var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); + var yawRate = kSwerveShimmyAngularRate.times(shimmyCCW ? 1 : -1); + + log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); + log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); + log_stickDesiredFieldZRot.accept(yawRate.in(RotationsPerSecond)); // TODO: Change loggers later + + return drive + .withVelocityX(driverXVelo) // Drive forward with Y (forward) + .withVelocityY(driverYVelo) // Drive left with X (left) + .withRotationalRate(yawRate); // Drive counterclockwise with negative X (left) + } + ); + } + /** + * A modified driveCommand that utilizes the swerveShimmy rotational value instead of what the driver requests. + * Translational control should not be affected. Regular driveCommand is used as defaultCommand. + * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing + * @return Swerve driveCommand with swerveShimmy rotational values + */ + private Command driveCommandWithSwerveShimmying(double speedMultiplier) { + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + // Drivetrain will execute this command periodically + return Commands.repeatingSequence( + swerveRequestWithSwerveShimmy(speedMultiplier, true).withTimeout(0.113), + swerveRequestWithSwerveShimmy(speedMultiplier, false).withTimeout(0.1) + ); + } //(nonsotm (just for simulating entire robot)) BLARGHHHHHH get intake dude (alex?) to give me his code (idk if he finished it yet) private void configureFuelSim() { @@ -310,7 +357,7 @@ private void configureBindings() { trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); - trg_swerveShimmy.whileTrue(m_superstructure.swerveShimmy()); + trg_swerveShimmy.whileTrue(driveCommandWithSwerveShimmying(RobotK.kRobotSpeedIntakingLimit)); trg_swerveShimmy.onFalse(Commands.print("Robot swerveShimmy stopped")); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index eaec4fa4..6443f130 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -202,10 +202,6 @@ public Command intakeArmShimmy() { return m_intake.shimmy(); } - public Command swerveShimmy() { - return m_swerve.swerveShimmy(); - } - /** * Initiates passing by activating intake and outtake. */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ce156acc..a28af9b2 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -377,29 +377,6 @@ public Command xBrake() { return runOnce(() -> setControl(stopReq)); } - public Command swerveShimmyRotate(boolean CCW) { - return applyRequest(() -> { - var yawRate = kSwerveShimmyAngularRate.times(CCW ? 1 : -1); - - log_swerveShimmyCCW.accept(CCW); - log_swerveShimmyYawRate.accept(yawRate.magnitude()); - - return swreq_drive - .withRotationalRate(yawRate); // Drive counterclockwise with negative X (left) - } - ); - } - - public Command swerveShimmy() { - return Commands.repeatingSequence( - Commands.print("Robot is now rotating counter clockwise"), - swerveShimmyRotate(true).withTimeout(0.113), - Commands.print("Robot is now rotating clockwise"), - swerveShimmyRotate(false).withTimeout(0.1), - Commands.print("Robot swerveShimmy cycle ended") - ); - } - /** * Creates a new auto factory for this drivetrain. * From 7fcd8fa7cc52e361af61625ecc73e7fff35a5a63 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 20:13:29 -0400 Subject: [PATCH 05/23] javadocs, comments, clean up code --- src/main/java/frc/robot/Robot.java | 21 ++++++++++++------- .../frc/robot/subsystems/Superstructure.java | 4 +--- .../java/frc/robot/subsystems/Swerve.java | 6 +----- 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 61e03de4..5d77b24e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -90,7 +90,7 @@ public class Robot extends TimedRobot { private final Indexer m_indexer = new Indexer(); // private final WaltVisualSim m_visualSim; - private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter, m_drivetrain); + private final Superstructure m_superstructure = new Superstructure(m_intake, m_indexer, m_shooter); //---AUTONS private Command m_autonomousCommand; @@ -122,7 +122,7 @@ public class Robot extends TimedRobot { private Trigger trg_passRight = m_manipulator.povRight().and(trg_driverOverride.negate()).and(trg_passLeft.negate()); private Trigger trg_shimmy = m_manipulator.leftBumper(); - private Trigger trg_swerveShimmy = m_driver.povLeft(); + private Trigger trg_swerveShimmy = m_driver.leftBumper(); //---OVERRIDE TRIGGERS private Trigger trg_deployIntakeOverride = trg_manipOverride.and(m_manipulator.rightTrigger()); @@ -138,6 +138,8 @@ public class Robot extends TimedRobot { private final DoubleLogger log_stickDesiredFieldX = WaltLogger.logDouble("Swerve", "stick desired teleop x"); private final DoubleLogger log_stickDesiredFieldY = WaltLogger.logDouble("Swerve", "stick desired teleop y"); private final DoubleLogger log_stickDesiredFieldZRot = WaltLogger.logDouble("Swerve", "stick desired teleop z rot"); + private final DoubleLogger log_robotDesiredFieldZRot = WaltLogger.logDouble("Swerve", "robot desired teleop z rot"); + private final BooleanLogger log_swerveShimmying = WaltLogger.logBoolean("Swerve", "swerveShimmying"); private final BooleanLogger log_povUp = WaltLogger.logBoolean(kLogTab, "Pov Up"); private final BooleanLogger log_povRight = WaltLogger.logBoolean(kLogTab, "Pov Right"); private final BooleanLogger log_povLeft = WaltLogger.logBoolean(kLogTab, "Pov Left"); @@ -199,6 +201,8 @@ private Command driveCommand(double speedMultiplier) { log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); log_stickDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); + log_robotDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); // Should be equal to stick desired IN THIS CASE + log_swerveShimmying.accept(false); return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) @@ -217,7 +221,7 @@ private Command driveCommand(double speedMultiplier) { private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean shimmyCCW) { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. - // Drivetrain will execute this command periodically + // Drivetrain will NOT execute this command periodically return m_drivetrain.applyRequest(() -> { LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : @@ -225,16 +229,19 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); + // Use shimmyAngularRate over driver request var yawRate = kSwerveShimmyAngularRate.times(shimmyCCW ? 1 : -1); log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); - log_stickDesiredFieldZRot.accept(yawRate.in(RotationsPerSecond)); // TODO: Change loggers later + log_stickDesiredFieldZRot.accept(kDriverMaxAngularRate.times(-m_driver.getRightX()).in(RotationsPerSecond)); // Logging driver requests even though they have no impact + log_robotDesiredFieldZRot.accept(yawRate.in(RotationsPerSecond)); + log_swerveShimmying.accept(true); return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) - .withRotationalRate(yawRate); // Drive counterclockwise with negative X (left) + .withRotationalRate(yawRate); // Use shimmy rotation rate } ); } @@ -245,9 +252,7 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh * @return Swerve driveCommand with swerveShimmy rotational values */ private Command driveCommandWithSwerveShimmying(double speedMultiplier) { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - // Drivetrain will execute this command periodically + // Return a sequence of two swerveRequests to shimmy in alternating directions return Commands.repeatingSequence( swerveRequestWithSwerveShimmy(speedMultiplier, true).withTimeout(0.113), swerveRequestWithSwerveShimmy(speedMultiplier, false).withTimeout(0.1) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 6443f130..5aab6b77 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -30,7 +30,6 @@ public class Superstructure extends SubsystemBase { private final Intake m_intake; private final Indexer m_indexer; private final Shooter m_shooter; - private final Swerve m_swerve; /* LOGGERS */ private HashSet m_activeCommands = new HashSet<>(); @@ -40,11 +39,10 @@ public class Superstructure extends SubsystemBase { private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); /* CONSTRUCTOR */ - public Superstructure(Intake intake, Indexer indexer, Shooter shooter, Swerve swerve) { + public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_intake = intake; m_indexer = indexer; m_shooter = shooter; - m_swerve = swerve; } /* BUTTON BIND SEQUENCES */ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index a28af9b2..fb774c34 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -55,7 +55,6 @@ */ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private static final double kSimLoopPeriod = 0.004; // 4 ms - private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 2); private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -81,10 +80,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { .withSteerRequestType(SteerRequestType.Position); private final Detection detection = new Detection(); - - private final BooleanLogger log_swerveShimmyCCW = WaltLogger.logBoolean("Swerve", "swerveShimmyCCW"); - private final DoubleLogger log_swerveShimmyYawRate = WaltLogger.logDouble("Swerve", "swerveShimmyYawRate"); - + private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); From 3446dcea8d38bd1f881a74eee502fcd40cfc62b5 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 20:23:02 -0400 Subject: [PATCH 06/23] fix tiny typo in comments --- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5aab6b77..4810ebf1 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -154,7 +154,7 @@ public Command activateOuttake(AngularVelocity RPS) { .onlyWhile(() -> m_shooter.isShooterSpunUp()) .andThen(Commands.waitUntil(() -> m_shooter.isShooterSpunUp())) .repeatedly(), - // m_intake.\my(), + // m_intake.shimmy(), logCommand ).finallyDo( () -> deactivateOuttake() From 6f4b1a03e6c751b3ba466e54e7efa00887ba48c7 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 20:27:41 -0400 Subject: [PATCH 07/23] more minor things + rid of whitespace --- src/main/java/frc/robot/Robot.java | 17 ++++++++--------- .../robot/autons/WaltSimpleAutonFactory.java | 4 ++-- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 3 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bd437655..fad11705 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -188,7 +188,7 @@ public Robot() { /* COMMANDS */ /** - * + * Normal driveCommand which takes in drive stick requests. Is defaultCommand * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing * @return swerve drive command */ @@ -200,7 +200,7 @@ private Command driveCommand(double speedMultiplier) { LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : kMaxTranslationSpeed); - + var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); var driverYawRate = kDriverMaxAngularRate.times(-m_driver.getRightX()); @@ -210,7 +210,7 @@ private Command driveCommand(double speedMultiplier) { log_stickDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); log_robotDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); // Should be equal to stick desired IN THIS CASE log_swerveShimmying.accept(false); - + return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) @@ -233,7 +233,7 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : kMaxTranslationSpeed); - + var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); // Use shimmyAngularRate over driver request @@ -244,7 +244,7 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh log_stickDesiredFieldZRot.accept(kDriverMaxAngularRate.times(-m_driver.getRightX()).in(RotationsPerSecond)); // Logging driver requests even though they have no impact log_robotDesiredFieldZRot.accept(yawRate.in(RotationsPerSecond)); log_swerveShimmying.accept(true); - + return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) @@ -270,7 +270,7 @@ private Command driveCommandWithSwerveShimmying(double speedMultiplier) { private void configureFuelSim() { FuelSim instance = FuelSim.getInstance(); // instance.spawnStartingFuel(); - + instance.registerRobot( kRobotFullWidth.in(Meters), kRobotFullLength.in(Meters), @@ -333,7 +333,7 @@ private void configureBindings() { m_drivetrain.registerTelemetry(logger::telemeterize); /* CUSTOM BINDS */ - trg_limitFPS.onTrue(WaltCamera.setFpsLimitCmd(true)); + trg_limitFPS.onTrue(WaltCamera.setFpsLimitCmd(true)); trg_unlimitFps.onTrue(WaltCamera.setFpsLimitCmd(false)); //robot heads toward fuel when detected :D (hypothetically)(robo could blow up instead) @@ -366,7 +366,7 @@ private void configureBindings() { trg_emergencyBarf.whileTrue( m_superstructure.emergencyBarf() ); - + trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); trg_swerveShimmy.whileTrue(driveCommandWithSwerveShimmying(RobotK.kRobotSpeedIntakingLimit)); @@ -416,7 +416,6 @@ private void configureTestBindings() { m_drivetrain.registerTelemetry(logger::telemeterize); - // Run SysId routines when holding back/start and X/Y. // Note that each routine should be run exactly once in a single log. // m_driver.back().and(m_driver.y()).whileTrue(m_drivetrain.sysIdDynamic(Direction.kForward)); diff --git a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java index e846a47f..64b91a98 100644 --- a/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java +++ b/src/main/java/frc/robot/autons/WaltSimpleAutonFactory.java @@ -50,7 +50,7 @@ private Command logState(double state) { private Command tp(String message) { return WaltLogger.timedPrintCmd(message); - } + } public Command preheater() { return Commands.sequence( @@ -141,7 +141,7 @@ private Command preload_oneSweep(boolean isLeft) { Commands.waitSeconds(6), logState(4.2), tp("preload.shimmy.START"), - m_superstructure.shimmy(), + m_superstructure.intakeArmShimmy(), tp("preload.shimmy.END") ) ), diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index dd814ae3..2ff72595 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -81,7 +81,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { .withSteerRequestType(SteerRequestType.Position); private final Detection detection = new Detection(); - + private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); From d260ec9b78217b717357d6667b4690c82f9749a2 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 20:33:58 -0400 Subject: [PATCH 08/23] remove unnecessary Swerve.java changes --- src/main/java/frc/robot/Robot.java | 10 +++++----- .../frc/robot/subsystems/Superstructure.java | 8 +++----- src/main/java/frc/robot/subsystems/Swerve.java | 17 +++-------------- 3 files changed, 11 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fad11705..9bfc8b37 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -197,7 +197,7 @@ private Command driveCommand(double speedMultiplier) { // and Y is defined as to the left according to WPILib convention. // Drivetrain will execute this command periodically return m_drivetrain.applyRequest(() -> { - LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? + LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : kMaxTranslationSpeed); @@ -230,7 +230,7 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh // and Y is defined as to the left according to WPILib convention. // Drivetrain will NOT execute this command periodically return m_drivetrain.applyRequest(() -> { - LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? + LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? kMaxTranslationSpeed.times(speedMultiplier) : kMaxTranslationSpeed); @@ -451,7 +451,7 @@ private void configureTestingDashboard() { @Override public void robotPeriodic() { m_periodicTracer.addEpoch("Entry (Unused Time)"); - CommandScheduler.getInstance().run(); + CommandScheduler.getInstance().run(); m_periodicTracer.addEpoch("CommandScheduler"); @@ -491,7 +491,7 @@ public void robotPeriodic() { // ); /* for the mechanism2D in 3D, drag all 3 mechanisms2ds onto the robot pose - and also log the shooter position pose */ + and also log the shooter position pose */ // m_periodicTracer.printEpochs(); } @@ -561,7 +561,7 @@ public void teleopExit() {} @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); - + CommandScheduler.getInstance().schedule( Commands.sequence( m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 4810ebf1..3383fb70 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.IndexerK; -import frc.robot.Constants.IntakeK; -import frc.robot.Constants.ShooterK; import frc.robot.subsystems.Intake.IntakeArmPosition; import frc.robot.subsystems.shooter.Shooter; import frc.util.WaltLogger; @@ -37,7 +35,7 @@ public class Superstructure extends SubsystemBase { private HashSet m_activeOverrideCommands = new HashSet<>(); private final StringArrayLogger log_activeOverrideCommands = WaltLogger.logStringArray(kLogTab, "Active Override Commands"); - + /* CONSTRUCTOR */ public Superstructure(Intake intake, Indexer indexer, Shooter shooter) { m_intake = intake; @@ -96,7 +94,7 @@ public Command intake(BooleanSupplier isPassing) { m_shooter.setTurretPos(Rotations.of(-0.250)); m_intake.setIntakeRollersVelocity(Constants.IntakeK.kIntakeRollersMaxRPS); m_indexer.setSpindexerVelocity(isPassing.getAsBoolean() ? Constants.IndexerK.kSpindexerShootRPS : Constants.IndexerK.kSpindexerIntakeRPS); - }, + }, () -> { if (m_intake.getIntakeArmStatorCurrent() < 40) { Commands.run(() -> m_shooter.setShotCalcCmd(true)); @@ -144,7 +142,7 @@ public Command startShootSequence(AngularVelocity RPS) { public Command activateOuttake(AngularVelocity RPS) { Command logCommand; if (RPS == ShooterK.kShooterRPS) { - logCommand = logActiveCommands("shooting", "deactivateOuttake", "emergencyDump"); + logCommand = logActiveCommands("shooting", "deactivateOuttake", "emergencyDump"); } else { logCommand = logActiveCommands("emergencyDump", "shooting", "deactivateOuttake"); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 2ff72595..65637839 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.*; import java.util.Optional; -import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.photonvision.targeting.PhotonTrackedTarget; @@ -29,8 +28,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -43,12 +40,8 @@ import frc.robot.vision.Detection; import frc.util.WaltLogger; -import frc.util.WaltLogger; -import frc.util.WaltLogger.BooleanLogger; -import frc.util.WaltLogger.DoubleLogger; - /** - * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class + * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class * and implements Subsystem so it can easily be used in command-based projects. * * Generated by the 2026 Tuner X Swerve Project Generator @@ -276,10 +269,6 @@ public void periodic() { } } - @Override - public void simulationPeriodic() { - } - private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); @@ -425,11 +414,11 @@ public Command toPose(Pose2d destination) { /** * robot goes to detected target */ - public Command swerveToObject() { + public Command swerveToObject() { PhotonTrackedTarget target = detection.getClosestObject(); Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - + return toPose(destination); } From a5170d28c0adeb87193f652379dabcb681c83250 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 20:48:49 -0400 Subject: [PATCH 09/23] remove useless print --- src/main/java/frc/robot/Robot.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9bfc8b37..e1d0aef1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -371,8 +371,6 @@ private void configureBindings() { trg_swerveShimmy.whileTrue(driveCommandWithSwerveShimmying(RobotK.kRobotSpeedIntakingLimit)); - trg_swerveShimmy.onFalse(Commands.print("Robot swerveShimmy stopped")); - trg_unjam.whileTrue( m_superstructure.unjamCmd() ); From bbb9439e22c7bb3c3556e71e699cd3a094d09862 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 21:29:59 -0400 Subject: [PATCH 10/23] added test rotation WIP --- src/main/java/frc/robot/Robot.java | 30 ++++++++---------------------- 1 file changed, 8 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e1d0aef1..12d6d338 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,8 @@ import choreo.auto.AutoFactory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DataLogManager; @@ -63,7 +65,7 @@ public class Robot extends TimedRobot { //---CONSTANTS private final LinearVelocity kMaxTranslationSpeed = TunerConstants.kSpeedAt12Volts; // kSpeedAt12Volts desired top speed private final AngularVelocity kDriverMaxAngularRate = RotationsPerSecond.of(1.05); // 3/4 of a rotation per second max angular velocity - private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 2); + private final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 3); private double m_visionSeenLastSec = Utils.getCurrentTimeSeconds(); private final BooleanLogger log_visionSeenPastSecond = new BooleanLogger(kLogTab, "VisionSeenLastSec"); @@ -397,29 +399,13 @@ private void configureBindings() { } private void configureTestBindings() { - /* GENERATED SWERVE BINDS */ - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - m_drivetrain.setDefaultCommand(driveCommand(RobotK.kRobotSpeedIntakingLimit)); + Pose2d testPose2d = new Pose2d(); + Angle rotationDegrees = Angle.ofBaseUnits(60, Degree); + testPose2d.rotateBy(new Rotation2d(rotationDegrees)); - // Idle while the robot is disabled. This ensures the configured - // neutral mode is applied to the drive motors while disabled. - final var idle = new SwerveRequest.Idle(); - RobotModeTriggers.disabled().whileTrue( - m_drivetrain.applyRequest(() -> idle).ignoringDisable(true) - ); - - // Reset the field-centric heading on left bumper press. - m_driver.leftBumper().and(trg_manipOverride.negate()).onTrue(m_drivetrain.runOnce(m_drivetrain::seedFieldCentric)); + m_driver.povUp().whileTrue(m_drivetrain.toPose(testPose2d)); - m_drivetrain.registerTelemetry(logger::telemeterize); - - // Run SysId routines when holding back/start and X/Y. - // Note that each routine should be run exactly once in a single log. - // m_driver.back().and(m_driver.y()).whileTrue(m_drivetrain.sysIdDynamic(Direction.kForward)); - // m_driver.back().and(m_driver.x()).whileTrue(m_drivetrain.sysIdDynamic(Direction.kReverse)); - // m_driver.start().and(m_driver.y()).whileTrue(m_drivetrain.sysIdQuasistatic(Direction.kForward)); - // m_driver.start().and(m_driver.x()).whileTrue(m_drivetrain.sysIdQuasistatic(Direction.kReverse)); + configureBindings(); } private void configureTestingDashboard() { From ed0b732030011767796aa4f9e710622b08c77377 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Thu, 12 Mar 2026 21:32:58 -0400 Subject: [PATCH 11/23] 90 deg coolsies (UNTESTED) --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 12d6d338..4bd6d59e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -400,7 +400,7 @@ private void configureBindings() { private void configureTestBindings() { Pose2d testPose2d = new Pose2d(); - Angle rotationDegrees = Angle.ofBaseUnits(60, Degree); + Angle rotationDegrees = Degrees.of(90); testPose2d.rotateBy(new Rotation2d(rotationDegrees)); m_driver.povUp().whileTrue(m_drivetrain.toPose(testPose2d)); From 9df7b03fd0e1e8e08be844b747b04a0c11592ef3 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sat, 14 Mar 2026 14:44:24 -0400 Subject: [PATCH 12/23] new P for rotational --- src/main/java/frc/robot/Robot.java | 9 +++--- .../java/frc/robot/subsystems/Swerve.java | 31 +++++++++++++++++-- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4bd6d59e..ba3fecf0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -263,8 +263,8 @@ private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean sh private Command driveCommandWithSwerveShimmying(double speedMultiplier) { // Return a sequence of two swerveRequests to shimmy in alternating directions return Commands.repeatingSequence( - swerveRequestWithSwerveShimmy(speedMultiplier, true).withTimeout(0.113), - swerveRequestWithSwerveShimmy(speedMultiplier, false).withTimeout(0.1) + swerveRequestWithSwerveShimmy(speedMultiplier, true), //.withTimeout(0.113), + swerveRequestWithSwerveShimmy(speedMultiplier, false) //.withTimeout(0.1) ); } @@ -399,11 +399,10 @@ private void configureBindings() { } private void configureTestBindings() { - Pose2d testPose2d = new Pose2d(); Angle rotationDegrees = Degrees.of(90); - testPose2d.rotateBy(new Rotation2d(rotationDegrees)); - m_driver.povUp().whileTrue(m_drivetrain.toPose(testPose2d)); + m_driver.povUp().whileTrue(m_drivetrain.roboToAngle(rotationDegrees)); + m_driver.povLeft().whileTrue(m_drivetrain.roboToAngle(Degrees.zero())); configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 65637839..059bb091 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; +import static frc.robot.Constants.IndexerK.kLogTab; import java.util.Optional; import java.util.function.Supplier; @@ -28,8 +29,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -39,6 +42,7 @@ import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.vision.Detection; import frc.util.WaltLogger; +import frc.util.WaltLogger.DoubleLogger; /** * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class @@ -68,13 +72,16 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final PIDController m_pathXController = new PIDController(7.7, 0, 0); private final PIDController m_pathYController = new PIDController(7.7, 0, 0); - private final PIDController m_pathThetaController = new PIDController(7, 0, 0); + private final PIDController m_pathThetaController = new PIDController(4.68, 0, 0); private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds() .withDriveRequestType(DriveRequestType.Velocity) .withSteerRequestType(SteerRequestType.Position); private final Detection detection = new Detection(); + private DoubleLogger log_PIDThetaOutput = WaltLogger.logDouble("Swerve", "PIDThetaOutput"); + private DoubleLogger log_PIDThetaInput = WaltLogger.logDouble("Swerve", "PIDThetaInput"); + private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -214,6 +221,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } + SmartDashboard.putData("Turning PID", m_pathThetaController); } /** @@ -333,7 +341,7 @@ public ChassisSpeeds getChassisSpeeds() { return m_kinematics.toChassisSpeeds(getState().ModuleStates); } - private void followPath(SwerveSample sample) { + private void followPath(SwerveSample sample) { m_pathThetaController.enableContinuousInput(-Math.PI, Math.PI); var pose = getState().Pose; var samplePose = sample.getPose(); @@ -406,11 +414,30 @@ public Command toPose(Pose2d destination) { double ySpeed = m_pathYController.calculate(curPose.getY(), destination.getY()); double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destination.getRotation().getRadians()); + log_PIDThetaInput.accept(destination.getRotation().getDegrees()); + log_PIDThetaOutput.accept(thetaSpeed); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); } ); } + public Command roboToAngle(Angle desiredAngle) { + return Commands.run( + () -> { + Pose2d curPose = getState().Pose; + + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); + + log_PIDThetaInput.accept(desiredAngle.in(Degrees)); + log_PIDThetaOutput.accept(thetaSpeed); + + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + } + + ); + } + /** * robot goes to detected target */ From f8399db0f571e32c520ff7826c44208db46a1259 Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sat, 14 Mar 2026 15:27:51 -0400 Subject: [PATCH 13/23] tune PID of swerve during auton --- src/main/java/frc/robot/Robot.java | 55 +------------------ .../java/frc/robot/subsystems/Swerve.java | 43 +++++++++++---- .../frc/robot/subsystems/shooter/Shooter.java | 10 ++++ 3 files changed, 44 insertions(+), 64 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ba3fecf0..fda45d54 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -220,53 +220,6 @@ private Command driveCommand(double speedMultiplier) { } ); } - /** - * Returns a modified swerveRequest which uses swerveShimmy's rotational value instead of the driver's stick's. - * Does not move the robot, only returns a swerveRequest. - * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing - * @param shimmyCCW If the shimmy direction is CCW - * @return A swerveRequest with shimmying rotational values - */ - private Command swerveRequestWithSwerveShimmy(double speedMultiplier, boolean shimmyCCW) { - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - // Drivetrain will NOT execute this command periodically - return m_drivetrain.applyRequest(() -> { - LinearVelocity translationSpeed = (m_driver.leftTrigger().getAsBoolean() ? - kMaxTranslationSpeed.times(speedMultiplier) : - kMaxTranslationSpeed); - - var driverXVelo = translationSpeed.times(-m_driver.getLeftY()); - var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); - // Use shimmyAngularRate over driver request - var yawRate = kSwerveShimmyAngularRate.times(shimmyCCW ? 1 : -1); - - log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); - log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); - log_stickDesiredFieldZRot.accept(kDriverMaxAngularRate.times(-m_driver.getRightX()).in(RotationsPerSecond)); // Logging driver requests even though they have no impact - log_robotDesiredFieldZRot.accept(yawRate.in(RotationsPerSecond)); - log_swerveShimmying.accept(true); - - return drive - .withVelocityX(driverXVelo) // Drive forward with Y (forward) - .withVelocityY(driverYVelo) // Drive left with X (left) - .withRotationalRate(yawRate); // Use shimmy rotation rate - } - ); - } - /** - * A modified driveCommand that utilizes the swerveShimmy rotational value instead of what the driver requests. - * Translational control should not be affected. Regular driveCommand is used as defaultCommand. - * @param speedMultiplier How much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing - * @return Swerve driveCommand with swerveShimmy rotational values - */ - private Command driveCommandWithSwerveShimmying(double speedMultiplier) { - // Return a sequence of two swerveRequests to shimmy in alternating directions - return Commands.repeatingSequence( - swerveRequestWithSwerveShimmy(speedMultiplier, true), //.withTimeout(0.113), - swerveRequestWithSwerveShimmy(speedMultiplier, false) //.withTimeout(0.1) - ); - } //(nonsotm (just for simulating entire robot)) BLARGHHHHHH get intake dude (alex?) to give me his code (idk if he finished it yet) private void configureFuelSim() { @@ -371,7 +324,7 @@ private void configureBindings() { trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); - trg_swerveShimmy.whileTrue(driveCommandWithSwerveShimmying(RobotK.kRobotSpeedIntakingLimit)); + // trg_swerveShimmy.whileTrue(m_drivetrain.swerveShimmy()); trg_unjam.whileTrue( m_superstructure.unjamCmd() @@ -399,10 +352,8 @@ private void configureBindings() { } private void configureTestBindings() { - Angle rotationDegrees = Degrees.of(90); - - m_driver.povUp().whileTrue(m_drivetrain.roboToAngle(rotationDegrees)); - m_driver.povLeft().whileTrue(m_drivetrain.roboToAngle(Degrees.zero())); + // m_driver.povUp().whileTrue(m_drivetrain.roboToTranslation(Meters.of(0.5), Meters.of(0.0))); + // m_driver.povLeft().whileTrue(m_drivetrain.roboToTranslation(Meters.zero(), Meters.zero())); configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 059bb091..05247809 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -24,12 +24,15 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; 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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -70,8 +73,8 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private final PIDController m_pathXController = new PIDController(7.7, 0, 0); - private final PIDController m_pathYController = new PIDController(7.7, 0, 0); + private final PIDController m_pathXController = new PIDController(4.69, 0, 0); + private final PIDController m_pathYController = new PIDController(4.69, 0, 0); private final PIDController m_pathThetaController = new PIDController(4.68, 0, 0); private final SwerveRequest.ApplyFieldSpeeds m_pathApplyFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds() .withDriveRequestType(DriveRequestType.Velocity) @@ -79,9 +82,6 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final Detection detection = new Detection(); - private DoubleLogger log_PIDThetaOutput = WaltLogger.logDouble("Swerve", "PIDThetaOutput"); - private DoubleLogger log_PIDThetaInput = WaltLogger.logDouble("Swerve", "PIDThetaInput"); - private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -165,6 +165,11 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } + + // SmartDashboard.putData("Turning PID", m_pathThetaController); + // SmartDashboard.putData("X Controller", m_pathXController); + // SmartDashboard.putData("Y Controller", m_pathYController); + } /** @@ -189,6 +194,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } + } /** @@ -221,7 +227,6 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } - SmartDashboard.putData("Turning PID", m_pathThetaController); } /** @@ -414,9 +419,6 @@ public Command toPose(Pose2d destination) { double ySpeed = m_pathYController.calculate(curPose.getY(), destination.getY()); double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destination.getRotation().getRadians()); - log_PIDThetaInput.accept(destination.getRotation().getDegrees()); - log_PIDThetaOutput.accept(thetaSpeed); - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); } ); @@ -429,15 +431,32 @@ public Command roboToAngle(Angle desiredAngle) { double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); - log_PIDThetaInput.accept(desiredAngle.in(Degrees)); - log_PIDThetaOutput.accept(thetaSpeed); - setControl(swreq_drive.withRotationalRate(thetaSpeed)); } ); } + public Command roboToTranslation(Distance x, Distance y) { + return Commands.run( + () -> { + Pose2d curPose = getState().Pose; + + double xSpeed = m_pathXController.calculate(curPose.getX(), x.in(Meters)); + double ySpeed = m_pathYController.calculate(curPose.getY(), y.in(Meters)); + + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + } + + ); + } + + // public Command swerveShimmy(Pose3d target) { + // return Commands.repeatingSequence( + // toPose(null) + // ); + // } + /** * robot goes to detected target */ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f8fd7deb..2f1585f8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -78,6 +78,7 @@ public class Shooter extends SubsystemBase { private boolean m_onLeftSide; //need to implement the differing targets (if in neutral zone, shoot to X point (passing)) private Translation3d m_currentTarget = Translation3d.kZero; + private Translation3d m_currentCalcTarget = Translation3d.kZero; private final TurretVisualizer m_turretVisualizer; private final FuelSim m_fuelSim; @@ -294,6 +295,14 @@ public AngularVelocity getFlywheelCalcVelocity() { return RotationsPerSecond.of(m_calcFlywheelVelocity.in(RotationsPerSecond)); } + public Translation3d getCurrentTarget() { + return m_currentTarget; + } + + public Translation3d getCurrentCalcTarget() { + return m_currentCalcTarget; + } + public double getFlywheelStatorCurrent() { return m_shooterA.getStatorCurrent().getValueAsDouble(); } @@ -488,6 +497,7 @@ private void calculateAndSetShot(Pose2d robotPose, boolean staticShot, Angle tur // setHoodPosition(Degrees.of(calculatedShot.getHoodAngle().in(Degrees))); m_calcTurret = azimuthAngle; + m_currentCalcTarget = calculatedShot.getTarget(); m_calcFlywheelVelocity = ShotCalculator.linearToAngularVelocity(calculatedShot.getExitVelocity(), kFlywheelRadius); m_periodicTracer.addEpoch("calculateShot/setOutputs"); } From 44bd65f2e52fccbdc11b1cfc6cbe6a5081f0a65c Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sat, 14 Mar 2026 15:29:48 -0400 Subject: [PATCH 14/23] remove unused loggers --- src/main/java/frc/robot/Robot.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fda45d54..8695fe76 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -142,15 +142,6 @@ public class Robot extends TimedRobot { private final DoubleLogger log_stickDesiredFieldZRot = WaltLogger.logDouble("Swerve", "stick desired teleop z rot"); private final DoubleLogger log_robotDesiredFieldZRot = WaltLogger.logDouble("Swerve", "robot desired teleop z rot"); private final BooleanLogger log_swerveShimmying = WaltLogger.logBoolean("Swerve", "swerveShimmying"); - private final BooleanLogger log_povUp = WaltLogger.logBoolean(kLogTab, "Pov Up"); - private final BooleanLogger log_povRight = WaltLogger.logBoolean(kLogTab, "Pov Right"); - private final BooleanLogger log_povLeft = WaltLogger.logBoolean(kLogTab, "Pov Left"); - private final BooleanLogger log_povDown = WaltLogger.logBoolean(kLogTab, "Pov Down"); - - private final BooleanLogger log_manipLeftTrigger = WaltLogger.logBoolean(kLogTab, "Manip Left Trigger"); - private final BooleanLogger log_manipRightTrigger = WaltLogger.logBoolean(kLogTab, "Manip Right Trigger"); - private final BooleanLogger log_driverLeftTrigger = WaltLogger.logBoolean(kLogTab, "Driver Left Trigger"); - private final BooleanLogger log_driverRightTrigger = WaltLogger.logBoolean(kLogTab, "Driver Right Trigger"); private final DoubleLogger log_miniPCCurrent = WaltLogger.logDouble(kLogTab, "MiniPC current"); From c94273cd7ac6469d175307714f596e101e50e8ee Mon Sep 17 00:00:00 2001 From: HrehaanB Date: Sat, 14 Mar 2026 17:19:49 -0400 Subject: [PATCH 15/23] continuing to work on swerve shimmy. UNTESTED FR NOW --- src/main/java/frc/robot/Robot.java | 6 +- .../java/frc/robot/subsystems/Swerve.java | 105 ++++++++++++------ 2 files changed, 75 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8695fe76..3f7086fb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -20,6 +20,7 @@ import choreo.auto.AutoFactory; 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.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -315,7 +316,7 @@ private void configureBindings() { trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); - // trg_swerveShimmy.whileTrue(m_drivetrain.swerveShimmy()); + trg_swerveShimmy.whileTrue(m_drivetrain.swerveShimmy(() -> m_shooter.getCurrentTarget())); trg_unjam.whileTrue( m_superstructure.unjamCmd() @@ -343,9 +344,6 @@ private void configureBindings() { } private void configureTestBindings() { - // m_driver.povUp().whileTrue(m_drivetrain.roboToTranslation(Meters.of(0.5), Meters.of(0.0))); - // m_driver.povLeft().whileTrue(m_drivetrain.roboToTranslation(Meters.zero(), Meters.zero())); - configureBindings(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 05247809..3cbfe86f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,9 +1,9 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; -import static frc.robot.Constants.IndexerK.kLogTab; import java.util.Optional; +import java.util.Set; import java.util.function.Supplier; import org.photonvision.targeting.PhotonTrackedTarget; @@ -21,11 +21,16 @@ import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; 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.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N1; @@ -43,9 +48,13 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.shooter.ShotCalculator; import frc.robot.vision.Detection; import frc.util.WaltLogger; import frc.util.WaltLogger.DoubleLogger; +import frc.util.WaltLogger.Pose2dLogger; +import frc.util.WaltLogger.Pose3dLogger; +import frc.util.WaltLogger.Transform3dLogger; /** * CommandSwerveDrivetrain: Class that extends the Phoenix 6 SwerveDrivetrain class @@ -82,6 +91,14 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final Detection detection = new Detection(); + private final DoubleLogger log_shimmyYDist = WaltLogger.logDouble("Swerve", "shimmyYDist"); + private final DoubleLogger log_shimmyXDist = WaltLogger.logDouble("Swerve", "shimmyXDist"); + private final DoubleLogger log_shimmyXMvmt = WaltLogger.logDouble("Swerve", "shimmyXMvmt"); + private final DoubleLogger log_shimmyYMvmt = WaltLogger.logDouble("Swerve", "shimmyYMvmt"); + private final Pose3dLogger log_shimmyPose = WaltLogger.logPose3d("Swerve", "shimmyPose"); + + private final Pose2dLogger log_targetPose = WaltLogger.logPose2d("Swerve", "targetPose"); + private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -411,51 +428,75 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) * @return */ public Command toPose(Pose2d destination) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + return Commands.run(() -> { + Pose2d curPose = getState().Pose; + 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()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }); + } - 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 Command roboToAngle(Angle desiredAngle) { + return Commands.run(() -> { + Pose2d curPose = getState().Pose; + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); + setControl(swreq_drive.withRotationalRate(thetaSpeed)); + }); + } - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - } + public Command roboToTranslation(Distance x, Distance y) { + return Commands.run(() -> { + Pose2d curPose = getState().Pose; + double xSpeed = m_pathXController.calculate(curPose.getX(), x.in(Meters)); + double ySpeed = m_pathYController.calculate(curPose.getY(), y.in(Meters)); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }); + } + + public Command shimmy(Distance x, Distance y, double secondsBetween, boolean waitBack) { + return Commands.sequence( + roboToTranslation(x, y), + Commands.waitSeconds(secondsBetween), + roboToTranslation(x.unaryMinus(), y.unaryMinus()), + waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none() ); } - public Command roboToAngle(Angle desiredAngle) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + private record SwerveShimmyData(Distance xMovement, Distance yMovement) {} - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); + private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) { + Pose2d curPose = getState().Pose; + var target = targetSup.get(); + Pose2d targetPose = new Pose2d(target.getMeasureX().in(Meters), target.getMeasureY().in(Meters), curPose.getRotation()); - setControl(swreq_drive.withRotationalRate(thetaSpeed)); - } + Distance xDistance = targetPose.getMeasureX().minus(curPose.getMeasureX()); + Distance yDistance = targetPose.getMeasureY().minus(curPose.getMeasureY()); - ); - } + Distance xMovement = Meters.of(MathUtil.clamp(Math.abs(xDistance.in(Meters)), 0, 0.3)); + Distance yMovement = Meters.of(MathUtil.clamp(Math.abs(yDistance.in(Meters)), 0, 0.3)); - public Command roboToTranslation(Distance x, Distance y) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + log_shimmyXDist.accept(xDistance.in(Meters)); + log_shimmyYDist.accept(yDistance.in(Meters)); - double xSpeed = m_pathXController.calculate(curPose.getX(), x.in(Meters)); - double ySpeed = m_pathYController.calculate(curPose.getY(), y.in(Meters)); + log_shimmyXMvmt.accept(xMovement.in(Meters)); + log_shimmyYMvmt.accept(yMovement.in(Meters)); - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); - } + log_targetPose.accept(targetPose); - ); + Pose3d shimmyPose = new Pose3d(new Pose2d(xDistance, yDistance, curPose.getRotation())); + log_shimmyPose.accept(shimmyPose); + return new SwerveShimmyData(xMovement, yMovement); } - // public Command swerveShimmy(Pose3d target) { - // return Commands.repeatingSequence( - // toPose(null) - // ); - // } + public Command swerveShimmy(Supplier targetSup) { + return Commands.repeatingSequence( + Commands.defer(() -> { + var shimmyDat = calcSwerveShimmyData(targetSup); + return shimmy(shimmyDat.xMovement, shimmyDat.yMovement, 0.3, true); + }, Set.of(this)) + ); + } /** * robot goes to detected target From 3a7b11af323a477e5e80cc48cd51049e9f24710c Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Mon, 16 Mar 2026 19:46:56 -0400 Subject: [PATCH 16/23] shimmying motion f --- src/main/java/frc/robot/subsystems/Swerve.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3cbfe86f..3ddd504b 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -446,7 +446,7 @@ public Command roboToAngle(Angle desiredAngle) { } public Command roboToTranslation(Distance x, Distance y) { - return Commands.run(() -> { + return Commands.runOnce(() -> { Pose2d curPose = getState().Pose; double xSpeed = m_pathXController.calculate(curPose.getX(), x.in(Meters)); double ySpeed = m_pathYController.calculate(curPose.getY(), y.in(Meters)); From a6127fe33de6a62880583fa5c2792b931b383ead Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Mon, 16 Mar 2026 21:26:47 -0400 Subject: [PATCH 17/23] UNTESTED ts doesnt work but i fixed SOME(!!!) bugs --- .../java/frc/robot/subsystems/Swerve.java | 21 ++++++++++++------- .../frc/robot/subsystems/shooter/Shooter.java | 1 - 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3ddd504b..6c78c213 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -96,7 +96,7 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final DoubleLogger log_shimmyXMvmt = WaltLogger.logDouble("Swerve", "shimmyXMvmt"); private final DoubleLogger log_shimmyYMvmt = WaltLogger.logDouble("Swerve", "shimmyYMvmt"); private final Pose3dLogger log_shimmyPose = WaltLogger.logPose3d("Swerve", "shimmyPose"); - + private final Pose2dLogger log_targetPose = WaltLogger.logPose2d("Swerve", "targetPose"); private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() @@ -455,10 +455,11 @@ public Command roboToTranslation(Distance x, Distance y) { } public Command shimmy(Distance x, Distance y, double secondsBetween, boolean waitBack) { + Pose2d curPose = getState().Pose; return Commands.sequence( - roboToTranslation(x, y), + roboToTranslation(curPose.getMeasureX().plus(x), curPose.getMeasureY().plus(y)), Commands.waitSeconds(secondsBetween), - roboToTranslation(x.unaryMinus(), y.unaryMinus()), + roboToTranslation(curPose.getMeasureX().minus(x), curPose.getMeasureY().minus(y)), waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none() ); } @@ -467,14 +468,20 @@ private record SwerveShimmyData(Distance xMovement, Distance yMovement) {} private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) { Pose2d curPose = getState().Pose; - var target = targetSup.get(); + var target = targetSup.get().toTranslation2d(); Pose2d targetPose = new Pose2d(target.getMeasureX().in(Meters), target.getMeasureY().in(Meters), curPose.getRotation()); Distance xDistance = targetPose.getMeasureX().minus(curPose.getMeasureX()); Distance yDistance = targetPose.getMeasureY().minus(curPose.getMeasureY()); + Distance xDistanceSquared = xDistance.times(xDistance.baseUnitMagnitude()); + Distance yDistanceSquared = yDistance.times(yDistance.baseUnitMagnitude()); + + Distance hypotenuseDistance = Distance.ofBaseUnits(Math.sqrt(xDistanceSquared.minus(yDistanceSquared).baseUnitMagnitude()), Meter); + + - Distance xMovement = Meters.of(MathUtil.clamp(Math.abs(xDistance.in(Meters)), 0, 0.3)); - Distance yMovement = Meters.of(MathUtil.clamp(Math.abs(yDistance.in(Meters)), 0, 0.3)); + Distance xMovement = Meters.of(MathUtil.clamp(xDistance.in(Meters), -0.3, 0.3)); + Distance yMovement = Meters.of(MathUtil.clamp(yDistance.in(Meters), -0.3, 0.3)); log_shimmyXDist.accept(xDistance.in(Meters)); log_shimmyYDist.accept(yDistance.in(Meters)); @@ -501,7 +508,7 @@ public Command swerveShimmy(Supplier targetSup) { /** * robot goes to detected target */ - public Command swerveToObject() { + public Command swerveToObject() { PhotonTrackedTarget target = detection.getClosestObject(); Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2f1585f8..06138211 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -548,7 +548,6 @@ public void periodic() { Pose2d pose = m_poseSupplier.get(); m_periodicTracer.addEpoch("Getting Values"); - m_currentTarget = calculateTarget(pose); log_globalShotTarget.accept(m_currentTarget); From ce71acd03e0a193a8fd05302a3fb72cd3fc6e9df Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Tue, 17 Mar 2026 18:43:35 -0400 Subject: [PATCH 18/23] fixed swerveshimmy (yippee) thank you saarth --- .../java/frc/robot/subsystems/Swerve.java | 88 +++++++++++++------ 1 file changed, 61 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6c78c213..63d88ec0 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -21,7 +21,6 @@ import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -95,9 +94,12 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final DoubleLogger log_shimmyXDist = WaltLogger.logDouble("Swerve", "shimmyXDist"); private final DoubleLogger log_shimmyXMvmt = WaltLogger.logDouble("Swerve", "shimmyXMvmt"); private final DoubleLogger log_shimmyYMvmt = WaltLogger.logDouble("Swerve", "shimmyYMvmt"); + private final DoubleLogger log_shimmySlope = WaltLogger.logDouble("Swerve", "shimmySlope"); private final Pose3dLogger log_shimmyPose = WaltLogger.logPose3d("Swerve", "shimmyPose"); - private final Pose2dLogger log_targetPose = WaltLogger.logPose2d("Swerve", "targetPose"); + private final Pose2dLogger log_curPose = WaltLogger.logPose2d("Swerve", "curPose"); + private final Pose2dLogger log_targetPosePos = WaltLogger.logPose2d("Swerve", "targetPosePos"); + private final Pose2dLogger log_targetPoseNeg = WaltLogger.logPose2d("Swerve", "targetPoseNeg"); private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); @@ -297,6 +299,8 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + + log_curPose.accept(getState().Pose); } private void startSimThread() { @@ -424,21 +428,31 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) /** * robot goes to specified pose - * @param destination + *

+ * this method is ragebait btw + * @param destinationPose * @return */ - public Command toPose(Pose2d destination) { - return Commands.run(() -> { + public Command toPose(Pose2d destinationPose) { + return Commands.runOnce(() -> { Pose2d curPose = getState().Pose; - 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()); + double xSpeed = m_pathXController.calculate(curPose.getX(), destinationPose.getX()); + double ySpeed = m_pathYController.calculate(curPose.getY(), destinationPose.getY()); + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destinationPose.getRotation().getRadians()); setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - }); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, destinationPose, 0.05))) + .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); + } + + public boolean isNearPose(Pose2d curPose, Pose2d destinationPose, double tolerance) { + return Math.hypot( + destinationPose.getMeasureX().minus(curPose.getMeasureX()).baseUnitMagnitude(), + destinationPose.getMeasureY().minus(curPose.getMeasureY()).baseUnitMagnitude() + ) <= tolerance; } public Command roboToAngle(Angle desiredAngle) { - return Commands.run(() -> { + return Commands.runOnce(() -> { Pose2d curPose = getState().Pose; double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); setControl(swreq_drive.withRotationalRate(thetaSpeed)); @@ -454,53 +468,73 @@ public Command roboToTranslation(Distance x, Distance y) { }); } - public Command shimmy(Distance x, Distance y, double secondsBetween, boolean waitBack) { - Pose2d curPose = getState().Pose; + public Command shimmy(Pose2d positive, Pose2d negative, double secondsBetween, boolean waitBack) { + Pose2d intialPose = getState().Pose; return Commands.sequence( - roboToTranslation(curPose.getMeasureX().plus(x), curPose.getMeasureY().plus(y)), + toPose(positive), Commands.waitSeconds(secondsBetween), - roboToTranslation(curPose.getMeasureX().minus(x), curPose.getMeasureY().minus(y)), - waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none() + toPose(negative), + waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none(), + toPose(intialPose) ); } - private record SwerveShimmyData(Distance xMovement, Distance yMovement) {} + private record SwerveShimmyData(Pose2d forward, Pose2d backward) {} private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) { + Alliance alliance = DriverStation.getAlliance().get(); Pose2d curPose = getState().Pose; var target = targetSup.get().toTranslation2d(); Pose2d targetPose = new Pose2d(target.getMeasureX().in(Meters), target.getMeasureY().in(Meters), curPose.getRotation()); + final Distance limitDistance = Meters.of(0.2); + Distance xDistance = targetPose.getMeasureX().minus(curPose.getMeasureX()); Distance yDistance = targetPose.getMeasureY().minus(curPose.getMeasureY()); - Distance xDistanceSquared = xDistance.times(xDistance.baseUnitMagnitude()); - Distance yDistanceSquared = yDistance.times(yDistance.baseUnitMagnitude()); + double slope = yDistance.baseUnitMagnitude() / xDistance.baseUnitMagnitude(); + + Distance xLimited = Meters.of(Math.sqrt( + (Math.pow(limitDistance.baseUnitMagnitude(), 2)) + / ((Math.pow(slope, 2)) + 1) + )); - Distance hypotenuseDistance = Distance.ofBaseUnits(Math.sqrt(xDistanceSquared.minus(yDistanceSquared).baseUnitMagnitude()), Meter); + Distance yLimited = xLimited.times(slope); - + Pose2d positive = new Pose2d( + alliance == Alliance.Blue ? curPose.getMeasureX().plus(xLimited) : curPose.getMeasureX().minus(xLimited), + alliance == Alliance.Blue ? curPose.getMeasureY().plus(yLimited) : curPose.getMeasureY().minus(yLimited), + curPose.getRotation() + ); + + Pose2d negative = new Pose2d( + alliance == Alliance.Blue ? curPose.getMeasureX().minus(xLimited) : curPose.getMeasureX().plus(xLimited), + alliance == Alliance.Blue ? curPose.getMeasureY().minus(yLimited) : curPose.getMeasureY().plus(yLimited), + curPose.getRotation() + ); - Distance xMovement = Meters.of(MathUtil.clamp(xDistance.in(Meters), -0.3, 0.3)); - Distance yMovement = Meters.of(MathUtil.clamp(yDistance.in(Meters), -0.3, 0.3)); + // Distance xMovement = Meters.of(MathUtil.clamp(xDistance.in(Meters), -0.3, 0.3)); + // Distance yMovement = Meters.of(MathUtil.clamp(yDistance.in(Meters), -0.3, 0.3)); log_shimmyXDist.accept(xDistance.in(Meters)); log_shimmyYDist.accept(yDistance.in(Meters)); - log_shimmyXMvmt.accept(xMovement.in(Meters)); - log_shimmyYMvmt.accept(yMovement.in(Meters)); + log_shimmyXMvmt.accept(xLimited.in(Meters)); + log_shimmyYMvmt.accept(yLimited.in(Meters)); + log_shimmySlope.accept(slope); - log_targetPose.accept(targetPose); + log_targetPosePos.accept(positive); + log_targetPoseNeg.accept(negative); Pose3d shimmyPose = new Pose3d(new Pose2d(xDistance, yDistance, curPose.getRotation())); log_shimmyPose.accept(shimmyPose); - return new SwerveShimmyData(xMovement, yMovement); + return new SwerveShimmyData(positive, negative); } public Command swerveShimmy(Supplier targetSup) { return Commands.repeatingSequence( Commands.defer(() -> { var shimmyDat = calcSwerveShimmyData(targetSup); - return shimmy(shimmyDat.xMovement, shimmyDat.yMovement, 0.3, true); + return shimmy(shimmyDat.forward, shimmyDat.backward, 0.2, true); }, Set.of(this)) ); } From a51f0110cbc14a810747485ce9a55da1560f0654 Mon Sep 17 00:00:00 2001 From: alexanderan187 Date: Wed, 18 Mar 2026 17:31:53 -0400 Subject: [PATCH 19/23] comments + clean some stuff up --- src/main/java/frc/robot/subsystems/Swerve.java | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 63d88ec0..3cfd2906 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -95,7 +95,6 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem { private final DoubleLogger log_shimmyXMvmt = WaltLogger.logDouble("Swerve", "shimmyXMvmt"); private final DoubleLogger log_shimmyYMvmt = WaltLogger.logDouble("Swerve", "shimmyYMvmt"); private final DoubleLogger log_shimmySlope = WaltLogger.logDouble("Swerve", "shimmySlope"); - private final Pose3dLogger log_shimmyPose = WaltLogger.logPose3d("Swerve", "shimmyPose"); private final Pose2dLogger log_curPose = WaltLogger.logPose2d("Swerve", "curPose"); private final Pose2dLogger log_targetPosePos = WaltLogger.logPose2d("Swerve", "targetPosePos"); @@ -489,18 +488,18 @@ private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) final Distance limitDistance = Meters.of(0.2); - Distance xDistance = targetPose.getMeasureX().minus(curPose.getMeasureX()); + Distance xDistance = targetPose.getMeasureX().minus(curPose.getMeasureX()); // X and Y distances from robot to target Distance yDistance = targetPose.getMeasureY().minus(curPose.getMeasureY()); - double slope = yDistance.baseUnitMagnitude() / xDistance.baseUnitMagnitude(); + double slope = yDistance.baseUnitMagnitude() / xDistance.baseUnitMagnitude(); // Calculate the slope of the hypotenuse - Distance xLimited = Meters.of(Math.sqrt( + Distance xLimited = Meters.of(Math.sqrt( // Normalizing x value to be within limitDistance (Math.pow(limitDistance.baseUnitMagnitude(), 2)) / ((Math.pow(slope, 2)) + 1) )); - Distance yLimited = xLimited.times(slope); + Distance yLimited = xLimited.times(slope); // y always equals x times slope - Pose2d positive = new Pose2d( + Pose2d positive = new Pose2d( // Calculate the 2 poses depending on blue or red alliance alliance == Alliance.Blue ? curPose.getMeasureX().plus(xLimited) : curPose.getMeasureX().minus(xLimited), alliance == Alliance.Blue ? curPose.getMeasureY().plus(yLimited) : curPose.getMeasureY().minus(yLimited), curPose.getRotation() @@ -525,8 +524,6 @@ private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) log_targetPosePos.accept(positive); log_targetPoseNeg.accept(negative); - Pose3d shimmyPose = new Pose3d(new Pose2d(xDistance, yDistance, curPose.getRotation())); - log_shimmyPose.accept(shimmyPose); return new SwerveShimmyData(positive, negative); } From 506be2c6562b2b2d8355d61ef0263831c923522e Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 18 Mar 2026 19:40:11 -0400 Subject: [PATCH 20/23] fixed roboToRotation and roboToTranslation methods no longer throughRotation and throughTranslation --- .../java/frc/robot/subsystems/Swerve.java | 79 ++++++++++++------- 1 file changed, 50 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 3cfd2906..a7356d87 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -21,6 +21,7 @@ import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -426,55 +427,75 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) } /** - * robot goes to specified pose - *

- * this method is ragebait btw - * @param destinationPose - * @return + * Robot moves to desired pose + * @param desPose pose to move to */ - public Command toPose(Pose2d destinationPose) { + public Command roboToPose(Pose2d desPose) { + Pose2d curPose = getState().Pose; return Commands.runOnce(() -> { - Pose2d curPose = getState().Pose; - double xSpeed = m_pathXController.calculate(curPose.getX(), destinationPose.getX()); - double ySpeed = m_pathYController.calculate(curPose.getY(), destinationPose.getY()); - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), destinationPose.getRotation().getRadians()); + 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, destinationPose, 0.05))) + }).andThen(Commands.waitUntil(() -> isNearPose(curPose, desPose, 0.05))) .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); } - public boolean isNearPose(Pose2d curPose, Pose2d destinationPose, double tolerance) { - return Math.hypot( - destinationPose.getMeasureX().minus(curPose.getMeasureX()).baseUnitMagnitude(), - destinationPose.getMeasureY().minus(curPose.getMeasureY()).baseUnitMagnitude() - ) <= tolerance; + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) + && isNearAngle(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + } + + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); } - public Command roboToAngle(Angle desiredAngle) { + /** + * Robot turns to desired angle + * @param desiredAngle angle to turn to + */ + public Command roboToAngle(Rotation2d desRotation) { + Rotation2d curRotation = getState().Pose.getRotation(); return Commands.runOnce(() -> { - Pose2d curPose = getState().Pose; - double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desiredAngle.in(Radians)); + double thetaSpeed = m_pathThetaController.calculate(curRotation.getRadians(), desRotation.getRadians()); setControl(swreq_drive.withRotationalRate(thetaSpeed)); - }); + }).andThen(Commands.waitUntil(() -> isNearAngle(curRotation, desRotation, 0.05))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); } - public Command roboToTranslation(Distance x, Distance y) { + public boolean isNearAngle(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + } + + /** + * Robot goes to desired translation + * @param desTranslation translation to go to + */ + public Command roboToTranslation(Translation2d desTranslation) { + Translation2d curTranslation = getState().Pose.getTranslation(); return Commands.runOnce(() -> { - Pose2d curPose = getState().Pose; - double xSpeed = m_pathXController.calculate(curPose.getX(), x.in(Meters)); - double ySpeed = m_pathYController.calculate(curPose.getY(), y.in(Meters)); + 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(curTranslation, desTranslation, 0.05))) + .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; } public Command shimmy(Pose2d positive, Pose2d negative, double secondsBetween, boolean waitBack) { Pose2d intialPose = getState().Pose; return Commands.sequence( - toPose(positive), + roboToPose(positive), Commands.waitSeconds(secondsBetween), - toPose(negative), + roboToPose(negative), waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none(), - toPose(intialPose) + roboToPose(intialPose) ); } @@ -544,7 +565,7 @@ public Command swerveToObject() { Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - return toPose(destination); + return roboToPose(destination); } public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { From 0d692ec7a6eade99dd937d141d38bad16e1bad8a Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 18 Mar 2026 20:20:39 -0400 Subject: [PATCH 21/23] changed swerveshilly to use translations instead of poses --- .../java/frc/robot/subsystems/Swerve.java | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index a7356d87..ae9b9be8 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,6 +6,8 @@ import java.util.Set; import java.util.function.Supplier; +import javax.naming.InitialContext; + import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.SignalLogger; @@ -431,19 +433,19 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) * @param desPose pose to move to */ public Command roboToPose(Pose2d desPose) { - Pose2d curPose = getState().Pose; 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(curPose, desPose, 0.05))) + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, 0.05))) .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); } public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance, double rotationTolerance) { return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance) - && isNearAngle(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); } public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { @@ -454,16 +456,16 @@ public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTole * Robot turns to desired angle * @param desiredAngle angle to turn to */ - public Command roboToAngle(Rotation2d desRotation) { - Rotation2d curRotation = getState().Pose.getRotation(); + public Command roboToRotation(Rotation2d desRotation) { 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(() -> isNearAngle(curRotation, desRotation, 0.05))) + }).andThen(Commands.waitUntil(() -> isNearRotation(getState().Pose.getRotation(), desRotation, 0.05))) .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); } - public boolean isNearAngle(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); } @@ -472,12 +474,12 @@ public boolean isNearAngle(Rotation2d curRotation, Rotation2d desRotation, doubl * @param desTranslation translation to go to */ public Command roboToTranslation(Translation2d desTranslation) { - Translation2d curTranslation = getState().Pose.getTranslation(); 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(curTranslation, desTranslation, 0.05))) + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, 0.05))) .andThen(() -> setControl(swreq_drive.withVelocityX(0).withVelocityY(0).withRotationalRate(0))); } @@ -488,18 +490,18 @@ public boolean isNearTranslation(Translation2d curTranslation, Translation2d des ) <= tolerance; } - public Command shimmy(Pose2d positive, Pose2d negative, double secondsBetween, boolean waitBack) { - Pose2d intialPose = getState().Pose; + public Command shimmy(Translation2d positive, Translation2d negative, double secondsBetween, boolean waitBack) { + Translation2d intialTranslation = getState().Pose.getTranslation(); return Commands.sequence( - roboToPose(positive), + roboToTranslation(positive), Commands.waitSeconds(secondsBetween), - roboToPose(negative), + roboToTranslation(negative), waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none(), - roboToPose(intialPose) + roboToTranslation(intialTranslation) ); } - private record SwerveShimmyData(Pose2d forward, Pose2d backward) {} + private record SwerveShimmyData(Translation2d forward, Translation2d backward) {} private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) { Alliance alliance = DriverStation.getAlliance().get(); @@ -520,16 +522,14 @@ private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) Distance yLimited = xLimited.times(slope); // y always equals x times slope - Pose2d positive = new Pose2d( // Calculate the 2 poses depending on blue or red alliance + Translation2d positive = new Translation2d( // Calculate the 2 poses depending on blue or red alliance alliance == Alliance.Blue ? curPose.getMeasureX().plus(xLimited) : curPose.getMeasureX().minus(xLimited), - alliance == Alliance.Blue ? curPose.getMeasureY().plus(yLimited) : curPose.getMeasureY().minus(yLimited), - curPose.getRotation() + alliance == Alliance.Blue ? curPose.getMeasureY().plus(yLimited) : curPose.getMeasureY().minus(yLimited) ); - Pose2d negative = new Pose2d( + Translation2d negative = new Translation2d( alliance == Alliance.Blue ? curPose.getMeasureX().minus(xLimited) : curPose.getMeasureX().plus(xLimited), - alliance == Alliance.Blue ? curPose.getMeasureY().minus(yLimited) : curPose.getMeasureY().plus(yLimited), - curPose.getRotation() + alliance == Alliance.Blue ? curPose.getMeasureY().minus(yLimited) : curPose.getMeasureY().plus(yLimited) ); // Distance xMovement = Meters.of(MathUtil.clamp(xDistance.in(Meters), -0.3, 0.3)); From 7e79397d3e6bd715b08868164f9f4ed6cc33f948 Mon Sep 17 00:00:00 2001 From: Saarth Date: Wed, 18 Mar 2026 20:30:10 -0400 Subject: [PATCH 22/23] added a rotational shimmy --- .../java/frc/robot/subsystems/Swerve.java | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ae9b9be8..89af8778 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -23,6 +23,7 @@ import choreo.Choreo.TrajectoryLogger; import choreo.auto.AutoFactory; import choreo.trajectory.SwerveSample; +import edu.wpi.first.hal.simulation.RoboRioDataJNI; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; @@ -490,7 +491,7 @@ public boolean isNearTranslation(Translation2d curTranslation, Translation2d des ) <= tolerance; } - public Command shimmy(Translation2d positive, Translation2d negative, double secondsBetween, boolean waitBack) { + public Command translationShimmy(Translation2d positive, Translation2d negative, double secondsBetween, boolean waitBack) { Translation2d intialTranslation = getState().Pose.getTranslation(); return Commands.sequence( roboToTranslation(positive), @@ -501,6 +502,17 @@ public Command shimmy(Translation2d positive, Translation2d negative, double sec ); } + public Command rotationShimmy(Rotation2d variation, double secondsBetween, boolean waitBack) { + Rotation2d initialRotation = getState().Pose.getRotation(); + return Commands.sequence( + roboToRotation(initialRotation.plus(variation)), + Commands.waitSeconds(secondsBetween), + roboToRotation(initialRotation.minus(variation)), + waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none(), + roboToRotation(initialRotation) + ); + } + private record SwerveShimmyData(Translation2d forward, Translation2d backward) {} private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) { @@ -551,8 +563,9 @@ private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) public Command swerveShimmy(Supplier targetSup) { return Commands.repeatingSequence( Commands.defer(() -> { - var shimmyDat = calcSwerveShimmyData(targetSup); - return shimmy(shimmyDat.forward, shimmyDat.backward, 0.2, true); + // var shimmyDat = calcSwerveShimmyData(targetSup); + // return translationShimmy(shimmyDat.forward, shimmyDat.backward, 0.2, true); + return rotationShimmy(new Rotation2d(0.5), 0.2, true); }, Set.of(this)) ); } From c88cf11b10ab13616fef8c73c15be05737193d44 Mon Sep 17 00:00:00 2001 From: Saarth Date: Mon, 23 Mar 2026 19:47:26 -0400 Subject: [PATCH 23/23] fixed bad merge issues **NOTE** sim robot pose no work no more so i need to fix that --- src/main/java/frc/robot/Robot.java | 13 +++++----- .../java/frc/robot/subsystems/Swerve.java | 13 +++++++--- .../frc/robot/subsystems/shooter/Shooter.java | 20 +++++++--------- src/main/java/frc/util/WaltLogger.java | 24 +++++++++++++++++++ 4 files changed, 49 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8544a6df..0e8c42ae 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -210,11 +210,11 @@ private Command driveCommand(double speedMultiplier) { var driverYVelo = translationSpeed.times(-m_driver.getLeftX()); var driverYawRate = kDriverMaxAngularRate.times(-m_driver.getRightX()); - log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); - log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); - log_stickDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); - log_robotDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); // Should be equal to stick desired IN THIS CASE - log_swerveShimmying.accept(false); + // log_stickDesiredFieldX.accept(driverXVelo.in(MetersPerSecond)); + // log_stickDesiredFieldY.accept(driverYVelo.in(MetersPerSecond)); + // log_stickDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); + // log_robotDesiredFieldZRot.accept(driverYawRate.in(RotationsPerSecond)); // Should be equal to stick desired IN THIS CASE + // log_swerveShimmying.accept(false); return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) @@ -343,7 +343,7 @@ private void configureBindings() { trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); - trg_swerveShimmy.whileTrue(m_drivetrain.swerveShimmy(() -> m_shooter.getCurrentTarget())); + trg_swerveShimmy.whileTrue(m_drivetrain.swerveTranslationShimmy(m_shooter.getTargetPose())); trg_unjam.and(trg_shoot.negate()).whileTrue( m_superstructure.unjamCmd(() -> false) @@ -372,6 +372,7 @@ private void configureBindings() { m_driver.povDown().onTrue(m_shooter.setTurretLockCmd(false)); m_driver.povRight().onTrue(m_shooter.setTurretLockCmd(true)); + } private void configureTestBindings() { m_driver.povLeft().onTrue(m_hood.setHoodPosCmd(kHoodMinPosition)); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 9c5b81d2..43b80883 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -563,16 +563,23 @@ private SwerveShimmyData calcSwerveShimmyData(Supplier targetSup) return new SwerveShimmyData(positive, negative); } - public Command swerveShimmy(Supplier targetSup) { + public Command swerveRotationShimmy(Supplier targetSup) { return Commands.repeatingSequence( Commands.defer(() -> { - // var shimmyDat = calcSwerveShimmyData(targetSup); - // return translationShimmy(shimmyDat.forward, shimmyDat.backward, 0.2, true); return rotationShimmy(new Rotation2d(0.5), 0.2, true); }, Set.of(this)) ); } + public Command swerveTranslationShimmy(Supplier targetSup) { + return Commands.repeatingSequence( + Commands.defer(() -> { + var shimmyDat = calcSwerveShimmyData(targetSup); + return translationShimmy(shimmyDat.forward, shimmyDat.backward, 0.2, true); + }, Set.of(this)) + ); + } + /** * robot goes to detected target */ diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8171748f..0ae63f25 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -299,18 +299,6 @@ public boolean isShooterSpunUp() { log_spunUp.accept(isNear); return isNear; } - - public Translation3d getCurrentTarget() { - return m_currentTarget; - } - - public Translation3d getCurrentCalcTarget() { - return m_currentCalcTarget; - } - - public double getFlywheelStatorCurrent() { - return m_shooterA.getStatorCurrent().getValueAsDouble(); - } // ---TURRET (Motionmagic Angle Control) public Command setTurretPosCmd(Angle rots) { @@ -341,6 +329,14 @@ public AngularVelocity getShooterVelocity() { return m_flywheelVelocity; } + public double getFlywheelStatorCurrent() { + return m_shooterA.getStatorCurrent().getValueAsDouble(); + } + + public Supplier getTargetPose() { + return () -> m_shooterCalc.getLatestAimTarget(); + } + /* SIMULATION */ public boolean simAbleToIntake() { return canIntake(); diff --git a/src/main/java/frc/util/WaltLogger.java b/src/main/java/frc/util/WaltLogger.java index 64a6d0c1..f1271253 100644 --- a/src/main/java/frc/util/WaltLogger.java +++ b/src/main/java/frc/util/WaltLogger.java @@ -162,6 +162,30 @@ public static Pose3dLogger logPose3d(String name, String table, PubSubOption... return new Pose3dLogger(name, table, options); } + public static final class Transform3dLogger implements Consumer { + public final StructPublisher ntPub; + public final StructLogEntry logEntry; + + public Transform3dLogger(String subTable, String name, PubSubOption... options) { + StructTopic topic = logTable.getSubTable(subTable).getStructTopic(name, new Transform3dStruct()); + ntPub = topic.publish(options); + logEntry = StructLogEntry.create(DataLogManager.getLog(), "Robot/" + subTable + "/" + name, new Transform3dStruct()); + } + + @Override + public void accept(Transform3d value) { + if (shouldPublishNt()) { + ntPub.set(value); + } else { + logEntry.append(value); + } + } + } + + public static Transform3dLogger logTransform3d(String name, String table, PubSubOption... options) { + return new Transform3dLogger(name, table, options); + } + // public static final class Translation2dLogger implements Consumer { // public final StructPublisher ntPub; // public final StructLogEntry logEntry;