From 6b89e979c6df9c1fc3c920ebe2639766852b6d5c Mon Sep 17 00:00:00 2001
From: Kevin Cooney <kcooney@gmail.com>
Date: Fri, 25 Jul 2025 19:31:37 -0700
Subject: [PATCH] Update MotorSubsystem to optionally publish to NetworkTables

---
 .../lib2813/subsystems/MotorSubsystem.java    | 35 +++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
index 7c9c8ee0..93231b41 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
@@ -1,10 +1,17 @@
 package com.team2813.lib2813.subsystems;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Rotations;
+
 import com.team2813.lib2813.control.ControlMode;
 import com.team2813.lib2813.control.Encoder;
 import com.team2813.lib2813.control.Motor;
 import com.team2813.lib2813.control.PIDMotor;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.AngleUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
@@ -29,6 +36,10 @@ public abstract class MotorSubsystem<T extends Supplier<Angle>> extends Subsyste
   protected final AngleUnit rotationUnit;
   protected final double acceptableError;
   protected final PIDController controller;
+  private final DoublePublisher positionPublisher;
+  private final BooleanPublisher atPositionPublisher;
+  private final DoublePublisher appliedCurrentPublisher;
+  private final DoublePublisher setpointPublisher;
 
   private boolean isEnabled;
 
@@ -40,6 +51,18 @@ protected MotorSubsystem(MotorSubsystemConfiguration builder) {
     encoder = builder.encoder;
     controlMode = builder.controlMode;
     rotationUnit = builder.rotationUnit;
+    if (builder.ntInstance != null) {
+      NetworkTable networkTable = builder.ntInstance.getTable(getName());
+      positionPublisher = networkTable.getDoubleTopic("position").publish();
+      atPositionPublisher = networkTable.getBooleanTopic("at position").publish();
+      appliedCurrentPublisher = networkTable.getDoubleTopic("applied current").publish();
+      setpointPublisher = networkTable.getDoubleTopic("setpoint").publish();
+    } else {
+      positionPublisher = null;
+      atPositionPublisher = null;
+      appliedCurrentPublisher = null;
+      setpointPublisher = null;
+    }
   }
 
   /**
@@ -190,6 +213,12 @@ public void periodic() {
     if (isEnabled) {
       useOutput(controller.calculate(getMeasurement()));
     }
+    if (positionPublisher != null) {
+      positionPublisher.set(getPositionMeasure().in(Rotations));
+      setpointPublisher.set(getSetpoint().in(Rotations));
+      atPositionPublisher.set(atPosition());
+      appliedCurrentPublisher.set(getAppliedCurrent().in(Amps));
+    }
   }
 
   /** A configuration for a MotorSubsystem */
@@ -207,6 +236,7 @@ public static class MotorSubsystemConfiguration {
     private PIDController controller;
     private double acceptableError;
     private double startingPosition;
+    private NetworkTableInstance ntInstance;
 
     /**
      * Creates a new config for MotorSubsystems. The default acceptable error is {@value
@@ -306,5 +336,10 @@ public MotorSubsystemConfiguration rotationUnit(AngleUnit rotationUnit) {
       this.rotationUnit = rotationUnit;
       return this;
     }
+
+    public MotorSubsystemConfiguration publishTo(NetworkTableInstance ntInstance) {
+      this.ntInstance = ntInstance;
+      return this;
+    }
   }
 }