diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e1c267ff..0e8c42ae 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,9 @@ 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; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DataLogManager; @@ -62,14 +65,15 @@ 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 final AngularVelocity kSwerveShimmyAngularRate = RotationsPerSecond.of(1.3 / 3); 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(); @@ -121,6 +125,7 @@ public class Robot extends TimedRobot { private Trigger trg_emergencyBarf = m_driver.rightTrigger().and(trg_driverOverride); private Trigger trg_shimmy = m_manipulator.leftBumper(); + private Trigger trg_swerveShimmy = m_driver.leftBumper(); //---OVERRIDE TRIGGERS private Trigger trg_deployIntakeOverride = trg_manipOverride.and(m_manipulator.rightTrigger()); @@ -188,8 +193,8 @@ public Robot() { /* COMMANDS */ /** - * - * @param speedMultiplier how much you want to limit speed as a decimal percentage of kMaxTranslation. 1 does nothing + * 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 */ private Command driveCommand(double speedMultiplier) { @@ -197,18 +202,20 @@ 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); - + 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)); + // 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)); - return drive .withVelocityX(driverXVelo) // Drive forward with Y (forward) .withVelocityY(driverYVelo) // Drive left with X (left) @@ -217,6 +224,42 @@ private Command driveCommand(double speedMultiplier) { ); } +// //(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() { +// FuelSim instance = FuelSim.getInstance(); +// // instance.spawnStartingFuel(); + +// instance.registerRobot( +// kRobotFullWidth.in(Meters), +// kRobotFullLength.in(Meters), +// kBumperHeight.in(Meters), +// () -> m_drivetrain.getState().Pose, +// () -> m_drivetrain.getChassisSpeeds()); +// // instance.registerIntake( +// // -kRobotFullLength.div(2).in(Meters), +// // kRobotFullLength.div(2).in(Meters), +// // -kRobotFullWidth.div(2).plus(Inches.of(7)).in(Meters), +// // -kRobotFullWidth.div(2).in(Meters), +// // () -> intake.isRightDeployed() && m_shooter.simAbleToIntake(), +// // m_shooter::simIntake); +// // instance.registerIntake( +// // -kRobotFullLength.div(2).in(Meters), +// // kRobotFullLength.div(2).in(Meters), +// // kRobotFullWidth.div(2).in(Meters), +// // kRobotFullWidth.div(2).plus(Inches.of(7)).in(Meters), +// // () -> intake.isLeftDeployed() && m_shooter.simAbleToIntake(), +// // m_shooter::simIntake); + +// instance.start(); +// instance.logFuels(); +// SmartDashboard.putData(Commands.runOnce(() -> { +// FuelSim.getInstance().clearFuel(); +// FuelSim.getInstance().spawnStartingFuel(); +// }) +// .withName("Reset Fuel") +// .ignoringDisable(true)); +// } + private void setBothRumble(RumbleType type, double intensity) { m_driver.setRumble(type, intensity); m_manipulator.setRumble(type, intensity); @@ -254,7 +297,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) @@ -297,8 +340,10 @@ private void configureBindings() { trg_emergencyBarf.whileTrue( m_superstructure.emergencyBarf() ); - - trg_shimmy.whileTrue(m_superstructure.shimmy()); + + trg_shimmy.whileTrue(m_superstructure.intakeArmShimmy()); + + trg_swerveShimmy.whileTrue(m_drivetrain.swerveTranslationShimmy(m_shooter.getTargetPose())); trg_unjam.and(trg_shoot.negate()).whileTrue( m_superstructure.unjamCmd(() -> false) @@ -327,21 +372,11 @@ private void configureBindings() { m_driver.povDown().onTrue(m_shooter.setTurretLockCmd(false)); m_driver.povRight().onTrue(m_shooter.setTurretLockCmd(true)); - - // m_driver.start().whileTrue(m_superstructure.activateOuttakeNOSHOOT()); - // trg_optimalPrefireTime.whileTrue( - // Commands.run(() -> setBothRumble(RumbleType.kBothRumble, 0.5)).finallyDo(() -> setBothRumble(RumbleType.kBothRumble, 0)) - // ); - - // trg_comebackTime.whileTrue( - // Commands.run(() -> setBothRumble(RumbleType.kRightRumble, 0.5)).finallyDo(()-> setBothRumble(RumbleType.kRightRumble, 0)) - // ); } private void configureTestBindings() { m_driver.povLeft().onTrue(m_hood.setHoodPosCmd(kHoodMinPosition)); m_driver.povUp().onTrue(m_hood.setHoodPosCmd(kHoodMaxDegs)); - } private void configureTestingDashboard() { @@ -421,7 +456,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(); } @@ -485,7 +520,7 @@ public void teleopExit() {} @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); - + CommandScheduler.getInstance().schedule( Commands.sequence( m_drivetrain.runOnce(m_drivetrain::seedFieldCentric), @@ -509,7 +544,7 @@ public void testInit() { m_drivetrain.applyRequest(() -> drive.withVelocityX(0) .withVelocityY(0) - .withRotationalRate(kMaxAngularRate) + .withRotationalRate(kDriverMaxAngularRate) ), Commands.waitSeconds(2.5), m_drivetrain.xBrakeCmd(), diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 3979598b..81fbc46b 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -222,7 +222,7 @@ public Command emergencyBarf() { ); } - public Command shimmy() { + public Command intakeArmShimmy() { return m_intake.shimmy(); } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index beda904b..43b80883 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -3,8 +3,11 @@ import static edu.wpi.first.units.Units.*; import java.util.Optional; +import java.util.Set; import java.util.function.Supplier; +import javax.naming.InitialContext; + import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.SignalLogger; @@ -20,16 +23,27 @@ 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; 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; 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; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -37,12 +51,16 @@ 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 + * 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 @@ -76,6 +94,15 @@ 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 DoubleLogger log_shimmySlope = WaltLogger.logDouble("Swerve", "shimmySlope"); + + 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 DoubleLogger log_absoluteRobotSpeed = new WaltLogger.DoubleLogger("Swerve", "absoluteRobotSpeed"); private final SwerveRequest.FieldCentric swreq_drive = new SwerveRequest.FieldCentric() @@ -161,6 +188,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); + } /** @@ -185,6 +217,7 @@ public Swerve( if (Utils.isSimulation()) { startSimThread(); } + } /** @@ -271,6 +304,7 @@ public void periodic() { }); } + log_curPose.accept(getState().Pose); ChassisSpeeds currentChassisSpeeds = this.getChassisSpeeds(); log_absoluteRobotSpeed.accept(Math.hypot(currentChassisSpeeds.vxMetersPerSecond, currentChassisSpeeds.vyMetersPerSecond)); } @@ -369,7 +403,6 @@ public Command xBrakeCmd() { return runOnce(() -> setControl(stopReq)); } - /** * Creates a new auto factory for this drivetrain. * @@ -400,33 +433,162 @@ public AutoFactory createAutoFactory(TrajectoryLogger trajLogger) } /** - * robot goes to specified pose - * @param destination - * @return + * Robot moves to desired pose + * @param desPose pose to move to + */ + public Command roboToPose(Pose2d desPose) { + return Commands.runOnce(() -> { + Pose2d curPose = getState().Pose; + double xSpeed = m_pathXController.calculate(curPose.getX(), desPose.getX()); + double ySpeed = m_pathYController.calculate(curPose.getY(), desPose.getY()); + double thetaSpeed = m_pathThetaController.calculate(curPose.getRotation().getRadians(), desPose.getRotation().getRadians()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); + }).andThen(Commands.waitUntil(() -> isNearPose(getState().Pose, desPose, 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) + && isNearRotation(curPose.getRotation(), desPose.getRotation(), rotationTolerance); + } + + public boolean isNearPose(Pose2d curPose, Pose2d desPose, double translationTolerance) { + return isNearTranslation(curPose.getTranslation(), desPose.getTranslation(), translationTolerance); + } + + /** + * Robot turns to desired angle + * @param desiredAngle angle to turn to + */ + 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(() -> isNearRotation(getState().Pose.getRotation(), desRotation, 0.05))) + .andThen(() -> setControl(swreq_drive.withRotationalRate(0))); + } + + public boolean isNearRotation(Rotation2d curRotation, Rotation2d desRotation, double tolerance) { + return Radians.of(curRotation.getRadians()).isNear(Radians.of(desRotation.getRadians()), Radians.of(tolerance)); + } + + /** + * Robot goes to desired translation + * @param desTranslation translation to go to */ - public Command toPose(Pose2d destination) { - return Commands.run( - () -> { - Pose2d curPose = getState().Pose; + public Command roboToTranslation(Translation2d desTranslation) { + return Commands.runOnce(() -> { + Translation2d curTranslation = getState().Pose.getTranslation(); + double xSpeed = m_pathXController.calculate(curTranslation.getX(), desTranslation.getX()); + double ySpeed = m_pathYController.calculate(curTranslation.getY(), desTranslation.getY()); + setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed)); + }).andThen(Commands.waitUntil(() -> isNearTranslation(getState().Pose.getTranslation(), desTranslation, 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 translationShimmy(Translation2d positive, Translation2d negative, double secondsBetween, boolean waitBack) { + Translation2d intialTranslation = getState().Pose.getTranslation(); + return Commands.sequence( + roboToTranslation(positive), + Commands.waitSeconds(secondsBetween), + roboToTranslation(negative), + waitBack ? Commands.waitSeconds(secondsBetween) : Commands.none(), + roboToTranslation(intialTranslation) + ); + } + + 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) { + 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()); // X and Y distances from robot to target + Distance yDistance = targetPose.getMeasureY().minus(curPose.getMeasureY()); + double slope = yDistance.baseUnitMagnitude() / xDistance.baseUnitMagnitude(); // Calculate the slope of the hypotenuse + + 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); // y always equals x times slope + + 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) + ); + + 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) + ); + + // 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(xLimited.in(Meters)); + log_shimmyYMvmt.accept(yLimited.in(Meters)); + log_shimmySlope.accept(slope); - 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()); + log_targetPosePos.accept(positive); + log_targetPoseNeg.accept(negative); - setControl(swreq_drive.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(thetaSpeed)); - } + return new SwerveShimmyData(positive, negative); + } + + public Command swerveRotationShimmy(Supplier targetSup) { + return Commands.repeatingSequence( + Commands.defer(() -> { + 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 */ - public Command swerveToObject() { + public Command swerveToObject() { PhotonTrackedTarget target = detection.getClosestObject(); Pose2d destination = detection.targetToPose(getState().Pose, target); detection.addFuel(destination); - - return toPose(destination); + + return roboToPose(destination); } public static Pose2d faceFuelPose(Pose2d robotPose, Pose2d fuelLocation) { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 2114c062..0ae63f25 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -299,7 +299,7 @@ public boolean isShooterSpunUp() { log_spunUp.accept(isNear); return isNear; } - + // ---TURRET (Motionmagic Angle Control) public Command setTurretPosCmd(Angle rots) { return runOnce(() -> setTurretPos(rots)); @@ -329,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;