added shot point logging into advantage scope

Team334 · Apr 2, 2024 · 1e9eef9 · 1e9eef9
1 parent 86ff8a6
commit 1e9eef9
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -93,6 +93,9 @@ public class SwerveDriveSubsystem extends SubsystemBase {
   StructPublisher<Pose2d> posePublisher = NetworkTableInstance.getDefault()
     .getStructTopic("/Advantage Robot Pose", Pose2d.struct).publish();
 
+  StructPublisher<Pose3d> shotPointPublisher = NetworkTableInstance.getDefault()
+    .getStructTopic("/Advantage Shot Point", Pose3d.struct).publish();
+
   // Pose Estimator -> Has built in odometry and uses supplied vision measurements
   private final SwerveDrivePoseEstimator _estimator = new SwerveDrivePoseEstimator(
     Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
@@ -128,6 +131,8 @@ public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     _headingController.setTolerance(2);
     _headingController.enableContinuousInput(-180, 180);
 
+    resetPose(new Pose2d(5, 5, Rotation2d.fromDegrees(180)));
+
     // setupOrchestra();
 
     // pathplannerlib setup
@@ -181,6 +186,7 @@ public void initSendable(SendableBuilder builder) {
   public void periodic() {
     swervePublisher.set(states);
     posePublisher.set(getPose());
+    shotPointPublisher.set(UtilFuncs.GetSpeakerPose());
 
     SmartDashboard.putBoolean("VALID TAG(S)", updateBotpose());