Skip to content

Update MotorSubsystem to optionally publish to NetworkTables #47

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
kcooney merged 1 commit into from
Aug 25, 2025
Merged

Update MotorSubsystem to optionally publish to NetworkTables #47

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;

Expand All @@ -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;
}
}

/**
Expand Down Expand Up @@ -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 */
Expand All @@ -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
Expand Down Expand Up @@ -306,5 +336,10 @@ public MotorSubsystemConfiguration rotationUnit(AngleUnit rotationUnit) {
this.rotationUnit = rotationUnit;
return this;
}

public MotorSubsystemConfiguration publishTo(NetworkTableInstance ntInstance) {
this.ntInstance = ntInstance;
return this;
}
}
}
Loading