From 26acb0dbddcbd6fd96404302bd2b73a965029776 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 8 Apr 2025 20:18:48 -0700 Subject: [PATCH 01/30] Questnav --- .../vision/MotionTrackerVision.java | 58 +++++++++++ .../vision/PoseEstimateConsumer.java | 20 ++++ .../robot/subsystems/vision/TrackerIO.java | 25 +++++ .../subsystems/vision/TrackerIOQuest.java | 96 +++++++++++++++++++ 4 files changed, 199 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java create mode 100644 src/main/java/frc/robot/subsystems/vision/PoseEstimateConsumer.java create mode 100644 src/main/java/frc/robot/subsystems/vision/TrackerIO.java create mode 100644 src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java new file mode 100644 index 0000000..7dfcb33 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -0,0 +1,58 @@ + +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class MotionTrackerVision extends SubsystemBase { + + private final TrackerIO io_; + private final TrackerInputsAutoLogged inputs_; + + private final Alert disconnectedAlert_ = new Alert("Motion Tracker Disconnected!", AlertType.kError); + + private final PoseEstimateConsumer estimateConsumer_; + + public MotionTrackerVision(TrackerIO io, PoseEstimateConsumer estimateConsumer) { + io_ = io; + inputs_ = new TrackerInputsAutoLogged(); + estimateConsumer_ = estimateConsumer; + } + + @Override + public void periodic() { + io_.updateInputs(inputs_); + Logger.processInputs("MotionTrackerVision", inputs_); + + boolean disconnected = !inputs_.connected; + disconnectedAlert_.set(disconnected); + + if (disconnected) return; + + //TODO: process data + estimateConsumer_.integrate(inputs_.pose.toPose2d(), inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); + } + + public void setPose(Pose2d pose) { + io_.zeroPosition(); // todo: fix this + } + + public void zeroHeading() { + io_.zeroHeading(); + } + + public Pose3d getPose() { + return inputs_.pose; + } + + public boolean isConnected() { + return inputs_.connected; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/PoseEstimateConsumer.java b/src/main/java/frc/robot/subsystems/vision/PoseEstimateConsumer.java new file mode 100644 index 0000000..7accc82 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/PoseEstimateConsumer.java @@ -0,0 +1,20 @@ + +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +@FunctionalInterface +public interface PoseEstimateConsumer { + public void integrate( + Pose2d robotPose, + double timestampSecnds, + Matrix standardDeviations + ); + + public static PoseEstimateConsumer ignore() { + return (Pose2d robotPose, double timestampSecnds, Matrix standardDeviations) -> {}; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java new file mode 100644 index 0000000..51dda35 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; + +public interface TrackerIO { + @AutoLog + public static class TrackerInputs { + public boolean connected = false; + public double timestamp = 0.0; + public double batteryPercent = 0.0; + public long frameCount = 0; + public boolean isTracking = false; + public long trackingLostCount = 0; + + public Pose3d pose = new Pose3d(); + public Quaternion quaternion = new Quaternion(); + } + + public default void updateInputs(TrackerInputs inputs) {} + public default void zeroPosition() {} + public default void zeroHeading() {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java new file mode 100644 index 0000000..7c63323 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -0,0 +1,96 @@ +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.BooleanSubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.FloatArraySubscriber; +import edu.wpi.first.networktables.IntegerPublisher; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; + +public class TrackerIOQuest implements TrackerIO { + + private final NetworkTable nt = NetworkTableInstance.getDefault().getTable("questnav"); + private final IntegerSubscriber miso = nt.getIntegerTopic("miso").subscribe(0); + private final IntegerPublisher mosi = nt.getIntegerTopic("mosi").publish(); + + private final DoubleSubscriber questTimestamp = nt.getDoubleTopic("timestamp").subscribe(0); + private final DoubleSubscriber questBatteryPercent = nt.getDoubleTopic("device/batteryPercent").subscribe(0.0f); + private final IntegerSubscriber questFrameCount = nt.getIntegerTopic("frameCount").subscribe(0); + private final BooleanSubscriber questIsTracking = nt.getBooleanTopic("device/isTracking").subscribe(false); + private final IntegerSubscriber questTrackingLostCount = nt.getIntegerTopic("device/trackingLostCounter").subscribe(0); + private final FloatArraySubscriber questPosition = nt.getFloatArrayTopic("position").subscribe(new float[]{0.0f, 0.0f, 0.0f}); + private final FloatArraySubscriber questQuaternion = nt.getFloatArrayTopic("quaternion").subscribe(new float[]{0.0f, 0.0f, 0.0f, 0.0f}); + + @Override + public void updateInputs(TrackerInputs inputs) { + + // Reset request on success to idle. + if (miso.get() == 99) { + mosi.set(0); + } + + inputs.connected = Timer.getFPGATimestamp() - Microseconds.of(questBatteryPercent.get()).in(Seconds) < 0.25; + inputs.timestamp = questTimestamp.get(); + inputs.batteryPercent = questBatteryPercent.get(); + inputs.frameCount = questFrameCount.get(); + inputs.isTracking = questIsTracking.get(); + inputs.trackingLostCount = questTrackingLostCount.get(); + + inputs.pose = getPose3d(); + inputs.quaternion = getQuaternion(); + } + + @Override + public void zeroHeading() { + if (miso.get() != 99) { + mosi.set(1); + } + } + + @Override + public void zeroPosition() { + // TODO Auto-generated method stub + TrackerIO.super.zeroPosition(); + } + + private Pose3d getPose3d() { + return new Pose3d( + getTranslation3d(), + getRotation3d() + ); + } + + private Rotation3d getRotation3d() { + return new Rotation3d(getQuaternion()); + } + + private Translation3d getTranslation3d() { + float[] values = questPosition.get(); + return new Translation3d( + values[2], + -values[0], // Todo: why is this negative? + values[1] + ); + } + + private Quaternion getQuaternion() { + float[] values = questQuaternion.get(); + + return new Quaternion( + values[0], + values[1], + values[2], + values[3] + ); + } + +} \ No newline at end of file From e1da0b587d331fcb72e6e897976d5fa96bd6d7d7 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 16 Apr 2025 13:17:02 -0700 Subject: [PATCH 02/30] questnav --- src/main/java/frc/robot/RobotContainer.java | 27 +++++-------------- .../vision/MotionTrackerVision.java | 9 ++++++- 2 files changed, 15 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 10ccd96..a1ecea0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,6 +86,8 @@ import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; import frc.robot.subsystems.vision.CameraIOPhotonSim; +import frc.robot.subsystems.vision.MotionTrackerVision; +import frc.robot.subsystems.vision.TrackerIOQuest; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.ReefUtil; import frc.simulator.engine.ISimulatedSubsystem; @@ -117,6 +119,7 @@ public static RobotContainer getInstance() { // Subsystems private Drive drivebase_; private AprilTagVision vision_; + private MotionTrackerVision questnav_; private OISubsystem oi_; private ManipulatorSubsystem manipulator_; private GrabberSubsystem grabber_; @@ -143,6 +146,7 @@ private RobotContainer() { ReefUtil.initialize(); + questnav_ = new MotionTrackerVision(MotionTrackerVision.getIO(), drivebase_::addVisionMeasurement); /** * Subsystem setup @@ -543,28 +547,11 @@ private void configureDriveBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - Command ret = null; - if (Robot.useXeroSimulator()) { - // - // In the Xero simulator, set the auto mode you want to run - // Note: the auto used here must match the simulation stimulus file set in the - // Robot.java file. - // - - // ret = AutoCommands.oneCoralAuto(brain_, drivebase_, manipulator_, grabber_) ; - ret = AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true) ; - // ret = AutoCommands.oneCoralOneAlgaeAuto(brain_, drivebase_, manipulator_, grabber_) ; - // ret = AutoCommands.twoCoralCenterAuto(brain_, drivebase_, manipulator_, grabber_, funnel_, true); - - // Command autoChosen = autoChooser_.get(); - // ret = autoChosen != null ? autoChosen : tuningChooser_.get(); - - } else { - Command autoChosen = autoChooser_.get(); - ret = autoChosen != null ? autoChosen : tuningChooser_.get(); + return AutoCommands.threeCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true); } - return ret; + Command autoChosen = autoChooser_.get(); + return autoChosen != null ? autoChosen : tuningChooser_.get(); } } diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 7dfcb33..81c3c1b 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class MotionTrackerVision extends SubsystemBase { @@ -35,7 +36,6 @@ public void periodic() { if (disconnected) return; - //TODO: process data estimateConsumer_.integrate(inputs_.pose.toPose2d(), inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); } @@ -55,4 +55,11 @@ public boolean isConnected() { return inputs_.connected; } + public static TrackerIO getIO() { + return switch(Constants.getRobot()) { + case COMPETITION -> new TrackerIOQuest(); + default -> new TrackerIO() {}; + }; + } + } \ No newline at end of file From 4c88cfde05d5b26e7fa568d911ec077c7ec99f33 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 6 May 2025 20:48:37 -0700 Subject: [PATCH 03/30] Quest --- src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 22 +- .../subsystems/vision/AprilTagVision.java | 2 +- .../vision/MotionTrackerVision.java | 11 +- .../frc/robot/subsystems/vision/QuestNav.java | 247 ++++++++++++++++++ .../robot/subsystems/vision/TrackerIO.java | 7 +- .../subsystems/vision/TrackerIOQuest.java | 96 ++----- 7 files changed, 285 insertions(+), 103 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/vision/QuestNav.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e6632b1..aff8176 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,7 @@ import frc.robot.commands.misc.StateCmd; import frc.robot.generated.CompTunerConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.vision.MotionTrackerVision; import frc.simulator.engine.SimulationEngine; /** @@ -219,11 +220,13 @@ public void disabledPeriodic() { AutoModeBaseCmd autoCmd = (AutoModeBaseCmd) cmd; if (autoCmd != null) { Drive d = RobotContainer.getInstance().drivebase() ; + MotionTrackerVision quest = RobotContainer.getInstance().quest(); Pose2d autopose = autoCmd.getStartingPose() ; if (auto_cmd_ == null || auto_cmd_ != autoCmd) { Logger.recordOutput("automode/name", autoCmd.getName()) ; Logger.recordOutput("automode/pose", autopose) ; d.setPose(autopose) ; + quest.setPose(autopose); auto_cmd_ = autoCmd ; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a1ecea0..aacc90d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -85,8 +85,8 @@ import frc.robot.subsystems.vision.AprilTagVision.PoseEstimateConsumer; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; -import frc.robot.subsystems.vision.CameraIOPhotonSim; import frc.robot.subsystems.vision.MotionTrackerVision; +import frc.robot.subsystems.vision.TrackerIO; import frc.robot.subsystems.vision.TrackerIOQuest; import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.ReefUtil; @@ -146,8 +146,6 @@ private RobotContainer() { ReefUtil.initialize(); - questnav_ = new MotionTrackerVision(MotionTrackerVision.getIO(), drivebase_::addVisionMeasurement); - /** * Subsystem setup */ @@ -168,6 +166,8 @@ private RobotContainer() { new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); + questnav_ = new MotionTrackerVision(new TrackerIOQuest(), PoseEstimateConsumer.ignore()); + try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); } catch (Exception ex) { @@ -251,14 +251,6 @@ private RobotContainer() { CompTunerConstants.BackRight, CompTunerConstants.kSpeedAt12Volts); - vision_ = new AprilTagVision( - PoseEstimateConsumer.ignore(), - new CameraIOPhotonSim("Front", VisionConstants.frontTransform, - drivebase_::getPose, true), - new CameraIOPhotonSim("Back", VisionConstants.backTransform, - drivebase_::getPose, true) - ); - try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); } catch (Exception ex) { @@ -333,6 +325,10 @@ private RobotContainer() { cams); } + if (questnav_ == null) { + questnav_ = new MotionTrackerVision(new TrackerIO() {}, PoseEstimateConsumer.ignore()); + } + if (manipulator_ == null) { manipulator_ = new ManipulatorSubsystem(new ManipulatorIO() { }); @@ -389,6 +385,10 @@ public Drive drivebase() { return drivebase_; } + public MotionTrackerVision quest() { + return questnav_; + } + public XeroGamepad gamepad() { return gamepad_; } diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 0d00da2..44cff3b 100644 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -177,7 +177,7 @@ public void integrate( Matrix standardDeviations ); - public static PoseEstimateConsumer ignore() { + public static frc.robot.subsystems.vision.PoseEstimateConsumer ignore() { return (Pose2d robotPose, double timestampSecnds, Matrix standardDeviations) -> {}; } } diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 81c3c1b..9401399 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -36,18 +35,14 @@ public void periodic() { if (disconnected) return; - estimateConsumer_.integrate(inputs_.pose.toPose2d(), inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); + estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); } public void setPose(Pose2d pose) { - io_.zeroPosition(); // todo: fix this + io_.setPose(pose); } - public void zeroHeading() { - io_.zeroHeading(); - } - - public Pose3d getPose() { + public Pose2d getPose() { return inputs_.pose; } diff --git a/src/main/java/frc/robot/subsystems/vision/QuestNav.java b/src/main/java/frc/robot/subsystems/vision/QuestNav.java new file mode 100644 index 0000000..5d7e842 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/QuestNav.java @@ -0,0 +1,247 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.*; +import edu.wpi.first.wpilibj.Timer; + +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Seconds; + +/** + * The QuestNav class provides an interface to communicate with an Oculus/Meta Quest VR headset + * for robot localization and tracking purposes. It uses NetworkTables to exchange data between + * the robot and the Quest device. + */ +public class QuestNav { + // Static inner classes for status and command codes + public static class Status { + /** Status indicating system is ready for commands */ + public static final int READY = 0; + /** Status indicating heading reset completion */ + public static final int HEADING_RESET_COMPLETE = 99; + /** Status indicating pose reset completion */ + public static final int POSE_RESET_COMPLETE = 98; + /** Status indicating ping response receipt */ + public static final int PING_RESPONSE = 97; + } + + public static class Command { + /** Clear status */ + public static final int CLEAR = 0; + /** Command to reset the heading */ + public static final int RESET_HEADING = 1; + /** Command to reset the pose */ + public static final int RESET_POSE = 2; + /** Command to ping the system */ + public static final int PING = 3; + } + + /** NetworkTable instance used for communication */ + NetworkTableInstance nt4Instance = NetworkTableInstance.getDefault(); + + /** NetworkTable for Quest navigation data */ + NetworkTable nt4Table = nt4Instance.getTable("questnav"); + + /** Subscriber for message input from Quest (MISO - Master In Slave Out) */ + private final IntegerSubscriber questMiso = nt4Table.getIntegerTopic("miso").subscribe(-1); + + /** Publisher for message output to Quest (MOSI - Master Out Slave In) */ + private final IntegerPublisher questMosi = nt4Table.getIntegerTopic("mosi").publish(); + + /** Subscriber for Quest timestamp data */ + private final DoubleSubscriber questTimestamp = nt4Table.getDoubleTopic("timestamp").subscribe(-1.0f); + + /** Subscriber for raw position data from Quest in Unity coordinate system (in getTranslation, Unity's x becomes FRC's -y, and Unity's z becomes FRC's x) */ + private final FloatArraySubscriber questPosition = nt4Table.getFloatArrayTopic("position").subscribe(new float[]{0.0f, 0.0f, 0.0f}); + + /** Subscriber for Quest orientation as quaternion */ + private final FloatArraySubscriber questQuaternion = nt4Table.getFloatArrayTopic("quaternion").subscribe(new float[]{0.0f, 0.0f, 0.0f, 0.0f}); + + /** Subscriber for Quest orientation as Euler angles */ + private final FloatArraySubscriber questEulerAngles = nt4Table.getFloatArrayTopic("eulerAngles").subscribe(new float[]{0.0f, 0.0f, 0.0f}); + + /** Subscriber for Quest frame counter */ + private final IntegerSubscriber questFrameCount = nt4Table.getIntegerTopic("frameCount").subscribe(-1); + + /** Subscriber for Quest battery percentage */ + private final DoubleSubscriber questBatteryPercent = nt4Table.getDoubleTopic("device/batteryPercent").subscribe(-1.0f); + + /** Subscriber for Quest tracking status */ + private final BooleanSubscriber questIsTracking = nt4Table.getBooleanTopic("device/isTracking").subscribe(false); + + /** Subscriber for Quest tracking loss counter */ + private final IntegerSubscriber questTrackingLostCount = nt4Table.getIntegerTopic("device/trackingLostCounter").subscribe(-1); + + /** Publisher for resetting Quest pose */ + private final DoubleArrayPublisher resetPosePub = nt4Table.getDoubleArrayTopic("resetpose").publish(); + + /** Subscriber for heartbeat requests from Quest */ + private final DoubleSubscriber heartbeatRequestSub = nt4Table.getDoubleTopic("heartbeat/quest_to_robot").subscribe(-1.0); + + /** Publisher for heartbeat responses to Quest */ + private final DoublePublisher heartbeatResponsePub = nt4Table.getDoubleTopic("heartbeat/robot_to_quest").publish(); + + /** Last processed heartbeat request ID */ + private double lastProcessedHeartbeatId = 0; + + /** + * Sets the FRC field relative pose of the Quest. This is the QUESTS POSITION, NOT THE ROBOTS! + * Make sure you correctly offset back from the center of your robot first! + * */ + public void setPose(Pose2d pose) { + resetPosePub.set( + new double[] { + pose.getX(), + pose.getY(), + pose.getRotation().getDegrees() + }); + questMosi.set(Command.RESET_POSE); + } + + /** + * Processes heartbeat requests from the Quest headset and responds with the same ID. + * This helps maintain connection status between the robot and the Quest. + *
MUST BE RUN IN PERIODIC METHOD + */ + public void processHeartbeat() { + double requestId = heartbeatRequestSub.get(); + // Only respond to new requests to avoid flooding + if (requestId > 0 && requestId != lastProcessedHeartbeatId) { + heartbeatResponsePub.set(requestId); + lastProcessedHeartbeatId = requestId; + } + } + + /** + * Gets the battery percentage of the Quest headset. + * + * @return The battery percentage as a Double value + */ + public Double getBatteryPercent() { + return questBatteryPercent.get(); + } + + /** + * Gets the current tracking state of the Quest headset. + * + * @return Boolean indicating if the Quest is currently tracking (true) or not (false) + */ + public Boolean getTrackingStatus() { + return questIsTracking.get(); + } + + /** + * Gets the current frame count from the Quest headset. + * + * @return The frame count as a Long value + */ + public Long getFrameCount() { + return questFrameCount.get(); + } + + /** + * Gets the number of tracking lost events since the Quest connected to the robot. + * + * @return The tracking lost counter as a Long value + */ + public Long getTrackingLostCounter() { + return questTrackingLostCount.get(); + } + + /** + * Determines if the Quest headset is currently connected to the robot. + * Connection is determined by checking when the last battery update was received. + * + * @return Boolean indicating if the Quest is connected (true) or not (false) + */ + public Boolean getConnected() { + return Seconds.of(Timer.getTimestamp()) + .minus(Microseconds.of(questTimestamp.getLastChange())) + .lt(Seconds.of(0.25)); + } + + /** + * Gets the orientation of the Quest headset as a Quaternion. + * + * @return The orientation as a Quaternion object + */ + public Quaternion getQuaternion() { + float[] qqFloats = questQuaternion.get(); + return new Quaternion(qqFloats[0], qqFloats[1], qqFloats[2], qqFloats[3]); + } + + /** + * Gets the Quest's timestamp in NetworkTables server time. + * + * @return The timestamp as a double value + */ + public double getTimestamp() { + return questTimestamp.getAtomic().serverTime; + } + + /** + * Cleans up Quest navigation subroutine messages after processing on the headset. + * Resets the MOSI value to zero if MISO is non-zero. + *
MUST BE RUN IN PERIODIC METHOD + */ + public void cleanupResponses() { + if (questMiso.get() != Status.READY) { + switch ((int) questMiso.get()) { + case Status.POSE_RESET_COMPLETE -> { + questMosi.set(Command.CLEAR); + } + case Status.HEADING_RESET_COMPLETE -> { + questMosi.set(Command.CLEAR); + } + case Status.PING_RESPONSE -> { + questMosi.set(Command.CLEAR); + } + } + } + } + + /** + * Converts the raw QuestNav yaw to a Rotation2d object. Applies necessary coordinate system + * transformations. + * + * @return Rotation2d representing the headset's yaw + */ + private Rotation2d getYaw() { + return Rotation2d.fromDegrees(-questEulerAngles.get()[1]); + } + + /** + * Gets the position of the Quest headset as a Translation2d object. + * Converts the Quest coordinate system to the robot coordinate system. + * + * @return The position as a Translation2d object + */ + private Translation2d getTranslation() { + float[] questnavPosition = questPosition.get(); + return new Translation2d(questnavPosition[2], -questnavPosition[0]); + } + + private Translation3d getTranslation3d() { + float[] questnavPosition = questPosition.get(); + return new Translation3d(questnavPosition[2], -questnavPosition[0], questnavPosition[1]); + } + + /** + * Gets the complete pose (position and orientation) of the Quest headset. + * + * @return The pose as a Pose2d object + */ + public Pose2d getPose() { + return new Pose2d(getTranslation(), getYaw()); + } + + public Pose3d getPose3d() { + return new Pose3d(getTranslation3d(), new Rotation3d(getQuaternion())); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java index 51dda35..2ed6b57 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; @@ -15,11 +16,11 @@ public static class TrackerInputs { public boolean isTracking = false; public long trackingLostCount = 0; - public Pose3d pose = new Pose3d(); + public Pose2d pose = new Pose2d(); + public Pose3d pose3d = new Pose3d(); public Quaternion quaternion = new Quaternion(); } public default void updateInputs(TrackerInputs inputs) {} - public default void zeroPosition() {} - public default void zeroHeading() {} + public default void setPose(Pose2d pose) {} } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java index 7c63323..d21f094 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -1,96 +1,32 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.Microseconds; -import static edu.wpi.first.units.Units.Seconds; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.BooleanSubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.FloatArraySubscriber; -import edu.wpi.first.networktables.IntegerPublisher; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.geometry.Pose2d; public class TrackerIOQuest implements TrackerIO { - private final NetworkTable nt = NetworkTableInstance.getDefault().getTable("questnav"); - private final IntegerSubscriber miso = nt.getIntegerTopic("miso").subscribe(0); - private final IntegerPublisher mosi = nt.getIntegerTopic("mosi").publish(); - - private final DoubleSubscriber questTimestamp = nt.getDoubleTopic("timestamp").subscribe(0); - private final DoubleSubscriber questBatteryPercent = nt.getDoubleTopic("device/batteryPercent").subscribe(0.0f); - private final IntegerSubscriber questFrameCount = nt.getIntegerTopic("frameCount").subscribe(0); - private final BooleanSubscriber questIsTracking = nt.getBooleanTopic("device/isTracking").subscribe(false); - private final IntegerSubscriber questTrackingLostCount = nt.getIntegerTopic("device/trackingLostCounter").subscribe(0); - private final FloatArraySubscriber questPosition = nt.getFloatArrayTopic("position").subscribe(new float[]{0.0f, 0.0f, 0.0f}); - private final FloatArraySubscriber questQuaternion = nt.getFloatArrayTopic("quaternion").subscribe(new float[]{0.0f, 0.0f, 0.0f, 0.0f}); + private final QuestNav quest = new QuestNav(); @Override public void updateInputs(TrackerInputs inputs) { - // Reset request on success to idle. - if (miso.get() == 99) { - mosi.set(0); - } + quest.cleanupResponses(); + quest.processHeartbeat(); - inputs.connected = Timer.getFPGATimestamp() - Microseconds.of(questBatteryPercent.get()).in(Seconds) < 0.25; - inputs.timestamp = questTimestamp.get(); - inputs.batteryPercent = questBatteryPercent.get(); - inputs.frameCount = questFrameCount.get(); - inputs.isTracking = questIsTracking.get(); - inputs.trackingLostCount = questTrackingLostCount.get(); - - inputs.pose = getPose3d(); - inputs.quaternion = getQuaternion(); - } + inputs.connected = quest.getConnected(); + inputs.timestamp = quest.getTimestamp(); + inputs.batteryPercent = quest.getBatteryPercent(); + inputs.frameCount = quest.getFrameCount(); + inputs.isTracking = quest.getTrackingStatus(); + inputs.trackingLostCount = quest.getTrackingLostCounter(); - @Override - public void zeroHeading() { - if (miso.get() != 99) { - mosi.set(1); - } + inputs.pose = quest.getPose(); + inputs.pose3d = quest.getPose3d(); + inputs.quaternion = quest.getQuaternion(); } - + @Override - public void zeroPosition() { - // TODO Auto-generated method stub - TrackerIO.super.zeroPosition(); - } - - private Pose3d getPose3d() { - return new Pose3d( - getTranslation3d(), - getRotation3d() - ); - } - - private Rotation3d getRotation3d() { - return new Rotation3d(getQuaternion()); - } - - private Translation3d getTranslation3d() { - float[] values = questPosition.get(); - return new Translation3d( - values[2], - -values[0], // Todo: why is this negative? - values[1] - ); - } - - private Quaternion getQuaternion() { - float[] values = questQuaternion.get(); - - return new Quaternion( - values[0], - values[1], - values[2], - values[3] - ); + public void setPose(Pose2d pose) { + quest.setPose(pose); } } \ No newline at end of file From c715feae91f6a961118e2981fd694e461bf4a9af Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 27 May 2025 19:47:43 -0700 Subject: [PATCH 04/30] Spelling --- src/main/java/frc/robot/subsystems/vision/AprilTagVision.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index 44cff3b..f76955c 100644 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -173,12 +173,12 @@ public void periodic() { public static interface PoseEstimateConsumer { public void integrate( Pose2d robotPose, - double timestampSecnds, + double timestampSeconds, Matrix standardDeviations ); public static frc.robot.subsystems.vision.PoseEstimateConsumer ignore() { - return (Pose2d robotPose, double timestampSecnds, Matrix standardDeviations) -> {}; + return (Pose2d robotPose, double timestampSeconds, Matrix standardDeviations) -> {}; } } From ad96ffd0c5fdad3b2cece27823c31f300fa64687 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 3 Jun 2025 20:00:51 -0700 Subject: [PATCH 05/30] questnav --- .../frc/robot/subsystems/drive/GyroIO.java | 1 - .../robot/subsystems/drive/GyroIONavX.java | 6 +-- .../robot/subsystems/drive/GyroIOPigeon2.java | 10 +---- .../robot/subsystems/drive/GyroIOQuest.java | 40 +++++++++++++++++++ .../subsystems/drive/ModuleIOTalonFX.java | 4 +- 5 files changed, 47 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 06d39db..4ca3039 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -21,7 +21,6 @@ public interface GyroIO { public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); - public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java index 6b38467..2c5dacb 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -13,11 +13,12 @@ package frc.robot.subsystems.drive; +import java.util.Queue; + import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; + import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import java.util.Queue; /** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { @@ -34,7 +35,6 @@ public GyroIONavX() { public void updateInputs(GyroIOInputs inputs) { inputs.connected = navX.isConnected(); inputs.yawPosition = Rotation2d.fromDegrees(-navX.getYaw()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navX.getRawGyroZ()); inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index f0c2a1c..32f8c1c 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -23,9 +23,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { @@ -33,29 +31,25 @@ public class GyroIOPigeon2 implements GyroIO { private final Queue yawPositionQueue; private final Queue yawTimestampQueue; private final StatusSignal yaw; - private final StatusSignal yawVelocity; public GyroIOPigeon2(int Pigeon2Id, CANBus bus) { // Init Pigeon and Statuses pigeon = new Pigeon2(Pigeon2Id, bus); yaw = pigeon.getYaw(); - yawVelocity = pigeon.getAngularVelocityZWorld(); pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); - yawVelocity.setUpdateFrequency(50.0); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); } @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); - inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java new file mode 100644 index 0000000..c9b7916 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.drive; + +import java.util.Queue; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.vision.QuestNav; + +public class GyroIOQuest implements GyroIO { + + private final QuestNav quest_; + private final Queue yawPositionQueue_; + private final Queue yawTimestampQueue_; + + public GyroIOQuest() { + quest_ = new QuestNav(); + yawTimestampQueue_ = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + yawPositionQueue_ = PhoenixOdometryThread.getInstance().registerSignal(() -> getYaw().getRadians()); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = quest_.getConnected(); + inputs.yawPosition = getYaw(); + + inputs.odometryYawPositions = yawPositionQueue_ + .stream() + .map(Rotation2d::fromRadians) + .toArray(Rotation2d[]::new); + + inputs.odometryYawTimestamps = yawTimestampQueue_ + .stream() + .mapToDouble((Double d) -> d) + .toArray(); + } + + private Rotation2d getYaw() { + return quest_.getPose().getRotation(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 7293cc7..9e4421c 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -159,7 +159,7 @@ public ModuleIOTalonFX( // Create drive status signals drivePosition = driveTalon.getPosition(); drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon.getPosition()); + PhoenixOdometryThread.getInstance().registerSignal(drivePosition.clone()); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); @@ -167,7 +167,7 @@ public ModuleIOTalonFX( // Create turn status signals turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnPosition.clone()); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); From 2a981eaaee7c07f78b86d8d78a1fb6819ba6b228 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 3 Jun 2025 20:01:58 -0700 Subject: [PATCH 06/30] questnav --- .../frc/robot/subsystems/vision/MotionTrackerVision.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 9401399..06fa07a 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; public class MotionTrackerVision extends SubsystemBase { @@ -50,11 +49,4 @@ public boolean isConnected() { return inputs_.connected; } - public static TrackerIO getIO() { - return switch(Constants.getRobot()) { - case COMPETITION -> new TrackerIOQuest(); - default -> new TrackerIO() {}; - }; - } - } \ No newline at end of file From f92cc1f67c168501977d8126c6f6116c920b22e8 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 3 Jun 2025 20:10:00 -0700 Subject: [PATCH 07/30] Clear queues --- src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java index c9b7916..2ac128b 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java @@ -31,6 +31,9 @@ public void updateInputs(GyroIOInputs inputs) { .stream() .mapToDouble((Double d) -> d) .toArray(); + + yawPositionQueue_.clear(); + yawTimestampQueue_.clear(); } private Rotation2d getYaw() { From 818d341166fcd4783822d048b95b2854be7cf0c8 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 19 Jun 2025 20:29:48 -0700 Subject: [PATCH 08/30] fix npe --- .../deploy/pathplanner/paths/ThreeCoral1.path | 8 +++--- src/main/java/frc/robot/RobotContainer.java | 28 +++++++++++-------- .../subsystems/vision/AprilTagVision.java | 16 ----------- .../vision/MotionTrackerVision.java | 2 +- 4 files changed, 21 insertions(+), 33 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/ThreeCoral1.path b/src/main/deploy/pathplanner/paths/ThreeCoral1.path index d6d3f08..51e970c 100644 --- a/src/main/deploy/pathplanner/paths/ThreeCoral1.path +++ b/src/main/deploy/pathplanner/paths/ThreeCoral1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.18930027173913, - "y": 1.2 + "x": 7.108709016393442, + "y": 0.4826331967213116 }, "prevControl": null, "nextControl": { - "x": 6.312992527173914, - "y": 1.2679093070652152 + "x": 6.232401271828226, + "y": 0.550542503786527 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bd37a0..6ebb511 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,6 +67,7 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.GyroIOQuest; import frc.robot.subsystems.drive.ModuleIOReplay; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; @@ -87,10 +88,10 @@ import frc.robot.subsystems.oi.OIIOHID; import frc.robot.subsystems.oi.OISubsystem; import frc.robot.subsystems.vision.AprilTagVision; -import frc.robot.subsystems.vision.AprilTagVision.PoseEstimateConsumer; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; import frc.robot.subsystems.vision.MotionTrackerVision; +import frc.robot.subsystems.vision.PoseEstimateConsumer; import frc.robot.subsystems.vision.TrackerIO; import frc.robot.subsystems.vision.TrackerIOQuest; import frc.robot.subsystems.vision.VisionConstants; @@ -163,21 +164,24 @@ private RobotContainer() { if (Constants.getMode() != Mode.REPLAY) { switch (Constants.getRobot()) { case COMPETITION: + drivebase_ = new Drive( - new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus), - ModuleIOTalonFX::new, - CompTunerConstants.FrontLeft, - CompTunerConstants.FrontRight, - CompTunerConstants.BackLeft, - CompTunerConstants.BackRight, - CompTunerConstants.kSpeedAt12Volts); - + new GyroIOQuest(), + ModuleIOTalonFX::new, + CompTunerConstants.FrontLeft, + CompTunerConstants.FrontRight, + CompTunerConstants.BackLeft, + CompTunerConstants.BackRight, + CompTunerConstants.kSpeedAt12Volts + ); + + questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement); + vision_ = new AprilTagVision( - drivebase_::addVisionMeasurement, + PoseEstimateConsumer.ignore(), new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); - questnav_ = new MotionTrackerVision(new TrackerIOQuest(), PoseEstimateConsumer.ignore()); try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); @@ -332,7 +336,7 @@ private RobotContainer() { }); vision_ = new AprilTagVision( - drivebase_::addVisionMeasurement, + PoseEstimateConsumer.ignore(), cams); } diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java index f76955c..8c45aaf 100644 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -8,12 +8,9 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -169,19 +166,6 @@ public void periodic() { } - @FunctionalInterface - public static interface PoseEstimateConsumer { - public void integrate( - Pose2d robotPose, - double timestampSeconds, - Matrix standardDeviations - ); - - public static frc.robot.subsystems.vision.PoseEstimateConsumer ignore() { - return (Pose2d robotPose, double timestampSeconds, Matrix standardDeviations) -> {}; - } - } - /** * Integrates a pose estimation with the PoseEstimator. * @param est diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 06fa07a..7ed3312 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -32,7 +32,7 @@ public void periodic() { boolean disconnected = !inputs_.connected; disconnectedAlert_.set(disconnected); - if (disconnected) return; + if (disconnected || !inputs_.isTracking) return; estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); } From b7ca7f33cc201f943af7066cc15b2909a0933d41 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 1 Jul 2025 20:50:08 -0700 Subject: [PATCH 09/30] Quest offsets and vendordep --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../robot/subsystems/drive/GyroIOQuest.java | 4 +- .../frc/robot/subsystems/vision/QuestNav.java | 247 ------------------ .../robot/subsystems/vision/TrackerIO.java | 4 - .../subsystems/vision/TrackerIOQuest.java | 29 +- vendordeps/questnavlib.json | 21 ++ 6 files changed, 43 insertions(+), 265 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/QuestNav.java create mode 100644 vendordeps/questnavlib.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6ebb511..5ad900c 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,7 +166,7 @@ private RobotContainer() { case COMPETITION: drivebase_ = new Drive( - new GyroIOQuest(), + new GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus), ModuleIOTalonFX::new, CompTunerConstants.FrontLeft, CompTunerConstants.FrontRight, @@ -182,7 +182,6 @@ private RobotContainer() { new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); - try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); } catch (Exception ex) { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java index 2ac128b..abf77ce 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java @@ -3,7 +3,7 @@ import java.util.Queue; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.vision.QuestNav; +import gg.questnav.questnav.QuestNav; public class GyroIOQuest implements GyroIO { @@ -19,7 +19,7 @@ public GyroIOQuest() { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = quest_.getConnected(); + inputs.connected = quest_.isConnected(); inputs.yawPosition = getYaw(); inputs.odometryYawPositions = yawPositionQueue_ diff --git a/src/main/java/frc/robot/subsystems/vision/QuestNav.java b/src/main/java/frc/robot/subsystems/vision/QuestNav.java deleted file mode 100644 index 5d7e842..0000000 --- a/src/main/java/frc/robot/subsystems/vision/QuestNav.java +++ /dev/null @@ -1,247 +0,0 @@ -package frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.networktables.*; -import edu.wpi.first.wpilibj.Timer; - -import static edu.wpi.first.units.Units.Microseconds; -import static edu.wpi.first.units.Units.Seconds; - -/** - * The QuestNav class provides an interface to communicate with an Oculus/Meta Quest VR headset - * for robot localization and tracking purposes. It uses NetworkTables to exchange data between - * the robot and the Quest device. - */ -public class QuestNav { - // Static inner classes for status and command codes - public static class Status { - /** Status indicating system is ready for commands */ - public static final int READY = 0; - /** Status indicating heading reset completion */ - public static final int HEADING_RESET_COMPLETE = 99; - /** Status indicating pose reset completion */ - public static final int POSE_RESET_COMPLETE = 98; - /** Status indicating ping response receipt */ - public static final int PING_RESPONSE = 97; - } - - public static class Command { - /** Clear status */ - public static final int CLEAR = 0; - /** Command to reset the heading */ - public static final int RESET_HEADING = 1; - /** Command to reset the pose */ - public static final int RESET_POSE = 2; - /** Command to ping the system */ - public static final int PING = 3; - } - - /** NetworkTable instance used for communication */ - NetworkTableInstance nt4Instance = NetworkTableInstance.getDefault(); - - /** NetworkTable for Quest navigation data */ - NetworkTable nt4Table = nt4Instance.getTable("questnav"); - - /** Subscriber for message input from Quest (MISO - Master In Slave Out) */ - private final IntegerSubscriber questMiso = nt4Table.getIntegerTopic("miso").subscribe(-1); - - /** Publisher for message output to Quest (MOSI - Master Out Slave In) */ - private final IntegerPublisher questMosi = nt4Table.getIntegerTopic("mosi").publish(); - - /** Subscriber for Quest timestamp data */ - private final DoubleSubscriber questTimestamp = nt4Table.getDoubleTopic("timestamp").subscribe(-1.0f); - - /** Subscriber for raw position data from Quest in Unity coordinate system (in getTranslation, Unity's x becomes FRC's -y, and Unity's z becomes FRC's x) */ - private final FloatArraySubscriber questPosition = nt4Table.getFloatArrayTopic("position").subscribe(new float[]{0.0f, 0.0f, 0.0f}); - - /** Subscriber for Quest orientation as quaternion */ - private final FloatArraySubscriber questQuaternion = nt4Table.getFloatArrayTopic("quaternion").subscribe(new float[]{0.0f, 0.0f, 0.0f, 0.0f}); - - /** Subscriber for Quest orientation as Euler angles */ - private final FloatArraySubscriber questEulerAngles = nt4Table.getFloatArrayTopic("eulerAngles").subscribe(new float[]{0.0f, 0.0f, 0.0f}); - - /** Subscriber for Quest frame counter */ - private final IntegerSubscriber questFrameCount = nt4Table.getIntegerTopic("frameCount").subscribe(-1); - - /** Subscriber for Quest battery percentage */ - private final DoubleSubscriber questBatteryPercent = nt4Table.getDoubleTopic("device/batteryPercent").subscribe(-1.0f); - - /** Subscriber for Quest tracking status */ - private final BooleanSubscriber questIsTracking = nt4Table.getBooleanTopic("device/isTracking").subscribe(false); - - /** Subscriber for Quest tracking loss counter */ - private final IntegerSubscriber questTrackingLostCount = nt4Table.getIntegerTopic("device/trackingLostCounter").subscribe(-1); - - /** Publisher for resetting Quest pose */ - private final DoubleArrayPublisher resetPosePub = nt4Table.getDoubleArrayTopic("resetpose").publish(); - - /** Subscriber for heartbeat requests from Quest */ - private final DoubleSubscriber heartbeatRequestSub = nt4Table.getDoubleTopic("heartbeat/quest_to_robot").subscribe(-1.0); - - /** Publisher for heartbeat responses to Quest */ - private final DoublePublisher heartbeatResponsePub = nt4Table.getDoubleTopic("heartbeat/robot_to_quest").publish(); - - /** Last processed heartbeat request ID */ - private double lastProcessedHeartbeatId = 0; - - /** - * Sets the FRC field relative pose of the Quest. This is the QUESTS POSITION, NOT THE ROBOTS! - * Make sure you correctly offset back from the center of your robot first! - * */ - public void setPose(Pose2d pose) { - resetPosePub.set( - new double[] { - pose.getX(), - pose.getY(), - pose.getRotation().getDegrees() - }); - questMosi.set(Command.RESET_POSE); - } - - /** - * Processes heartbeat requests from the Quest headset and responds with the same ID. - * This helps maintain connection status between the robot and the Quest. - *
MUST BE RUN IN PERIODIC METHOD - */ - public void processHeartbeat() { - double requestId = heartbeatRequestSub.get(); - // Only respond to new requests to avoid flooding - if (requestId > 0 && requestId != lastProcessedHeartbeatId) { - heartbeatResponsePub.set(requestId); - lastProcessedHeartbeatId = requestId; - } - } - - /** - * Gets the battery percentage of the Quest headset. - * - * @return The battery percentage as a Double value - */ - public Double getBatteryPercent() { - return questBatteryPercent.get(); - } - - /** - * Gets the current tracking state of the Quest headset. - * - * @return Boolean indicating if the Quest is currently tracking (true) or not (false) - */ - public Boolean getTrackingStatus() { - return questIsTracking.get(); - } - - /** - * Gets the current frame count from the Quest headset. - * - * @return The frame count as a Long value - */ - public Long getFrameCount() { - return questFrameCount.get(); - } - - /** - * Gets the number of tracking lost events since the Quest connected to the robot. - * - * @return The tracking lost counter as a Long value - */ - public Long getTrackingLostCounter() { - return questTrackingLostCount.get(); - } - - /** - * Determines if the Quest headset is currently connected to the robot. - * Connection is determined by checking when the last battery update was received. - * - * @return Boolean indicating if the Quest is connected (true) or not (false) - */ - public Boolean getConnected() { - return Seconds.of(Timer.getTimestamp()) - .minus(Microseconds.of(questTimestamp.getLastChange())) - .lt(Seconds.of(0.25)); - } - - /** - * Gets the orientation of the Quest headset as a Quaternion. - * - * @return The orientation as a Quaternion object - */ - public Quaternion getQuaternion() { - float[] qqFloats = questQuaternion.get(); - return new Quaternion(qqFloats[0], qqFloats[1], qqFloats[2], qqFloats[3]); - } - - /** - * Gets the Quest's timestamp in NetworkTables server time. - * - * @return The timestamp as a double value - */ - public double getTimestamp() { - return questTimestamp.getAtomic().serverTime; - } - - /** - * Cleans up Quest navigation subroutine messages after processing on the headset. - * Resets the MOSI value to zero if MISO is non-zero. - *
MUST BE RUN IN PERIODIC METHOD - */ - public void cleanupResponses() { - if (questMiso.get() != Status.READY) { - switch ((int) questMiso.get()) { - case Status.POSE_RESET_COMPLETE -> { - questMosi.set(Command.CLEAR); - } - case Status.HEADING_RESET_COMPLETE -> { - questMosi.set(Command.CLEAR); - } - case Status.PING_RESPONSE -> { - questMosi.set(Command.CLEAR); - } - } - } - } - - /** - * Converts the raw QuestNav yaw to a Rotation2d object. Applies necessary coordinate system - * transformations. - * - * @return Rotation2d representing the headset's yaw - */ - private Rotation2d getYaw() { - return Rotation2d.fromDegrees(-questEulerAngles.get()[1]); - } - - /** - * Gets the position of the Quest headset as a Translation2d object. - * Converts the Quest coordinate system to the robot coordinate system. - * - * @return The position as a Translation2d object - */ - private Translation2d getTranslation() { - float[] questnavPosition = questPosition.get(); - return new Translation2d(questnavPosition[2], -questnavPosition[0]); - } - - private Translation3d getTranslation3d() { - float[] questnavPosition = questPosition.get(); - return new Translation3d(questnavPosition[2], -questnavPosition[0], questnavPosition[1]); - } - - /** - * Gets the complete pose (position and orientation) of the Quest headset. - * - * @return The pose as a Pose2d object - */ - public Pose2d getPose() { - return new Pose2d(getTranslation(), getYaw()); - } - - public Pose3d getPose3d() { - return new Pose3d(getTranslation3d(), new Rotation3d(getQuaternion())); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java index 2ed6b57..80d305d 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java @@ -3,8 +3,6 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; public interface TrackerIO { @AutoLog @@ -17,8 +15,6 @@ public static class TrackerInputs { public long trackingLostCount = 0; public Pose2d pose = new Pose2d(); - public Pose3d pose3d = new Pose3d(); - public Quaternion quaternion = new Quaternion(); } public default void updateInputs(TrackerInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java index d21f094..b552284 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -1,32 +1,41 @@ package frc.robot.subsystems.vision; +import static edu.wpi.first.units.Units.Inches; + import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import gg.questnav.questnav.QuestNav; public class TrackerIOQuest implements TrackerIO { + + private static final Transform2d robotToQuest = new Transform2d( + Inches.of(12), + Inches.of(9.25), + Rotation2d.kZero + ); private final QuestNav quest = new QuestNav(); @Override public void updateInputs(TrackerInputs inputs) { - quest.cleanupResponses(); - quest.processHeartbeat(); + quest.commandPeriodic(); - inputs.connected = quest.getConnected(); - inputs.timestamp = quest.getTimestamp(); inputs.batteryPercent = quest.getBatteryPercent(); + inputs.connected = quest.isConnected(); + inputs.isTracking = quest.isTracking(); + inputs.frameCount = quest.getFrameCount(); - inputs.isTracking = quest.getTrackingStatus(); inputs.trackingLostCount = quest.getTrackingLostCounter(); - - inputs.pose = quest.getPose(); - inputs.pose3d = quest.getPose3d(); - inputs.quaternion = quest.getQuaternion(); + + inputs.timestamp = quest.getDataTimestamp(); + inputs.pose = quest.getPose().transformBy(robotToQuest.inverse()); } @Override public void setPose(Pose2d pose) { - quest.setPose(pose); + quest.setPose(pose.transformBy(robotToQuest)); } } \ No newline at end of file diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json new file mode 100644 index 0000000..1a6e07f --- /dev/null +++ b/vendordeps/questnavlib.json @@ -0,0 +1,21 @@ +{ + "fileName": "questnavlib.json", + "name": "questnavlib", + "version": "2025-1.0.0-beta", + "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.questnav.gg/releases", + "https://maven.questnav.gg/snapshots" + ], + "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.0.0-beta/questnavlib-json-2025-1.0.0-beta.json", + "javaDependencies": [ + { + "groupId": "gg.questnav", + "artifactId": "questnavlib-java", + "version": "2025-1.0.0-beta" + } + ], + "cppDependencies": [], + "jniDependencies": [] +} \ No newline at end of file From 53c27d7c915165ec7a022571ab2766dbdcd2131a Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 12 Jul 2025 10:24:29 -0700 Subject: [PATCH 10/30] use no offset and fix imports & update questnav --- src/main/java/frc/robot/RobotContainer.java | 1 - .../robot/subsystems/vision/MotionTrackerVision.java | 11 ++++++++++- .../frc/robot/subsystems/vision/TrackerIOQuest.java | 6 ++++-- vendordeps/questnavlib.json | 6 +++--- 4 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ad900c..cf42ee2 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,6 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; -import frc.robot.subsystems.drive.GyroIOQuest; import frc.robot.subsystems.drive.ModuleIOReplay; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 7ed3312..c7ac017 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -3,14 +3,23 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class MotionTrackerVision extends SubsystemBase { + private static final Matrix stdDevs = VecBuilder.fill( + 0.01, + 0.01, + 0.01 + ); + private final TrackerIO io_; private final TrackerInputsAutoLogged inputs_; @@ -34,7 +43,7 @@ public void periodic() { if (disconnected || !inputs_.isTracking) return; - estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, VecBuilder.fill(0.01, 0.01, 0.01)); + estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, stdDevs); } public void setPose(Pose2d pose) { diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java index b552284..60246fd 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -10,8 +10,10 @@ public class TrackerIOQuest implements TrackerIO { private static final Transform2d robotToQuest = new Transform2d( - Inches.of(12), - Inches.of(9.25), + // Inches.of(12), + // Inches.of(9.25), + Inches.zero(), + Inches.zero(), Rotation2d.kZero ); diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json index 1a6e07f..78ff669 100644 --- a/vendordeps/questnavlib.json +++ b/vendordeps/questnavlib.json @@ -1,19 +1,19 @@ { "fileName": "questnavlib.json", "name": "questnavlib", - "version": "2025-1.0.0-beta", + "version": "2025-1.0.1-beta", "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", "frcYear": "2025", "mavenUrls": [ "https://maven.questnav.gg/releases", "https://maven.questnav.gg/snapshots" ], - "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.0.0-beta/questnavlib-json-2025-1.0.0-beta.json", + "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.0.1-beta/questnavlib-json-2025-1.0.1-beta.json", "javaDependencies": [ { "groupId": "gg.questnav", "artifactId": "questnavlib-java", - "version": "2025-1.0.0-beta" + "version": "2025-1.0.1-beta" } ], "cppDependencies": [], From 502f13fad0b40c040b152c8b85f3b29b39e0a093 Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Sat, 6 Sep 2025 09:55:33 -0700 Subject: [PATCH 11/30] changes from the mainline --- src/main/deploy/pathplanner/paths/ThreeCoral1.path | 10 +++++----- .../java/frc/robot/subsystems/drive/Drive.java | 5 ++++- .../java/frc/robot/subsystems/drive/GyroIO.java | 1 + .../frc/robot/subsystems/drive/GyroIONavX.java | 4 +--- .../frc/robot/subsystems/drive/GyroIOPigeon2.java | 14 +++++++++++--- .../manipulator/ManipulatorConstants.java | 2 +- .../manipulator/ManipulatorIOHardware.java | 5 +++-- .../robot/subsystems/vision/AprilTagVision.java | 1 + 8 files changed, 27 insertions(+), 15 deletions(-) mode change 100644 => 100755 src/main/deploy/pathplanner/paths/ThreeCoral1.path mode change 100644 => 100755 src/main/java/frc/robot/subsystems/drive/Drive.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/drive/GyroIO.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/drive/GyroIONavX.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java mode change 100644 => 100755 src/main/java/frc/robot/subsystems/vision/AprilTagVision.java diff --git a/src/main/deploy/pathplanner/paths/ThreeCoral1.path b/src/main/deploy/pathplanner/paths/ThreeCoral1.path old mode 100644 new mode 100755 index 51e970c..37d891b --- a/src/main/deploy/pathplanner/paths/ThreeCoral1.path +++ b/src/main/deploy/pathplanner/paths/ThreeCoral1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.108709016393442, - "y": 0.4826331967213116 + "x": 7.18930027173913, + "y": 1.2 }, "prevControl": null, "nextControl": { - "x": 6.232401271828226, - "y": 0.550542503786527 + "x": 6.312992527173914, + "y": 1.2679093070652152 }, "isLocked": false, "linkedName": null @@ -51,4 +51,4 @@ "rotation": 180.0 }, "useDefaultConstraints": false -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java old mode 100644 new mode 100755 index bd438f3..ff79643 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -73,6 +73,9 @@ public class Drive extends SubsystemBase { static final double ODOMETRY_FREQUENCY = new CANBus(CompTunerConstants.DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; public final double DRIVE_BASE_RADIUS; + // Gyro degrees-per-rotation correction/trim + static final double GYRO_YAW_DEG_PER_ROT_CORRECTION = -0.97; + // These constants should change for every drivebase private final LinearVelocity SPEED_12_VOLTS; private final RobotConfig PP_CONFIG; @@ -487,4 +490,4 @@ public Translation2d[] getModuleTranslations() { new Translation2d(brConfig_.LocationX, brConfig_.LocationY) }; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java old mode 100644 new mode 100755 index 4ca3039..06d39db --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -21,6 +21,7 @@ public interface GyroIO { public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadPerSec = 0.0; public double[] odometryYawTimestamps = new double[] {}; public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java old mode 100644 new mode 100755 index 2c5dacb..6c203c7 --- a/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIONavX.java @@ -13,12 +13,10 @@ package frc.robot.subsystems.drive; -import java.util.Queue; - import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; - import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Queue; /** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java old mode 100644 new mode 100755 index 32f8c1c..ccb9706 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -19,11 +19,14 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.GyroTrimConfigs; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { @@ -31,24 +34,29 @@ public class GyroIOPigeon2 implements GyroIO { private final Queue yawPositionQueue; private final Queue yawTimestampQueue; private final StatusSignal yaw; + private final StatusSignal yawVelocity; public GyroIOPigeon2(int Pigeon2Id, CANBus bus) { // Init Pigeon and Statuses pigeon = new Pigeon2(Pigeon2Id, bus); yaw = pigeon.getYaw(); - pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); pigeon.optimizeBusUtilization(); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone()); + yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw()); + + // Improve pigeon's yaw accuracy using the Gyroscope Sensitivity Calibration procedure + GyroTrimConfigs trimConfigs = new GyroTrimConfigs(); + trimConfigs.GyroScalarZ = Drive.GYRO_YAW_DEG_PER_ROT_CORRECTION; + pigeon.getConfigurator().apply(trimConfigs); } @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK); + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.odometryYawTimestamps = diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java old mode 100644 new mode 100755 index 3f056a9..dd810b3 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorConstants.java @@ -95,7 +95,7 @@ public class ThruBoreEncoder { public static final double kEncoderMax = 0; public static final double kEncoderMin = 1; public static final double kRobotCalibrationValue = 0; - public static final double kEncoderCalibrationValue = 0.512; + public static final double kEncoderCalibrationValue = 0.0; public static final int kAbsEncoder = 11; } diff --git a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java old mode 100644 new mode 100755 index 99ecb1d..69d2b57 --- a/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java +++ b/src/main/java/frc/robot/subsystems/manipulator/ManipulatorIOHardware.java @@ -227,8 +227,9 @@ private void createArm() throws Exception { ); // ENCODER CONFIGS: - mapper_.calibrate(ManipulatorConstants.Arm.ThruBoreEncoder.kRobotCalibrationValue, - ManipulatorConstants.Arm.ThruBoreEncoder.kEncoderCalibrationValue); + mapper_.calibrate( + ManipulatorConstants.Arm.ThruBoreEncoder.kRobotCalibrationValue, + ManipulatorConstants.Arm.ThruBoreEncoder.kEncoderCalibrationValue); // ARM CONFIGS: Slot0Configs arm_pids = new Slot0Configs(); diff --git a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java old mode 100644 new mode 100755 index 8c45aaf..8a0c237 --- a/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/frc/robot/subsystems/vision/AprilTagVision.java @@ -166,6 +166,7 @@ public void periodic() { } + /** * Integrates a pose estimation with the PoseEstimator. * @param est From 9dff85442c237b35cc0dddfae1e2195b826a9cdc Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 6 Sep 2025 10:10:54 -0700 Subject: [PATCH 12/30] Merge from --- src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index ccb9706..29645f8 100755 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -41,6 +41,10 @@ public GyroIOPigeon2(int Pigeon2Id, CANBus bus) { // Init Pigeon and Statuses pigeon = new Pigeon2(Pigeon2Id, bus); yaw = pigeon.getYaw(); +<<<<<<< HEAD +======= + yawVelocity = pigeon.getAngularVelocityZWorld(); +>>>>>>> a6544cb (Apply gyro trim value to improve accuracy.) pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); From 0e4a70715a93d436c753c598a9840e3ed7b8cefb Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Sat, 21 Jun 2025 09:47:02 -0700 Subject: [PATCH 13/30] created a single path automode --- .../deploy/pathplanner/paths/ThreeCoral1.path | 4 +- .../deploy/pathplanner/paths/onepath.path | 54 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 + .../frc/robot/commands/auto/AutoCommands.java | 14 +++++ 4 files changed, 72 insertions(+), 2 deletions(-) create mode 100755 src/main/deploy/pathplanner/paths/onepath.path diff --git a/src/main/deploy/pathplanner/paths/ThreeCoral1.path b/src/main/deploy/pathplanner/paths/ThreeCoral1.path index 37d891b..5fbf8bd 100755 --- a/src/main/deploy/pathplanner/paths/ThreeCoral1.path +++ b/src/main/deploy/pathplanner/paths/ThreeCoral1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.18930027173913, + "x": 7.189, "y": 1.2 }, "prevControl": null, "nextControl": { - "x": 6.312992527173914, + "x": 6.312692255434784, "y": 1.2679093070652152 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path new file mode 100755 index 0000000..0f3a5f2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/onepath.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.1892, + "y": 1.2 + }, + "prevControl": null, + "nextControl": { + "x": 6.195681180221097, + "y": 1.3276333302327707 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.123625, + "y": 2.5527499999999996 + }, + "prevControl": { + "x": 5.357128780026418, + "y": 2.3065557804007697 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 121.28265368163976 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.0502341435398 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cf42ee2..6bd5bc8 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -431,6 +431,8 @@ public void setupAutos() { autoChooser_.addDefaultOption("Do Nothing", new AutoModeBaseCmd("Do Nothing")) ; + autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, true)) ; + autoChooser_.addOption("Left Side Coral (2 Coral)", AutoCommands.twoCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true)); diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index e6014f8..43c3431 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -62,6 +62,20 @@ private static Command logState(String mode, String state) { return kDebug ? new StateCmd("Autos/" + mode, state) : null; } + public static AutoModeBaseCmd runOnePath(Drive driveSub, boolean mirroredX) { + final String onepath = "onepath" ; + + Optional path = DriveCommands.findPath(onepath, mirroredX) ; + if (!path.isPresent()) { + return new AutoModeBaseCmd("empty") ; + } + + AutoModeBaseCmd seq = new AutoModeBaseCmd("runOnePath", path.get()) ; + seq.addCommands(DriveCommands.followPathCommand(onepath, mirroredX)) ; + + return seq ; + } + public static AutoModeBaseCmd twoCoralSideAuto(BrainSubsystem brainSub, AprilTagVision vision, Drive driveSub, ManipulatorSubsystem manipSub, GrabberSubsystem grabberSub, FunnelSubsystem funnelSub, boolean mirroredX) { final String modename = "twoCoralSideAuto" ; From dcbf2f44e2f3cdd08e45ae22ec54b02a9b5ee4e5 Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Tue, 22 Jul 2025 19:54:20 -0700 Subject: [PATCH 14/30] work on path following[ --- .../paths/Tuning Path Straight.path | 2 +- .../deploy/pathplanner/paths/onepath.path | 54 +++++++++++++++---- src/main/deploy/pathplanner/settings.json | 4 +- .../frc/robot/subsystems/drive/Drive.java | 4 +- .../subsystems/vision/VisionConstants.java | 2 +- 5 files changed, 49 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Tuning Path Straight.path b/src/main/deploy/pathplanner/paths/Tuning Path Straight.path index 4355477..80498b0 100644 --- a/src/main/deploy/pathplanner/paths/Tuning Path Straight.path +++ b/src/main/deploy/pathplanner/paths/Tuning Path Straight.path @@ -20,7 +20,7 @@ "y": 7.546254678853475 }, "prevControl": { - "x": 2.303425944734135, + "x": 2.3034259447341343, "y": 7.543284205435554 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path index 0f3a5f2..f438838 100755 --- a/src/main/deploy/pathplanner/paths/onepath.path +++ b/src/main/deploy/pathplanner/paths/onepath.path @@ -3,25 +3,57 @@ "waypoints": [ { "anchor": { - "x": 7.1892, - "y": 1.2 + "x": 7.180875000000001, + "y": 0.43212499999999976 }, "prevControl": null, "nextControl": { - "x": 6.195681180221097, - "y": 1.3276333302327707 + "x": 6.090595663659997, + "y": 0.43212499999999987 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.123625, - "y": 2.5527499999999996 + "x": 1.8671250000000001, + "y": 1.3729999999999996 }, "prevControl": { - "x": 5.357128780026418, - "y": 2.3065557804007697 + "x": 2.6656392698937386, + "y": 0.4945859374999996 + }, + "nextControl": { + "x": 1.0686107301062513, + "y": 2.2514140624999994 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3942500000000002, + "y": 3.5374999999999996 + }, + "prevControl": { + "x": 1.1322300123983242, + "y": 3.2006171587978445 + }, + "nextControl": { + "x": 1.7013750000000003, + "y": 3.932374999999998 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0029999999999997, + "y": 4.039624999999999 + }, + "prevControl": { + "x": 2.6081250000000002, + "y": 4.0249999999999995 }, "nextControl": null, "isLocked": false, @@ -33,7 +65,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 3.7, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.28265368163976 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 179.0502341435398 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index aa616ad..4ff9e78 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.864, - "robotLength": 0.864, + "robotWidth": 0.71, + "robotLength": 0.71, "holonomicMode": true, "pathFolders": [ "Processor Algae", diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ff79643..3594113 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -173,8 +173,8 @@ public Drive( this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(8.0, 0.0, 0.0), - new PIDConstants(8.0, 0.0, 0.0)), + new PIDConstants(13.0, 0.0, 0.8), // 8 - original 13, 0.5 + new PIDConstants(12.0, 0.0, 0.5)), // 8 - original PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 0247431..937440e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -41,7 +41,7 @@ public class VisionConstants { // Standard Deviation Factors, (we need to talk about how stddev is calculated, dont change until then) public static final double baseLinearStdDev = 0.02; public static final double baseAngularStdDev = 0.06; - public static final double megatag2Factor = 0.5; + public static final double megatag2Factor = 0.1; // LL4 Config public static final boolean useIMU = false; // Whether or not to use the internal IMU From 9d6667b289435f30d0dd929699f391ca06c23248 Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Tue, 22 Jul 2025 20:39:39 -0700 Subject: [PATCH 15/30] updated on odom testing --- .../pathplanner/paths/Copy of onepath.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/onepath.path | 24 ++++----- 2 files changed, 66 insertions(+), 12 deletions(-) create mode 100755 src/main/deploy/pathplanner/paths/Copy of onepath.path diff --git a/src/main/deploy/pathplanner/paths/Copy of onepath.path b/src/main/deploy/pathplanner/paths/Copy of onepath.path new file mode 100755 index 0000000..7e1824c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of onepath.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.22475, + "y": 1.2511249999999996 + }, + "prevControl": null, + "nextControl": { + "x": 6.134470663659996, + "y": 1.2511249999999996 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.754375, + "y": 1.2511249999999996 + }, + "prevControl": { + "x": 3.5295000000000005, + "y": 1.270625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.7, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.1816970355481 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path index f438838..60aab8b 100755 --- a/src/main/deploy/pathplanner/paths/onepath.path +++ b/src/main/deploy/pathplanner/paths/onepath.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.090595663659997, - "y": 0.43212499999999987 + "x": 6.6798953579456235, + "y": 0.4587896886221912 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 1.3729999999999996 }, "prevControl": { - "x": 2.6656392698937386, - "y": 0.4945859374999996 + "x": 2.760057815767773, + "y": 0.6227651978273178 }, "nextControl": { - "x": 1.0686107301062513, - "y": 2.2514140624999994 + "x": 1.1889966132308873, + "y": 1.9427578889605035 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 3.5374999999999996 }, "prevControl": { - "x": 1.1322300123983242, - "y": 3.2006171587978445 + "x": 1.0951081192500565, + "y": 3.084466390452158 }, "nextControl": { - "x": 1.7013750000000003, - "y": 3.932374999999998 + "x": 1.7045037057678933, + "y": 4.007361845647465 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 4.039624999999999 }, "prevControl": { - "x": 2.6081250000000002, - "y": 4.0249999999999995 + "x": 2.1744511636753954, + "y": 3.8967657230834045 }, "nextControl": null, "isLocked": false, From 549055360d14c11ee1752decb96a38b484a286a4 Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Tue, 29 Jul 2025 18:52:37 -0700 Subject: [PATCH 16/30] test work from last saturday --- .../deploy/pathplanner/paths/onepath.path | 52 ++++--------------- src/main/java/frc/robot/RobotContainer.java | 4 +- 2 files changed, 13 insertions(+), 43 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path index 60aab8b..417a789 100755 --- a/src/main/deploy/pathplanner/paths/onepath.path +++ b/src/main/deploy/pathplanner/paths/onepath.path @@ -3,57 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.180875000000001, - "y": 0.43212499999999976 + "x": 13.49, + "y": 4.0249999999999995 }, "prevControl": null, "nextControl": { - "x": 6.6798953579456235, - "y": 0.4587896886221912 + "x": 12.5507556228542, + "y": 4.0249999999999995 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.8671250000000001, - "y": 1.3729999999999996 + "x": 5.97, + "y": 4.0249999999999995 }, "prevControl": { - "x": 2.760057815767773, - "y": 0.6227651978273178 - }, - "nextControl": { - "x": 1.1889966132308873, - "y": 1.9427578889605035 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3942500000000002, - "y": 3.5374999999999996 - }, - "prevControl": { - "x": 1.0951081192500565, - "y": 3.084466390452158 - }, - "nextControl": { - "x": 1.7045037057678933, - "y": 4.007361845647465 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0029999999999997, - "y": 4.039624999999999 - }, - "prevControl": { - "x": 2.1744511636753954, - "y": 3.8967657230834045 + "x": 6.7545113327734665, + "y": 4.0249999999999995 }, "nextControl": null, "isLocked": false, @@ -65,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.7, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -74,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bd5bc8..e464974 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.FeetPerSecond; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -180,6 +181,7 @@ private RobotContainer() { PoseEstimateConsumer.ignore(), new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); + vision_.setTagFilterDistance(Meters.of(1.2)); try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); @@ -431,7 +433,7 @@ public void setupAutos() { autoChooser_.addDefaultOption("Do Nothing", new AutoModeBaseCmd("Do Nothing")) ; - autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, true)) ; + autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, false)) ; autoChooser_.addOption("Left Side Coral (2 Coral)", AutoCommands.twoCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true)); From b077091ebcd18eb959bb019abefa8ed66561e3e1 Mon Sep 17 00:00:00 2001 From: Butch Griffin Date: Tue, 29 Jul 2025 19:55:01 -0700 Subject: [PATCH 17/30] test paths --- src/main/deploy/pathplanner/paths/onepath.path | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/vision/VisionConstants.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path index 417a789..432a556 100755 --- a/src/main/deploy/pathplanner/paths/onepath.path +++ b/src/main/deploy/pathplanner/paths/onepath.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e464974..d08c357 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -181,7 +181,7 @@ private RobotContainer() { PoseEstimateConsumer.ignore(), new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); - vision_.setTagFilterDistance(Meters.of(1.2)); + // vision_.setTagFilterDistance(Meters.of(1.2)); try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 937440e..2e50f0a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -41,7 +41,7 @@ public class VisionConstants { // Standard Deviation Factors, (we need to talk about how stddev is calculated, dont change until then) public static final double baseLinearStdDev = 0.02; public static final double baseAngularStdDev = 0.06; - public static final double megatag2Factor = 0.1; + public static final double megatag2Factor = 0.7; // LL4 Config public static final boolean useIMU = false; // Whether or not to use the internal IMU From d66e1ba4fd1db2aeabd4b5de2fae2bdbb176b0c5 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 2 Aug 2025 11:02:13 -0700 Subject: [PATCH 18/30] Worked on tuning odometry and the curved path to be more accurate --- .../pathplanner/paths/onepath_curved.path | 70 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/auto/AutoCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 4 +- 4 files changed, 74 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/onepath_curved.path diff --git a/src/main/deploy/pathplanner/paths/onepath_curved.path b/src/main/deploy/pathplanner/paths/onepath_curved.path new file mode 100644 index 0000000..7d21a40 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/onepath_curved.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.189, + "y": 0.4234144455797286 + }, + "prevControl": null, + "nextControl": { + "x": 6.192805301908255, + "y": 0.5105701883273868 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1407499999999997, + "y": 1.919000000000002 + }, + "prevControl": { + "x": 1.6574999999999982, + "y": 0.339500000000003 + }, + "nextControl": { + "x": 0.3252024704996366, + "y": 4.41180565620866 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 4.0 + }, + "prevControl": { + "x": 2.211724901825921, + "y": 3.969750000000001 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d08c357..a62deeb 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,7 +433,7 @@ public void setupAutos() { autoChooser_.addDefaultOption("Do Nothing", new AutoModeBaseCmd("Do Nothing")) ; - autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, false)) ; + autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, true)) ; autoChooser_.addOption("Left Side Coral (2 Coral)", AutoCommands.twoCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true)); diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index 43c3431..c3a2553 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -63,7 +63,7 @@ private static Command logState(String mode, String state) { } public static AutoModeBaseCmd runOnePath(Drive driveSub, boolean mirroredX) { - final String onepath = "onepath" ; + final String onepath = "onepath_curved" ; Optional path = DriveCommands.findPath(onepath, mirroredX) ; if (!path.isPresent()) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3594113..350bb25 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -173,8 +173,8 @@ public Drive( this::getChassisSpeeds, this::runVelocity, new PPHolonomicDriveController( - new PIDConstants(13.0, 0.0, 0.8), // 8 - original 13, 0.5 - new PIDConstants(12.0, 0.0, 0.5)), // 8 - original + new PIDConstants(16.0, 0.0, 0.8), // 8 - original 13, 0.5 + new PIDConstants(16.0, 0.0, 0.5)), // 8 - original PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this From 5d58bd3cb7b4bc17e4d3c5b7c49f31dd0c21e3de Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 9 Aug 2025 10:37:47 -0700 Subject: [PATCH 19/30] speed up path and add constraint zones --- .../pathplanner/paths/onepath_curved.path | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/onepath_curved.path b/src/main/deploy/pathplanner/paths/onepath_curved.path index 7d21a40..45763de 100644 --- a/src/main/deploy/pathplanner/paths/onepath_curved.path +++ b/src/main/deploy/pathplanner/paths/onepath_curved.path @@ -45,12 +45,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Slowdown", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.8, + "maxAcceleration": 3.8, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, From df625b5556bac412f2c1f889f45144ba6756afbf Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 26 Aug 2025 19:44:23 -0700 Subject: [PATCH 20/30] Update --- .../deploy/pathplanner/paths/onepath_curved.path | 15 +++++++++++++-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/drive/Drive.java | 10 ++++++++++ 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/onepath_curved.path b/src/main/deploy/pathplanner/paths/onepath_curved.path index 45763de..987df71 100644 --- a/src/main/deploy/pathplanner/paths/onepath_curved.path +++ b/src/main/deploy/pathplanner/paths/onepath_curved.path @@ -60,7 +60,18 @@ } } ], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.5, + "y": 4.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 1.5, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.8, @@ -78,7 +89,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a62deeb..08e54e0 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -192,7 +192,7 @@ private RobotContainer() { try { grabber_ = new GrabberSubsystem(new GrabberIOHardware()); } catch (Exception ex) { - subsystemCreateException(ex); + // subsystemCreateException(ex); } try { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 350bb25..faf108d 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -53,6 +53,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -422,6 +423,15 @@ public ChassisSpeeds getChassisSpeeds() { public ChassisSpeeds getFieldChassisSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds(getChassisSpeeds(), getRotation()); } + + @AutoLogOutput(key = "SwerveChassisSpeeds/TotalVelocity") + public LinearVelocity getVelocity() { + ChassisSpeeds speeds = getChassisSpeeds(); + double x = speeds.vxMetersPerSecond; + double y = speeds.vyMetersPerSecond; + + return MetersPerSecond.of(Math.sqrt( x * x + y * y )); + } /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { From 447013d884d375b3e0a51fa082cfed1af2c3ba97 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 26 Aug 2025 19:44:45 -0700 Subject: [PATCH 21/30] Undo --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 08e54e0..a62deeb 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -192,7 +192,7 @@ private RobotContainer() { try { grabber_ = new GrabberSubsystem(new GrabberIOHardware()); } catch (Exception ex) { - // subsystemCreateException(ex); + subsystemCreateException(ex); } try { From 14ebf5e0620519001e6179929a28408ad173c991 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 26 Aug 2025 20:37:47 -0700 Subject: [PATCH 22/30] Questnav Zeroing --- src/main/java/frc/robot/Robot.java | 27 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 10 ++++--- .../frc/robot/subsystems/drive/Drive.java | 1 - .../vision/MotionTrackerVision.java | 3 ++- .../subsystems/vision/TrackerIOQuest.java | 6 ++--- 5 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93f3d9e..91d036e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -57,7 +57,7 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private boolean hasSetupAutos = false; - private AutoModeBaseCmd auto_cmd_ = null ; + private AutoModeBaseCmd appliedAuto = null ; public Robot() throws RuntimeException { // @@ -217,23 +217,28 @@ public void disabledPeriodic() { hasSetupAutos = true; } + + if (hasSetupAutos) { Command cmd = robotContainer.getAutonomousCommand(); if (cmd != null) { - AutoModeBaseCmd autoCmd = (AutoModeBaseCmd) cmd; - if (autoCmd != null) { - Drive d = RobotContainer.getInstance().drivebase() ; + AutoModeBaseCmd currentAuto = (AutoModeBaseCmd) cmd; + if (currentAuto != null) { + Drive drivebase = RobotContainer.getInstance().drivebase() ; MotionTrackerVision quest = RobotContainer.getInstance().quest(); - Pose2d autopose = autoCmd.getStartingPose() ; - if (auto_cmd_ == null || auto_cmd_ != autoCmd) { - Logger.recordOutput("automode/name", autoCmd.getName()) ; + Pose2d autopose = currentAuto.getStartingPose() ; + if (appliedAuto == null || appliedAuto != currentAuto) { // Changing the Auto Mode + Logger.recordOutput("automode/name", currentAuto.getName()) ; Logger.recordOutput("automode/pose", autopose) ; - d.setPose(autopose) ; - quest.setPose(autopose); - auto_cmd_ = autoCmd ; + Logger.recordOutput("poseinit/setting", "Robot Pose"); + drivebase.setPose(autopose); + appliedAuto = currentAuto; + } else { // Initializing the Quest Repeatedly + quest.setPose(drivebase.getPose()); + Logger.recordOutput("poseinit/setting", "Quest Pose"); } } - } + } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a62deeb..7cc892e 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Feet; import static edu.wpi.first.units.Units.FeetPerSecond; -import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Seconds; @@ -31,6 +30,7 @@ import org.xerosw.util.MessageLogger; import org.xerosw.util.MessageType; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.RobotState; @@ -175,10 +175,10 @@ private RobotContainer() { CompTunerConstants.kSpeedAt12Volts ); - questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement); + questnav_ = new MotionTrackerVision(new TrackerIOQuest(), PoseEstimateConsumer.ignore()); vision_ = new AprilTagVision( - PoseEstimateConsumer.ignore(), + drivebase_::addVisionMeasurement, new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); // vision_.setTagFilterDistance(Meters.of(1.2)); @@ -374,7 +374,7 @@ private RobotContainer() { () -> -gamepad_.getLeftY(), () -> -gamepad_.getLeftX(), () -> -gamepad_.getRightX() - ); + ); // Shuffleboard Tabs ShuffleboardTab autonomousTab = Shuffleboard.getTab("Autonomous"); @@ -467,6 +467,8 @@ public void setupAutos() { odometryTest.addCommands(DriveCommands.initialFollowPathCommand(drivebase_, "Odom Test")); autoChooser_.addOption("Odom Test (aka Kachow)", odometryTest); + + autoChooser_.addOption("Zero Pose Reset", new AutoModeBaseCmd("Zero Pose", Pose2d.kZero)); } private void subsystemCreateException(Exception ex) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index faf108d..bc315f9 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -53,7 +53,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index c7ac017..af71ec9 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -41,7 +42,7 @@ public void periodic() { boolean disconnected = !inputs_.connected; disconnectedAlert_.set(disconnected); - if (disconnected || !inputs_.isTracking) return; + if (disconnected || !inputs_.isTracking || RobotState.isDisabled()) return; estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, stdDevs); } diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java index 60246fd..b552284 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -10,10 +10,8 @@ public class TrackerIOQuest implements TrackerIO { private static final Transform2d robotToQuest = new Transform2d( - // Inches.of(12), - // Inches.of(9.25), - Inches.zero(), - Inches.zero(), + Inches.of(12), + Inches.of(9.25), Rotation2d.kZero ); From 7c70ec28167d9376120f7c625673e680d2e09c69 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 6 Sep 2025 09:54:52 -0700 Subject: [PATCH 23/30] quest updates --- src/main/java/frc/robot/RobotContainer.java | 16 +++---- .../frc/robot/subsystems/drive/Drive.java | 3 +- .../robot/subsystems/drive/GyroIOQuest.java | 43 ------------------- .../vision/MotionTrackerVision.java | 38 +++++++++++++--- .../robot/subsystems/vision/TrackerIO.java | 3 +- .../subsystems/vision/TrackerIOQuest.java | 23 +++------- src/main/java/frc/robot/util/ReefUtil.java | 24 +++++------ vendordeps/questnavlib.json | 6 +-- 8 files changed, 64 insertions(+), 92 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cc892e..fb1fcee 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,11 +183,11 @@ private RobotContainer() { ); // vision_.setTagFilterDistance(Meters.of(1.2)); - try { - manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); - } catch (Exception ex) { - subsystemCreateException(ex); - } + // try { + // manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); + // } catch (Exception ex) { + // subsystemCreateException(ex); + // } try { grabber_ = new GrabberSubsystem(new GrabberIOHardware()); @@ -328,7 +328,7 @@ private RobotContainer() { if (vision_ == null) { int numCams = switch (Constants.getRobot()) { - default -> 3; + default -> 1; }; CameraIO[] cams = new CameraIO[numCams]; @@ -336,12 +336,12 @@ private RobotContainer() { }); vision_ = new AprilTagVision( - PoseEstimateConsumer.ignore(), + drivebase_::addVisionMeasurement, cams); } if (questnav_ == null) { - questnav_ = new MotionTrackerVision(new TrackerIO() {}, PoseEstimateConsumer.ignore()); + questnav_ = new MotionTrackerVision(new TrackerIO() {}, drivebase_::addVisionMeasurement); } if (manipulator_ == null) { diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bc315f9..afc6c2c 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -472,8 +472,7 @@ public void addVisionMeasurement( double timestampSeconds, Matrix visionMeasurementStdDevs) { Logger.recordOutput("Odometry/VisionMeasurement", visionRobotPoseMeters); - poseEstimator.addVisionMeasurement( - visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); + poseEstimator.addVisionMeasurement(visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } /** Returns the maximum linear speed in meters per sec. */ diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java b/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java deleted file mode 100644 index abf77ce..0000000 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOQuest.java +++ /dev/null @@ -1,43 +0,0 @@ -package frc.robot.subsystems.drive; - -import java.util.Queue; - -import edu.wpi.first.math.geometry.Rotation2d; -import gg.questnav.questnav.QuestNav; - -public class GyroIOQuest implements GyroIO { - - private final QuestNav quest_; - private final Queue yawPositionQueue_; - private final Queue yawTimestampQueue_; - - public GyroIOQuest() { - quest_ = new QuestNav(); - yawTimestampQueue_ = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - yawPositionQueue_ = PhoenixOdometryThread.getInstance().registerSignal(() -> getYaw().getRadians()); - } - - @Override - public void updateInputs(GyroIOInputs inputs) { - inputs.connected = quest_.isConnected(); - inputs.yawPosition = getYaw(); - - inputs.odometryYawPositions = yawPositionQueue_ - .stream() - .map(Rotation2d::fromRadians) - .toArray(Rotation2d[]::new); - - inputs.odometryYawTimestamps = yawTimestampQueue_ - .stream() - .mapToDouble((Double d) -> d) - .toArray(); - - yawPositionQueue_.clear(); - yawTimestampQueue_.clear(); - } - - private Rotation2d getYaw() { - return quest_.getPose().getRotation(); - } - -} diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index af71ec9..13bbcff 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -1,17 +1,22 @@ package frc.robot.subsystems.vision; +import static edu.wpi.first.units.Units.Inches; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import gg.questnav.questnav.PoseFrame; public class MotionTrackerVision extends SubsystemBase { @@ -21,11 +26,21 @@ public class MotionTrackerVision extends SubsystemBase { 0.01 ); + private static final Transform2d robotToQuest = new Transform2d( + Inches.of(12), + Inches.of(9.25), + Rotation2d.kZero + ); + + private static final boolean useQueuedFrames = true; + private final TrackerIO io_; private final TrackerInputsAutoLogged inputs_; private final Alert disconnectedAlert_ = new Alert("Motion Tracker Disconnected!", AlertType.kError); + private final Alert lowBatteryAlert_ = new Alert("Quest battery is low!", AlertType.kWarning); + private final PoseEstimateConsumer estimateConsumer_; public MotionTrackerVision(TrackerIO io, PoseEstimateConsumer estimateConsumer) { @@ -41,18 +56,29 @@ public void periodic() { boolean disconnected = !inputs_.connected; disconnectedAlert_.set(disconnected); + + lowBatteryAlert_.set(inputs_.batteryPercent < 20); if (disconnected || !inputs_.isTracking || RobotState.isDisabled()) return; - estimateConsumer_.integrate(inputs_.pose, inputs_.timestamp, stdDevs); + if (useQueuedFrames) { + for (PoseFrame frame : inputs_.unreadFrames) { + Pose2d robotPose = frame.questPose().transformBy(robotToQuest.inverse()); + double timestamp = frame.dataTimestamp(); + estimateConsumer_.integrate(robotPose, timestamp, stdDevs); + } + } else { + PoseFrame frame = inputs_.unreadFrames[inputs_.unreadFrames.length - 1]; + estimateConsumer_.integrate( + frame.questPose().transformBy(robotToQuest.inverse()), + frame.dataTimestamp(), + stdDevs + ); + } } public void setPose(Pose2d pose) { - io_.setPose(pose); - } - - public Pose2d getPose() { - return inputs_.pose; + io_.setPose(pose.transformBy(robotToQuest)); } public boolean isConnected() { diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java index 80d305d..2c7c6b8 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.AutoLog; import edu.wpi.first.math.geometry.Pose2d; +import gg.questnav.questnav.PoseFrame; public interface TrackerIO { @AutoLog @@ -14,7 +15,7 @@ public static class TrackerInputs { public boolean isTracking = false; public long trackingLostCount = 0; - public Pose2d pose = new Pose2d(); + public PoseFrame[] unreadFrames = {}; } public default void updateInputs(TrackerInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java index b552284..48cf166 100644 --- a/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -1,19 +1,9 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.Inches; - import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; import gg.questnav.questnav.QuestNav; public class TrackerIOQuest implements TrackerIO { - - private static final Transform2d robotToQuest = new Transform2d( - Inches.of(12), - Inches.of(9.25), - Rotation2d.kZero - ); private final QuestNav quest = new QuestNav(); @@ -22,20 +12,19 @@ public void updateInputs(TrackerInputs inputs) { quest.commandPeriodic(); - inputs.batteryPercent = quest.getBatteryPercent(); + inputs.batteryPercent = quest.getBatteryPercent().orElse(-1); inputs.connected = quest.isConnected(); inputs.isTracking = quest.isTracking(); - inputs.frameCount = quest.getFrameCount(); - inputs.trackingLostCount = quest.getTrackingLostCounter(); - - inputs.timestamp = quest.getDataTimestamp(); - inputs.pose = quest.getPose().transformBy(robotToQuest.inverse()); + inputs.frameCount = quest.getFrameCount().orElse(-1); + inputs.trackingLostCount = quest.getTrackingLostCounter().orElse(-1); + + inputs.unreadFrames = quest.getAllUnreadPoseFrames(); } @Override public void setPose(Pose2d pose) { - quest.setPose(pose.transformBy(robotToQuest)); + quest.setPose(pose); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/ReefUtil.java b/src/main/java/frc/robot/util/ReefUtil.java index 64fcbab..f19569e 100644 --- a/src/main/java/frc/robot/util/ReefUtil.java +++ b/src/main/java/frc/robot/util/ReefUtil.java @@ -34,18 +34,18 @@ public static void initialize() { public static Optional getTargetedReefFace(Pose2d robotPose) { Optional ret = Optional.empty(); - ReefFaceInfo nearestFace = getNearestReefFace(robotPose); - Pose2d nearestWall = nearestFace.getTagPose(); - - Rotation2d rotationToFace = new Rotation2d( - nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) - ); - - if (rotationToFace.getMeasure().lte(ReefConstants.maximumAngleToFace) && // Angle is within limit - getDistanceFromFace(robotPose, nearestFace).lte(ReefConstants.maximumDistanceToFace)) - { - ret = Optional.of(nearestFace); - } + // ReefFaceInfo nearestFace = getNearestReefFace(robotPose); + // Pose2d nearestWall = nearestFace.getTagPose(); + + // Rotation2d rotationToFace = new Rotation2d( + // nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) + // ); + + // if (rotationToFace.getMeasure().lte(ReefConstants.maximumAngleToFace) && // Angle is within limit + // getDistanceFromFace(robotPose, nearestFace).lte(ReefConstants.maximumDistanceToFace)) + // { + // ret = Optional.of(nearestFace); + // } return ret ; } diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json index 78ff669..cdbae27 100644 --- a/vendordeps/questnavlib.json +++ b/vendordeps/questnavlib.json @@ -1,19 +1,19 @@ { "fileName": "questnavlib.json", "name": "questnavlib", - "version": "2025-1.0.1-beta", + "version": "2025-1.1.1-beta", "uuid": "a706fe68-86e5-4aed-92c5-ce05aca007f0", "frcYear": "2025", "mavenUrls": [ "https://maven.questnav.gg/releases", "https://maven.questnav.gg/snapshots" ], - "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.0.1-beta/questnavlib-json-2025-1.0.1-beta.json", + "jsonUrl": "https://maven.questnav.gg/releases/gg/questnav/questnavlib-json/2025-1.1.1-beta/questnavlib-json-2025-1.1.1-beta.json", "javaDependencies": [ { "groupId": "gg.questnav", "artifactId": "questnavlib-java", - "version": "2025-1.0.1-beta" + "version": "2025-1.1.1-beta" } ], "cppDependencies": [], From fe6c859ee7005b7c494341d189ebbb9dce0bff56 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 6 Sep 2025 09:56:49 -0700 Subject: [PATCH 24/30] Uncomment --- src/main/java/frc/robot/util/ReefUtil.java | 24 +++++++++++----------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/util/ReefUtil.java b/src/main/java/frc/robot/util/ReefUtil.java index f19569e..fdbd7ff 100644 --- a/src/main/java/frc/robot/util/ReefUtil.java +++ b/src/main/java/frc/robot/util/ReefUtil.java @@ -34,18 +34,18 @@ public static void initialize() { public static Optional getTargetedReefFace(Pose2d robotPose) { Optional ret = Optional.empty(); - // ReefFaceInfo nearestFace = getNearestReefFace(robotPose); - // Pose2d nearestWall = nearestFace.getTagPose(); - - // Rotation2d rotationToFace = new Rotation2d( - // nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) - // ); - - // if (rotationToFace.getMeasure().lte(ReefConstants.maximumAngleToFace) && // Angle is within limit - // getDistanceFromFace(robotPose, nearestFace).lte(ReefConstants.maximumDistanceToFace)) - // { - // ret = Optional.of(nearestFace); - // } + ReefFaceInfo nearestFace = getNearestReefFace(robotPose); + Pose2d nearestWall = nearestFace.getTagPose(); + + Rotation2d rotationToFace = new Rotation2d( + nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) + ); + + if (rotationToFace.getMeasure().lte(ReefConstants.maximumAngleToFace) && // Angle is within limit + getDistanceFromFace(robotPose, nearestFace).lte(ReefConstants.maximumDistanceToFace)) + { + ret = Optional.of(nearestFace); + } return ret ; } From 043c9ded319bebe39135ddec28dfb8bf271ba560 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 6 Sep 2025 10:14:30 -0700 Subject: [PATCH 25/30] Fix error --- .../java/frc/robot/subsystems/drive/GyroIOPigeon2.java | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 29645f8..875df0e 100755 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -24,9 +24,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { @@ -34,17 +32,12 @@ public class GyroIOPigeon2 implements GyroIO { private final Queue yawPositionQueue; private final Queue yawTimestampQueue; private final StatusSignal yaw; - private final StatusSignal yawVelocity; public GyroIOPigeon2(int Pigeon2Id, CANBus bus) { // Init Pigeon and Statuses pigeon = new Pigeon2(Pigeon2Id, bus); yaw = pigeon.getYaw(); -<<<<<<< HEAD -======= - yawVelocity = pigeon.getAngularVelocityZWorld(); ->>>>>>> a6544cb (Apply gyro trim value to improve accuracy.) pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); yaw.setUpdateFrequency(Drive.ODOMETRY_FREQUENCY); @@ -60,7 +53,7 @@ >>>>>>> a6544cb (Apply gyro trim value to improve accuracy.) @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = BaseStatusSignal.refreshAll(yaw).equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.odometryYawTimestamps = From 029d1470e13520448e37bb9082ea579a5fdf78b4 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 13 Sep 2025 11:22:00 -0700 Subject: [PATCH 26/30] Final changes before merge --- src/main/java/frc/robot/RobotContainer.java | 18 +++++-------- .../frc/robot/subsystems/drive/Drive.java | 4 +++ .../vision/MotionTrackerVision.java | 26 ++++++++++++++----- 3 files changed, 29 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca370db..71c4a2c 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -90,15 +90,9 @@ import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; -<<<<<<< HEAD import frc.robot.subsystems.vision.MotionTrackerVision; -import frc.robot.subsystems.vision.PoseEstimateConsumer; import frc.robot.subsystems.vision.TrackerIO; import frc.robot.subsystems.vision.TrackerIOQuest; -======= -import frc.robot.subsystems.vision.CameraIOPhotonSim; -import frc.robot.subsystems.vision.PoseEstimateConsumer; ->>>>>>> main import frc.robot.subsystems.vision.VisionConstants; import frc.robot.util.Mechanism3d; import frc.robot.util.ReefUtil; @@ -180,7 +174,7 @@ private RobotContainer() { CompTunerConstants.kSpeedAt12Volts ); - questnav_ = new MotionTrackerVision(new TrackerIOQuest(), PoseEstimateConsumer.ignore()); + questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement); vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, @@ -188,11 +182,11 @@ private RobotContainer() { ); // vision_.setTagFilterDistance(Meters.of(1.2)); - // try { - // manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); - // } catch (Exception ex) { - // subsystemCreateException(ex); - // } + try { + manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); + } catch (Exception ex) { + subsystemCreateException(ex); + } try { grabber_ = new GrabberSubsystem(new GrabberIOHardware()); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index afc6c2c..bbc4437 100755 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -280,6 +280,10 @@ public void periodic() { ChassisSpeeds spd = getChassisSpeeds() ; Logger.recordOutput("drive/velocity", Math.hypot(spd.vxMetersPerSecond, spd.vyMetersPerSecond)) ; + + Pose2d pose = getPose(); + Logger.recordOutput("Odometry/Individual/X", pose.getX()); + Logger.recordOutput("Odometry/Individual/Y", pose.getY()); } /** diff --git a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java index 13bbcff..d860171 100644 --- a/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -1,7 +1,8 @@ package frc.robot.subsystems.vision; -import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; import org.littletonrobotics.junction.Logger; @@ -13,8 +14,8 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.SubsystemBase; import gg.questnav.questnav.PoseFrame; @@ -26,14 +27,20 @@ public class MotionTrackerVision extends SubsystemBase { 0.01 ); + private boolean useQuestNav = false; + private static final boolean calibration = false; + private static final boolean useQueuedFrames = true; + private static final Transform2d robotToQuest = new Transform2d( - Inches.of(12), - Inches.of(9.25), - Rotation2d.kZero + !calibration ? Meters.of(0.199) : Meters.zero(), + !calibration ? Meters.of(0.282) : Meters.zero(), + new Rotation2d(Degrees.of(45)) ); - private static final boolean useQueuedFrames = true; + // x: -0.216 y: -0.261 + // time: 565-593 + // time: 794-831 private final TrackerIO io_; private final TrackerInputsAutoLogged inputs_; @@ -59,7 +66,12 @@ public void periodic() { lowBatteryAlert_.set(inputs_.batteryPercent < 20); - if (disconnected || !inputs_.isTracking || RobotState.isDisabled()) return; + if ( + disconnected || + !inputs_.isTracking || + RobotState.isDisabled() || + !useQuestNav + ) return; if (useQueuedFrames) { for (PoseFrame frame : inputs_.unreadFrames) { From 02333c91357120b58506947e8401b4bf26d0b79e Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 13 Sep 2025 11:38:19 -0700 Subject: [PATCH 27/30] Remove Paths --- .../pathplanner/paths/Copy of onepath.path | 54 ----------- .../deploy/pathplanner/paths/onepath.path | 54 ----------- .../pathplanner/paths/onepath_curved.path | 95 ------------------- 3 files changed, 203 deletions(-) delete mode 100755 src/main/deploy/pathplanner/paths/Copy of onepath.path delete mode 100755 src/main/deploy/pathplanner/paths/onepath.path delete mode 100644 src/main/deploy/pathplanner/paths/onepath_curved.path diff --git a/src/main/deploy/pathplanner/paths/Copy of onepath.path b/src/main/deploy/pathplanner/paths/Copy of onepath.path deleted file mode 100755 index 7e1824c..0000000 --- a/src/main/deploy/pathplanner/paths/Copy of onepath.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.22475, - "y": 1.2511249999999996 - }, - "prevControl": null, - "nextControl": { - "x": 6.134470663659996, - "y": 1.2511249999999996 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.754375, - "y": 1.2511249999999996 - }, - "prevControl": { - "x": 3.5295000000000005, - "y": 1.270625 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.7, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 178.1816970355481 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/onepath.path b/src/main/deploy/pathplanner/paths/onepath.path deleted file mode 100755 index 432a556..0000000 --- a/src/main/deploy/pathplanner/paths/onepath.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 13.49, - "y": 4.0249999999999995 - }, - "prevControl": null, - "nextControl": { - "x": 12.5507556228542, - "y": 4.0249999999999995 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.97, - "y": 4.0249999999999995 - }, - "prevControl": { - "x": 6.7545113327734665, - "y": 4.0249999999999995 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/onepath_curved.path b/src/main/deploy/pathplanner/paths/onepath_curved.path deleted file mode 100644 index 987df71..0000000 --- a/src/main/deploy/pathplanner/paths/onepath_curved.path +++ /dev/null @@ -1,95 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.189, - "y": 0.4234144455797286 - }, - "prevControl": null, - "nextControl": { - "x": 6.192805301908255, - "y": 0.5105701883273868 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.1407499999999997, - "y": 1.919000000000002 - }, - "prevControl": { - "x": 1.6574999999999982, - "y": 0.339500000000003 - }, - "nextControl": { - "x": 0.3252024704996366, - "y": 4.41180565620866 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 4.0 - }, - "prevControl": { - "x": 2.211724901825921, - "y": 3.969750000000001 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Slowdown", - "minWaypointRelativePos": 1.0, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.5, - "y": 4.0 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.4, - "maxWaypointRelativePos": 1.5, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.8, - "maxAcceleration": 3.8, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file From dffd3d700ddf2fe3d9ba12abcee716399c06b880 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Sat, 13 Sep 2025 13:02:43 -0700 Subject: [PATCH 28/30] undo onepath stuff --- src/main/deploy/pathplanner/paths/ThreeCoral1.path | 4 ++-- .../pathplanner/paths/Tuning Path Straight.path | 2 +- .../java/frc/robot/commands/auto/AutoCommands.java | 14 -------------- 3 files changed, 3 insertions(+), 17 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/ThreeCoral1.path b/src/main/deploy/pathplanner/paths/ThreeCoral1.path index 5fbf8bd..37d891b 100755 --- a/src/main/deploy/pathplanner/paths/ThreeCoral1.path +++ b/src/main/deploy/pathplanner/paths/ThreeCoral1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 7.189, + "x": 7.18930027173913, "y": 1.2 }, "prevControl": null, "nextControl": { - "x": 6.312692255434784, + "x": 6.312992527173914, "y": 1.2679093070652152 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Tuning Path Straight.path b/src/main/deploy/pathplanner/paths/Tuning Path Straight.path index 80498b0..4355477 100644 --- a/src/main/deploy/pathplanner/paths/Tuning Path Straight.path +++ b/src/main/deploy/pathplanner/paths/Tuning Path Straight.path @@ -20,7 +20,7 @@ "y": 7.546254678853475 }, "prevControl": { - "x": 2.3034259447341343, + "x": 2.303425944734135, "y": 7.543284205435554 }, "nextControl": null, diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.java b/src/main/java/frc/robot/commands/auto/AutoCommands.java index c3a2553..e6014f8 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.java +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.java @@ -62,20 +62,6 @@ private static Command logState(String mode, String state) { return kDebug ? new StateCmd("Autos/" + mode, state) : null; } - public static AutoModeBaseCmd runOnePath(Drive driveSub, boolean mirroredX) { - final String onepath = "onepath_curved" ; - - Optional path = DriveCommands.findPath(onepath, mirroredX) ; - if (!path.isPresent()) { - return new AutoModeBaseCmd("empty") ; - } - - AutoModeBaseCmd seq = new AutoModeBaseCmd("runOnePath", path.get()) ; - seq.addCommands(DriveCommands.followPathCommand(onepath, mirroredX)) ; - - return seq ; - } - public static AutoModeBaseCmd twoCoralSideAuto(BrainSubsystem brainSub, AprilTagVision vision, Drive driveSub, ManipulatorSubsystem manipSub, GrabberSubsystem grabberSub, FunnelSubsystem funnelSub, boolean mirroredX) { final String modename = "twoCoralSideAuto" ; From bdc02d2c2ebcf8f2b00b7d272295de16f6ff88e7 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 16 Sep 2025 18:35:39 -0700 Subject: [PATCH 29/30] Fix error --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71c4a2c..9529680 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -432,8 +432,6 @@ public void setupAutos() { autoChooser_.addDefaultOption("Do Nothing", new AutoModeBaseCmd("Do Nothing")) ; - autoChooser_.addOption("RunOnePath", AutoCommands.runOnePath(drivebase_, true)) ; - autoChooser_.addOption("Left Side Coral (2 Coral)", AutoCommands.twoCoralSideAuto(brain_, vision_, drivebase_, manipulator_, grabber_, funnel_, true)); From 4dcff872329ceeb460f9d511811ffeb0e4c45d3f Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Tue, 16 Sep 2025 18:54:03 -0700 Subject: [PATCH 30/30] Fix pp config --- src/main/deploy/pathplanner/settings.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 4ff9e78..aa616ad 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.71, - "robotLength": 0.71, + "robotWidth": 0.864, + "robotLength": 0.864, "holonomicMode": true, "pathFolders": [ "Processor Algae",