From 5466399a569be7d2969121e04a32d90f617d8f9d Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Wed, 3 Apr 2024 17:25:27 -0700 Subject: [PATCH 1/2] put motor stats to NT --- .../frc/robot/subsystems/climb/ClimbArm.java | 21 ++++++++++++++++++ .../elevator/ElevatorSubsystem.java | 12 +++++----- .../intake/IntakePivotSubsystem.java | 20 +++++++++++++++++ .../intake/IntakeRollerSubsystem.java | 22 +++++++++++++++++++ .../shooter/ShooterFlywheelSubsystem.java | 22 +++++++++++++++++++ .../shooter/ShooterPivotSubsystem.java | 20 +++++++++++++++++ 6 files changed, 111 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java index cb802a15..3137d56a 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java @@ -10,6 +10,10 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; + import frc.robot.util.MotorUtil; /** @@ -23,6 +27,12 @@ public class ClimbArm { private double winchPower; private double prevWinchPower; + private NetworkTableInstance climbNTInstance; + private NetworkTable motorsNTTable; + private NetworkTableEntry currentEntry; + private NetworkTableEntry voltageEntry; + private NetworkTableEntry temperatureEntry; + /** * Constructs a new {@link ClimbArm}. * @@ -48,6 +58,13 @@ public ClimbArm(int winchMotorId, int zeroLimitPort, boolean isInverted) { winchPower = 0; prevWinchPower = 0; + + climbNTInstance = NetworkTableInstance.getDefault(); + motorsNTTable = climbNTInstance.getTable("Motors"); + currentEntry = motorsNTTable.getEntry("Climb" + winchMotorId + "Current"); + voltageEntry = motorsNTTable.getEntry("Climb" + winchMotorId + "Voltage"); + temperatureEntry = motorsNTTable.getEntry("Climb" + winchMotorId + "Temperature"); + } /** Run this function every periodic loop.*/ @@ -64,6 +81,10 @@ public void update(double speed) { } prevWinchPower = winchPower; + + currentEntry.setDouble(winchMotor.getOutputCurrent()); + voltageEntry.setDouble(winchMotor.getBusVoltage()); + temperatureEntry.setDouble(winchMotor.getMotorTemperature()); } /** diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 34157ac2..da53dd17 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -89,12 +89,12 @@ public ElevatorSubsystem() { limitSwitchEntry = elevatorNetworkTable.getEntry("LimitSwitch"); extensionPercentEntry = elevatorNetworkTable.getEntry("ExtensionPercent"); targetStateEntry = elevatorNetworkTable.getEntry("TargetState"); - motor10CurrentEntry = motorsNetworkTable.getEntry("10Current"); - motor10VoltageEntry = motorsNetworkTable.getEntry("10Voltage"); - motor10TemperatureEntry = motorsNetworkTable.getEntry("10Temperature"); - motor11CurrentEntry = motorsNetworkTable.getEntry("11Current"); - motor11VoltageEntry = motorsNetworkTable.getEntry("11Voltage"); - motor11TemperatureEntry = motorsNetworkTable.getEntry("11Temperature"); + motor10CurrentEntry = motorsNetworkTable.getEntry("Elevator10Current"); + motor10VoltageEntry = motorsNetworkTable.getEntry("Elevator10Voltage"); + motor10TemperatureEntry = motorsNetworkTable.getEntry("Elevator10Temperature"); + motor11CurrentEntry = motorsNetworkTable.getEntry("Elevator11Current"); + motor11VoltageEntry = motorsNetworkTable.getEntry("Elevator11Voltage"); + motor11TemperatureEntry = motorsNetworkTable.getEntry("Elevator11Temperature"); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java index c8c20286..48e3ffbd 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java @@ -7,6 +7,10 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; + /** The subsystem for the pivot on the intake. */ public class IntakePivotSubsystem extends SubsystemBase { @@ -15,6 +19,12 @@ public class IntakePivotSubsystem extends SubsystemBase { private PositionVoltage request = new PositionVoltage(0).withSlot(0); private double setPos = 0; + + private NetworkTableInstance ntInstance; + private NetworkTable motorsNTTable; + private NetworkTableEntry intake16CurrentEntry; + private NetworkTableEntry intake16VoltageEntry; + private NetworkTableEntry intake16TemperatureEntry; /** * Subsystem for controlling the pivot on the intake. @@ -34,6 +44,12 @@ public IntakePivotSubsystem() { pivotMotor.getConfigurator().apply(slot0Configs); pivotMotor.setPosition(0); + + ntInstance = NetworkTableInstance.getDefault(); + motorsNTTable = ntInstance.getTable("Motors"); + intake16CurrentEntry = motorsNTTable.getEntry("Intake16Current"); + intake16VoltageEntry = motorsNTTable.getEntry("Intake16Voltage"); + intake16TemperatureEntry = motorsNTTable.getEntry("Intake16Temperature"); } /** @@ -73,5 +89,9 @@ public void movePivot(double speed) { @Override public void periodic() { pivotMotor.setControl(request.withPosition(setPos / IntakeConstants.PIVOT_CONVERSION_FACTOR)); + + intake16CurrentEntry.setDouble(pivotMotor.getSupplyCurrent().getValueAsDouble()); + intake16VoltageEntry.setDouble(pivotMotor.getMotorVoltage().getValueAsDouble()); + intake16TemperatureEntry.setDouble(pivotMotor.getDeviceTemp().getValueAsDouble()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java index 36c11adc..4649d7f6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeRollerSubsystem.java @@ -50,6 +50,14 @@ public class IntakeRollerSubsystem extends SubsystemBase { private BooleanPublisher ntFrontPublisher; private BooleanPublisher ntBackPublisher; + private NetworkTable motorsNTTable; + private NetworkTableEntry frontMotorCurrentEntry; + private NetworkTableEntry frontMotorVoltageEntry; + private NetworkTableEntry frontMotorTemperatureEntry; + private NetworkTableEntry integrationMotorCurrentEntry; + private NetworkTableEntry integrationMotorVoltageEntry; + private NetworkTableEntry integrationMotorTemperatureEntry; + private NetworkTable intakeNTTable; private NetworkTableEntry frontSensorEntry; private NetworkTableEntry rockwellSensorEntry; @@ -81,6 +89,13 @@ public IntakeRollerSubsystem(LightBarSubsystem lightBarSubsystem) { ntFrontPublisher = ntTable.getBooleanTopic("FrontSensor").publish(); ntBackPublisher = ntTable.getBooleanTopic("BackSensor").publish(); + motorsNTTable = ntInstance.getTable("Motors"); + frontMotorCurrentEntry = motorsNTTable.getEntry("Intake17Current"); + frontMotorVoltageEntry = motorsNTTable.getEntry("Intake17Voltage"); + frontMotorTemperatureEntry = motorsNTTable.getEntry("Intake17Temperature"); + integrationMotorCurrentEntry = motorsNTTable.getEntry("Intake19Current"); + integrationMotorVoltageEntry = motorsNTTable.getEntry("Intake19Voltage"); + integrationMotorTemperatureEntry = motorsNTTable.getEntry("Intake19Temperature"); this.lightBarSubsystem = lightBarSubsystem; // colorResetTimer = new Timer(); @@ -149,6 +164,13 @@ public void periodic() { rockwellSensorEntry.setBoolean(getRockwellSensorValue()); ampSenSorEntry.setBoolean(getAmpSensor()); + frontMotorCurrentEntry.setDouble(frontMotors.getOutputCurrent()); + frontMotorVoltageEntry.setDouble(frontMotors.getBusVoltage()); + frontMotorTemperatureEntry.setDouble(frontMotors.getMotorTemperature()); + integrationMotorCurrentEntry.setDouble(integrationMotor.getSupplyCurrent()); + integrationMotorVoltageEntry.setDouble(integrationMotor.getMotorOutputVoltage()); + integrationMotorTemperatureEntry.setDouble(integrationMotor.getTemperature()); + ntFrontPublisher.set(getFrontSensorReached()); prevFrontSensorValue = getFrontSensorValue(); if (getFrontSensorValue()) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index 136290e9..a85f49be 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.networktables.BooleanPublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -54,6 +55,13 @@ public class ShooterFlywheelSubsystem extends SubsystemBase { private boolean autoAim = false; private Pose2d targetPose; + private NetworkTable motorsNTTable; + private NetworkTableEntry shooter13CurrentEntry; + private NetworkTableEntry shooter13VoltageEntry; + private NetworkTableEntry shooter13TemperatureEntry; + private NetworkTableEntry shooter14CurrentEntry; + private NetworkTableEntry shooter14VoltageEntry; + private NetworkTableEntry shooter14TemperatureEntry; /** * Runs the flywheels for the shooter. * @@ -105,6 +113,13 @@ public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier red ntInstance = NetworkTableInstance.getDefault(); ntTable = ntInstance.getTable("RobotStatus"); ntPublisher = ntTable.getBooleanTopic("shooterReady").publish(); + motorsNTTable = ntInstance.getTable("Motors"); + shooter13CurrentEntry = motorsNTTable.getEntry("Shooter13Current"); + shooter13VoltageEntry = motorsNTTable.getEntry("Shooter13Voltage"); + shooter13TemperatureEntry = motorsNTTable.getEntry("Shooter13Temperature"); + shooter14CurrentEntry = motorsNTTable.getEntry("Shooter14Current"); + shooter14VoltageEntry = motorsNTTable.getEntry("Shooter14Voltage"); + shooter14TemperatureEntry = motorsNTTable.getEntry("Shooter14Temperature"); } /** Sets shooting motor speed. */ @@ -237,5 +252,12 @@ public void periodic() { if (autoAim) { setShooterMotorSpeed(getTopMotorSplineSpeed(), getBottomMotorSplineSpeed()); } + + shooter13CurrentEntry.setDouble(shooterMotorTop.getSupplyCurrent().getValueAsDouble()); + shooter13VoltageEntry.setDouble(shooterMotorTop.getMotorVoltage().getValueAsDouble()); + shooter13TemperatureEntry.setDouble(shooterMotorTop.getDeviceTemp().getValueAsDouble()); + shooter14CurrentEntry.setDouble(shooterMotorBottom.getSupplyCurrent().getValueAsDouble()); + shooter14VoltageEntry.setDouble(shooterMotorBottom.getMotorVoltage().getValueAsDouble()); + shooter14TemperatureEntry.setDouble(shooterMotorBottom.getDeviceTemp().getValueAsDouble()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index fccf39ac..43d10185 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -7,6 +7,9 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; @@ -22,6 +25,7 @@ import org.apache.commons.math3.analysis.interpolation.AkimaSplineInterpolator; import org.apache.commons.math3.analysis.polynomials.PolynomialSplineFunction; +import org.apache.commons.math3.ml.neuralnet.Network; /** Controls motors and functions for the pivot part of shooter mech. */ public class ShooterPivotSubsystem extends SubsystemBase { @@ -53,6 +57,12 @@ public class ShooterPivotSubsystem extends SubsystemBase { private double angleOffset = 0; + private NetworkTableInstance ntInstance; + private NetworkTable motorsTable; + private NetworkTableEntry shooter12CurrentEntry; + private NetworkTableEntry shooter12VoltageEntry; + private NetworkTableEntry shooter12TemperatureEntry; + /** Inits motors and pose field. Also inits PID stuff. */ public ShooterPivotSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSupplier) { @@ -117,6 +127,12 @@ public ShooterPivotSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier redSup autoAim = false; rotationPIDController.setReference(Units.degreesToRadians(18), ControlType.kPosition); + + ntInstance = NetworkTableInstance.getDefault(); + motorsTable = ntInstance.getTable("Motors"); + shooter12CurrentEntry = motorsTable.getEntry("Shooter12CurrentEntry"); + shooter12VoltageEntry = motorsTable.getEntry("Shooter12Voltage"); + shooter12TemperatureEntry = motorsTable.getEntry("Shooter12Temperature"); } /** motor speed setting functions. */ @@ -177,6 +193,10 @@ public void periodic() { if (autoAim) { setAngle(getAutoAimAngle()); } + + shooter12CurrentEntry.setDouble(pivotMotor.getOutputCurrent()); + shooter12VoltageEntry.setDouble(pivotMotor.getBusVoltage()); + shooter12TemperatureEntry.setDouble(pivotMotor.getMotorTemperature()); } /** Sets the angle offset to a new value. From d8ce9b472d4d90f31746ca29ac4c6bf54d7ffda9 Mon Sep 17 00:00:00 2001 From: Jesse Chu Date: Wed, 3 Apr 2024 17:26:05 -0700 Subject: [PATCH 2/2] Add motor stats entries --- Readme.md | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/Readme.md b/Readme.md index 3aa180ad..2b140255 100644 --- a/Readme.md +++ b/Readme.md @@ -32,10 +32,18 @@ ## Motors | Entry Name | Type | Description | |:----------:|:----:|:-----------:| -| 10Current | Double | | -| 10Voltage | Double | | -| 11Current | Double | | -| 11Voltage | Double | | +| Climb20Current | Double | | +| Climb20Temperature | Double | | +| Climb20Voltage | Double | | +| Climb9Current | Double | | +| Climb9Temperature | Double | | +| Climb9Voltage | Double | | +| Elevator10Current | Double | | +| Elevator10Temperature| Double | | +| Elevator10Voltage | Double | | +| Elevator11Current | Double | | +| Elevator11Temperature| Double | | +| Elevator11Voltage | Double | | ## Elevator | Entry Name | Type | Description | |:----------:|:----:|:-----------:|