diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b5e948b..bebe34e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -257,6 +257,16 @@ public class RobotContainer { sensorTab.addDouble("ElevMotor2", elevator::getMotor2) .withWidget(BuiltInWidgets.kGraph); + + sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint) + .withSize(1, 1) + .withPosition(5, 0) + .withWidget(BuiltInWidgets.kTextView); + + sensorTab.addDouble("Elevator error", elevator::getPIDError) + .withSize(1, 1) + .withPosition(5, 1) + .withWidget(BuiltInWidgets.kTextView); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 2158df3..d9aa681 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -228,4 +228,12 @@ public class Elevator extends SubsystemBase { public double getMotor2() { return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); } + + public double getPIDSetpoint() { + return pidController.getSetpoint(); + } + + public double getPIDError() { + return pidController.getError(); + } } \ No newline at end of file