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 d6d3f08..37d891b --- a/src/main/deploy/pathplanner/paths/ThreeCoral1.path +++ b/src/main/deploy/pathplanner/paths/ThreeCoral1.path @@ -51,4 +51,4 @@ "rotation": 180.0 }, "useDefaultConstraints": false -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fa7fa8d..91d036e 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; /** @@ -56,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 { // @@ -216,21 +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() ; - Pose2d autopose = autoCmd.getStartingPose() ; - if (auto_cmd_ == null || auto_cmd_ != autoCmd) { - Logger.recordOutput("automode/name", autoCmd.getName()) ; + AutoModeBaseCmd currentAuto = (AutoModeBaseCmd) cmd; + if (currentAuto != null) { + Drive drivebase = RobotContainer.getInstance().drivebase() ; + MotionTrackerVision quest = RobotContainer.getInstance().quest(); + 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) ; - 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 5353d4b..9529680 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,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; @@ -89,8 +90,9 @@ import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOLimelight4; -import frc.robot.subsystems.vision.CameraIOPhotonSim; -import frc.robot.subsystems.vision.PoseEstimateConsumer; +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.Mechanism3d; import frc.robot.util.ReefUtil; @@ -123,6 +125,7 @@ public static RobotContainer getInstance() { // Subsystems private Drive drivebase_; private AprilTagVision vision_; + private MotionTrackerVision questnav_; private OISubsystem oi_; private ManipulatorSubsystem manipulator_; private GrabberSubsystem grabber_; @@ -160,19 +163,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 GyroIOPigeon2(CompTunerConstants.DrivetrainConstants.Pigeon2Id, CompTunerConstants.kCANBus), + ModuleIOTalonFX::new, + CompTunerConstants.FrontLeft, + CompTunerConstants.FrontRight, + CompTunerConstants.BackLeft, + CompTunerConstants.BackRight, + CompTunerConstants.kSpeedAt12Volts + ); + + questnav_ = new MotionTrackerVision(new TrackerIOQuest(), drivebase_::addVisionMeasurement); + vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, new CameraIOLimelight4(VisionConstants.frontLimelightName, drivebase_::getRotation) ); + // vision_.setTagFilterDistance(Meters.of(1.2)); try { manipulator_ = new ManipulatorSubsystem(new ManipulatorIOHardware()); @@ -257,14 +265,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) { @@ -327,7 +327,7 @@ private RobotContainer() { if (vision_ == null) { int numCams = switch (Constants.getRobot()) { - default -> 3; + default -> 1; }; CameraIO[] cams = new CameraIO[numCams]; @@ -339,6 +339,10 @@ private RobotContainer() { cams); } + if (questnav_ == null) { + questnav_ = new MotionTrackerVision(new TrackerIO() {}, drivebase_::addVisionMeasurement); + } + if (manipulator_ == null) { manipulator_ = new ManipulatorSubsystem(new ManipulatorIO() { }); @@ -369,7 +373,7 @@ private RobotContainer() { () -> -gamepad_.getLeftY(), () -> -gamepad_.getLeftX(), () -> -gamepad_.getRightX() - ); + ); // Shuffleboard Tabs ShuffleboardTab autonomousTab = Shuffleboard.getTab("Autonomous"); @@ -416,6 +420,10 @@ public Drive drivebase() { return drivebase_; } + public MotionTrackerVision quest() { + return questnav_; + } + public XeroGamepad gamepad() { return gamepad_; } @@ -456,6 +464,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) { @@ -599,28 +609,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/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bc315f9..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()); } /** @@ -472,8 +476,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/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index ff4696b..875df0e 100755 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.BaseStatusSignal; 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; @@ -52,7 +53,7 @@ public GyroIOPigeon2(int Pigeon2Id, CANBus bus) { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw).isOK() ; + inputs.connected = BaseStatusSignal.refreshAll(yaw).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 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 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..d860171 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/MotionTrackerVision.java @@ -0,0 +1,100 @@ + +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Meters; + +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.Alert.AlertType; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import gg.questnav.questnav.PoseFrame; + +public class MotionTrackerVision extends SubsystemBase { + + private static final Matrix stdDevs = VecBuilder.fill( + 0.01, + 0.01, + 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( + !calibration ? Meters.of(0.199) : Meters.zero(), + !calibration ? Meters.of(0.282) : Meters.zero(), + new Rotation2d(Degrees.of(45)) + ); + + // x: -0.216 y: -0.261 + + // time: 565-593 + // time: 794-831 + 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) { + 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); + + lowBatteryAlert_.set(inputs_.batteryPercent < 20); + + if ( + disconnected || + !inputs_.isTracking || + RobotState.isDisabled() || + !useQuestNav + ) return; + + 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.transformBy(robotToQuest)); + } + + 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 old mode 100755 new mode 100644 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..2c7c6b8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.vision; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; +import gg.questnav.questnav.PoseFrame; + +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 PoseFrame[] unreadFrames = {}; + } + + public default void updateInputs(TrackerInputs inputs) {} + 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 new file mode 100644 index 0000000..48cf166 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/TrackerIOQuest.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.math.geometry.Pose2d; +import gg.questnav.questnav.QuestNav; + +public class TrackerIOQuest implements TrackerIO { + + private final QuestNav quest = new QuestNav(); + + @Override + public void updateInputs(TrackerInputs inputs) { + + quest.commandPeriodic(); + + inputs.batteryPercent = quest.getBatteryPercent().orElse(-1); + inputs.connected = quest.isConnected(); + inputs.isTracking = quest.isTracking(); + + 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); + } + +} \ 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..fdbd7ff 100644 --- a/src/main/java/frc/robot/util/ReefUtil.java +++ b/src/main/java/frc/robot/util/ReefUtil.java @@ -38,7 +38,7 @@ public static Optional getTargetedReefFace(Pose2d robotPose) { Pose2d nearestWall = nearestFace.getTagPose(); Rotation2d rotationToFace = new Rotation2d( - nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) + nearestWall.relativeTo(robotPose).getTranslation().getAngle().getMeasure().abs(Radians) ); if (rotationToFace.getMeasure().lte(ReefConstants.maximumAngleToFace) && // Angle is within limit diff --git a/vendordeps/questnavlib.json b/vendordeps/questnavlib.json new file mode 100644 index 0000000..cdbae27 --- /dev/null +++ b/vendordeps/questnavlib.json @@ -0,0 +1,21 @@ +{ + "fileName": "questnavlib.json", + "name": "questnavlib", + "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.1.1-beta/questnavlib-json-2025-1.1.1-beta.json", + "javaDependencies": [ + { + "groupId": "gg.questnav", + "artifactId": "questnavlib-java", + "version": "2025-1.1.1-beta" + } + ], + "cppDependencies": [], + "jniDependencies": [] +} \ No newline at end of file