From ccad393e41aa02b4ac2659349b1fc0bb49ac308a Mon Sep 17 00:00:00 2001 From: MathNerd28 Date: Sun, 26 Mar 2023 17:29:35 -0400 Subject: [PATCH 1/2] Changes from previous PR - `DTSendable` forces subclasses to implement `AutoCloseable` - `DTSendable` convenience methods to limit NetworkTables entry refresh rate - Remove `CURRENTLIMITTYPE` generic parameter from `DTMotor` and change to a single `int` parameter - Add type-specific `configCurrentLimit()` methods to `DTTalonFX` and `DTNeo` - Rename `DTMotor::internal()` to 'getMotorImpl()` - Rename 'DTMotorFaults::internal()` to 'getFaultsImpl()` - Add `DTMotor::getMaxVelocity()` and `getStallTorque()` methods to interface - Bugfixes --- .../frc/dtlib/actuator/motor/DTMotor.java | 59 +++++++++---------- .../dtlib/actuator/motor/DTMotorFaults.java | 2 +- .../dtlib/actuator/motor/ctre/DTTalonFX.java | 31 ++++++++-- .../actuator/motor/revrobotics/DTNeo.java | 40 ++++++------- .../frc/dtlib/network/DTSendable.java | 37 +++++++++++- 5 files changed, 108 insertions(+), 61 deletions(-) diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java index ba70613..98b3b6a 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java @@ -1,14 +1,10 @@ package org.victorrobotics.frc.dtlib.actuator.motor; -import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedBooleanSupplier; -import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedDoubleSupplier; -import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedLongSupplier; -import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedSupplier; import org.victorrobotics.frc.dtlib.network.DTSendable; import edu.wpi.first.util.sendable.SendableBuilder; -public interface DTMotor extends DTSendable, AutoCloseable { +public interface DTMotor extends DTSendable { @Override void close(); @@ -17,52 +13,47 @@ default void initSendable(SendableBuilder builder) { builder.setActuator(true); builder.setSafeState(this::neutralOutput); - builder.addIntegerProperty("CAN ID", - new DTLimitedLongSupplier(this::getCanID, UPDATE_RATE_SLOW_HZ), null); + builder.addIntegerProperty("CAN ID", limitRate(this::getCanID, UPDATE_RATE_SLOW_HZ), null); - builder.addDoubleProperty("Output", this::getMotorOutputPercent, this::setPercentOutput); - builder.addDoubleProperty("Position", this::getEncoderPosition, this::setPosition); - builder.addDoubleProperty("Velocity", this::getEncoderVelocity, this::setVelocity); + builder.addDoubleProperty("Output", + limitRate(this::getMotorOutputPercent, UPDATE_RATE_FAST_HZ), + this::setPercentOutput); + builder.addDoubleProperty("Position", + limitRate(this::getEncoderPosition, UPDATE_RATE_FAST_HZ), this::setPosition); + builder.addDoubleProperty("Velocity", + limitRate(this::getEncoderVelocity, UPDATE_RATE_FAST_HZ), this::setVelocity); builder.addBooleanProperty("Brake mode", - new DTLimitedBooleanSupplier(this::isBrakeEnabled, UPDATE_RATE_SLOW_HZ), - this::configBrakeMode); + limitRate(this::isBrakeEnabled, UPDATE_RATE_SLOW_HZ), this::configBrakeMode); builder.addBooleanProperty("Inverted", - new DTLimitedBooleanSupplier(this::isOutputInverted, UPDATE_RATE_SLOW_HZ), - this::configOutputInverted); + limitRate(this::isOutputInverted, UPDATE_RATE_SLOW_HZ), this::configOutputInverted); - builder.addDoubleProperty("kP", - new DTLimitedDoubleSupplier(this::getPIDproportional, UPDATE_RATE_SLOW_HZ), + builder.addDoubleProperty("kP", limitRate(this::getPIDproportional, UPDATE_RATE_SLOW_HZ), this::configPIDproportional); - builder.addDoubleProperty("kI", - new DTLimitedDoubleSupplier(this::getPIDintegral, UPDATE_RATE_SLOW_HZ), + builder.addDoubleProperty("kI", limitRate(this::getPIDintegral, UPDATE_RATE_SLOW_HZ), this::configPIDintegral); - builder.addDoubleProperty("kD", - new DTLimitedDoubleSupplier(this::getPIDderivative, UPDATE_RATE_SLOW_HZ), + builder.addDoubleProperty("kD", limitRate(this::getPIDderivative, UPDATE_RATE_SLOW_HZ), this::configPIDderivative); - builder.addDoubleProperty("kF", - new DTLimitedDoubleSupplier(this::getPIDfeedforward, UPDATE_RATE_SLOW_HZ), + builder.addDoubleProperty("kF", limitRate(this::getPIDfeedforward, UPDATE_RATE_SLOW_HZ), this::configPIDfeedforward); - builder.addDoubleProperty("kIZ", - new DTLimitedDoubleSupplier(this::getPIDintegralZone, UPDATE_RATE_SLOW_HZ), + builder.addDoubleProperty("kIZ", limitRate(this::getPIDintegralZone, UPDATE_RATE_SLOW_HZ), this::configPIDintegralZone); - builder.addDoubleProperty("Voltage", - new DTLimitedDoubleSupplier(this::getInputVoltage, UPDATE_RATE_STD_HZ), null); + builder.addDoubleProperty("Voltage", limitRate(this::getInputVoltage, UPDATE_RATE_STD_HZ), + null); builder.addDoubleProperty("Temperature", - new DTLimitedDoubleSupplier(this::getTemperature, UPDATE_RATE_SLOW_HZ), null); + limitRate(this::getTemperature, UPDATE_RATE_SLOW_HZ), null); builder.addBooleanProperty("Fault", - new DTLimitedBooleanSupplier(() -> getFaults().hasAnyFault(), UPDATE_RATE_STD_HZ), - null); + limitRate(() -> getFaults().hasAnyFault(), UPDATE_RATE_STD_HZ), null); builder.addStringProperty("Firmware", - new DTLimitedSupplier<>(this::getFirmwareVersion, UPDATE_RATE_SLOW_HZ), null); + limitRate(this::getFirmwareVersion, UPDATE_RATE_SLOW_HZ), null); customizeSendable(builder); } default void customizeSendable(SendableBuilder builder) {} - MOTORTYPE internal(); + MOTORTYPE getMotorImpl(); int getCanID(); @@ -93,7 +84,7 @@ default void configPIDFIZ(double proportional, double integral, double derivativ configPIDintegralZone(integralZone); } - void configCurrentLimit(CURRENTLIMITTYPE limit); + void configCurrentLimit(int maxSupplyCurrent); boolean isOutputInverted(); @@ -132,4 +123,8 @@ default void configPIDFIZ(double proportional, double integral, double derivativ DTMotorFaults getFaults(); String getFirmwareVersion(); + + double getMaxVelocity(); + + double getStallTorque(); } diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java index e7c2a2a..aa2fb80 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java @@ -1,7 +1,7 @@ package org.victorrobotics.frc.dtlib.actuator.motor; public interface DTMotorFaults { - FAULT_TYPE internal(); + FAULT_TYPE getFaultsImpl(); boolean hasAnyFault(); diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java index 3362b77..6a4cd6a 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java @@ -4,6 +4,7 @@ import org.victorrobotics.frc.dtlib.actuator.motor.DTMotorFaults; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; import com.ctre.phoenix.motorcontrol.Faults; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -12,9 +13,11 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -public class DTTalonFX implements DTMotor { +public class DTTalonFX implements DTMotor { private static final double TICKS_PER_REV = 2048; private static final double SECONDS_PER_MINUTE = 60; + private static final double MAX_VELOCITY_RPM = 6380; + private static final double STALL_TORQUE = 4.69; private final WPI_TalonFX internal; private final int deviceID; @@ -35,11 +38,12 @@ public DTTalonFX(int canID) { public DTTalonFX(int canID, String canBus) { internal = new WPI_TalonFX(canID, canBus); + SendableRegistry.remove(internal); deviceID = canID; } @Override - public WPI_TalonFX internal() { + public WPI_TalonFX getMotorImpl() { return internal; } @@ -101,8 +105,13 @@ public void configPIDintegralZone(double iZone) { } @Override - public void configCurrentLimit(SupplyCurrentLimitConfiguration maxCurrent) { - internal.configSupplyCurrentLimit(maxCurrent); + public void configCurrentLimit(int maxSupplyCurrent) { + internal.configSupplyCurrentLimit( + new SupplyCurrentLimitConfiguration(true, maxSupplyCurrent, maxSupplyCurrent, 0)); + } + + public void configCurrentLimit(SupplyCurrentLimitConfiguration currentConfig) { + internal.configSupplyCurrentLimit(currentConfig); } public void configAllSettings(TalonFXConfiguration config) { @@ -197,7 +206,7 @@ public double getEncoderPosition() { @Override public double getEncoderVelocity() { - return internal.getSelectedSensorVelocity() / TICKS_PER_REV * 10; + return internal.getSelectedSensorVelocity() / TICKS_PER_REV * 10 * SECONDS_PER_MINUTE; } public DTTalonFXFaults getFaults() { @@ -245,7 +254,7 @@ public static class DTTalonFXFaults implements DTMotorFaults { } @Override - public Faults internal() { + public Faults getFaultsImpl() { return internal; } @@ -294,4 +303,14 @@ public boolean hardwareFailure() { return internal.HardwareFailure; } } + + @Override + public double getMaxVelocity() { + return MAX_VELOCITY_RPM; + } + + @Override + public double getStallTorque() { + return STALL_TORQUE; + } } diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java index d5b5b16..7bc9e76 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java @@ -4,12 +4,15 @@ import org.victorrobotics.frc.dtlib.actuator.motor.DTMotorFaults; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMax.ControlType; -public class DTNeo implements DTMotor { +public class DTNeo implements DTMotor { + private static final double MAX_VELOCITY_RPM = 5676; + private static final double STALL_TORQUE = 2.6; + private final CANSparkMax internal; private final SparkMaxPIDController pidController; private final RelativeEncoder encoder; @@ -28,7 +31,7 @@ public void close() { } @Override - public CANSparkMax internal() { + public CANSparkMax getMotorImpl() { return internal; } @@ -83,8 +86,8 @@ public void configPIDintegralZone(double iZone) { } @Override - public void configCurrentLimit(DTNeoCurrentLimit limit) { - internal.setSmartCurrentLimit(limit.stallCurrent, limit.freeCurrent); + public void configCurrentLimit(int maxCurrentDraw) { + internal.setSmartCurrentLimit(maxCurrentDraw, maxCurrentDraw); } @Override @@ -182,20 +185,6 @@ public String getFirmwareVersion() { return internal.getFirmwareString(); } - public static class DTNeoCurrentLimit { - public int stallCurrent; - public int freeCurrent; - - public DTNeoCurrentLimit() { - this(0, 0); - } - - public DTNeoCurrentLimit(int stall, int free) { - stallCurrent = stall; - freeCurrent = free; - } - } - public static class DTNeoFaults implements DTMotorFaults { private static final short OTHER_FAULTS_MASK = 0b00001101_11110110; @@ -206,7 +195,7 @@ public static class DTNeoFaults implements DTMotorFaults { } @Override - public Short internal() { + public Short getFaultsImpl() { return Short.valueOf(internal); } @@ -254,6 +243,15 @@ public boolean hasReset() { public boolean hardwareFailure() { return (internal & (1 << CANSparkMax.FaultID.kMotorFault.value)) != 0; } + } + @Override + public double getMaxVelocity() { + return MAX_VELOCITY_RPM; + } + + @Override + public double getStallTorque() { + return STALL_TORQUE; } } diff --git a/src/main/java/org/victorrobotics/frc/dtlib/network/DTSendable.java b/src/main/java/org/victorrobotics/frc/dtlib/network/DTSendable.java index debe652..9503336 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/network/DTSendable.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/network/DTSendable.java @@ -1,13 +1,48 @@ package org.victorrobotics.frc.dtlib.network; +import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedBooleanSupplier; +import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedDoubleSupplier; +import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedFloatSupplier; +import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedLongSupplier; +import org.victorrobotics.frc.dtlib.function.supplier.DTLimitedSupplier; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.LongSupplier; +import java.util.function.Supplier; + +import edu.wpi.first.util.function.FloatSupplier; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; -public interface DTSendable extends Sendable { +public interface DTSendable extends Sendable, AutoCloseable { double UPDATE_RATE_FAST_HZ = 25; double UPDATE_RATE_STD_HZ = 10; double UPDATE_RATE_SLOW_HZ = 2; @Override void initSendable(SendableBuilder builder); + + @Override + void close() throws Exception; + + default DTLimitedDoubleSupplier limitRate(DoubleSupplier supplier, double rateHz) { + return new DTLimitedDoubleSupplier(supplier, rateHz); + } + + default DTLimitedFloatSupplier limitRate(FloatSupplier supplier, double rateHz) { + return new DTLimitedFloatSupplier(supplier, rateHz); + } + + default DTLimitedBooleanSupplier limitRate(BooleanSupplier supplier, double rateHz) { + return new DTLimitedBooleanSupplier(supplier, rateHz); + } + + default DTLimitedLongSupplier limitRate(LongSupplier supplier, double rateHz) { + return new DTLimitedLongSupplier(supplier, rateHz); + } + + default DTLimitedSupplier limitRate(Supplier supplier, double rateHz) { + return new DTLimitedSupplier<>(supplier, rateHz); + } } From 9f1ed54542d363a6ede376ade6226ebb63eb9bce Mon Sep 17 00:00:00 2001 From: MathNerd28 Date: Mon, 27 Mar 2023 20:29:06 -0400 Subject: [PATCH 2/2] Requested PR changes --- .../frc/dtlib/actuator/motor/DTMotor.java | 6 +++--- .../frc/dtlib/actuator/motor/DTMotorFaults.java | 4 +--- .../frc/dtlib/actuator/motor/ctre/DTTalonFX.java | 15 ++++++--------- .../dtlib/actuator/motor/revrobotics/DTNeo.java | 9 ++------- 4 files changed, 12 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java index 98b3b6a..42887fb 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java @@ -20,8 +20,8 @@ default void initSendable(SendableBuilder builder) { this::setPercentOutput); builder.addDoubleProperty("Position", limitRate(this::getEncoderPosition, UPDATE_RATE_FAST_HZ), this::setPosition); - builder.addDoubleProperty("Velocity", - limitRate(this::getEncoderVelocity, UPDATE_RATE_FAST_HZ), this::setVelocity); + builder.addDoubleProperty("Velocity", limitRate(this::getVelocityRPM, UPDATE_RATE_FAST_HZ), + this::setVelocity); builder.addBooleanProperty("Brake mode", limitRate(this::isBrakeEnabled, UPDATE_RATE_SLOW_HZ), this::configBrakeMode); @@ -118,7 +118,7 @@ default void configPIDFIZ(double proportional, double integral, double derivativ double getEncoderPosition(); - double getEncoderVelocity(); + double getVelocityRPM(); DTMotorFaults getFaults(); diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java index aa2fb80..b241a2b 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java @@ -1,8 +1,6 @@ package org.victorrobotics.frc.dtlib.actuator.motor; -public interface DTMotorFaults { - FAULT_TYPE getFaultsImpl(); - +public interface DTMotorFaults { boolean hasAnyFault(); boolean lowVoltage(); diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java index 6a4cd6a..4e42037 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java @@ -110,8 +110,10 @@ public void configCurrentLimit(int maxSupplyCurrent) { new SupplyCurrentLimitConfiguration(true, maxSupplyCurrent, maxSupplyCurrent, 0)); } - public void configCurrentLimit(SupplyCurrentLimitConfiguration currentConfig) { - internal.configSupplyCurrentLimit(currentConfig); + public void configCurrentLimit(double baseCurrentLimit, double peakCurrentLimit, + double peakDuration) { + internal.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, + baseCurrentLimit, peakCurrentLimit, peakDuration)); } public void configAllSettings(TalonFXConfiguration config) { @@ -205,7 +207,7 @@ public double getEncoderPosition() { } @Override - public double getEncoderVelocity() { + public double getVelocityRPM() { return internal.getSelectedSensorVelocity() / TICKS_PER_REV * 10 * SECONDS_PER_MINUTE; } @@ -244,7 +246,7 @@ public double getInputVoltage() { return internal.getBusVoltage(); } - public static class DTTalonFXFaults implements DTMotorFaults { + public static class DTTalonFXFaults implements DTMotorFaults { private static final int OTHER_FAULTS_MASK = 0b00111111_10000000; private final Faults internal; @@ -253,11 +255,6 @@ public static class DTTalonFXFaults implements DTMotorFaults { this.internal = internal; } - @Override - public Faults getFaultsImpl() { - return internal; - } - @Override public boolean hasAnyFault() { return internal.hasAnyFault(); diff --git a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java index 7bc9e76..ffc7b46 100644 --- a/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java +++ b/src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java @@ -171,7 +171,7 @@ public double getEncoderPosition() { } @Override - public double getEncoderVelocity() { + public double getVelocityRPM() { return encoder.getVelocity(); } @@ -185,7 +185,7 @@ public String getFirmwareVersion() { return internal.getFirmwareString(); } - public static class DTNeoFaults implements DTMotorFaults { + public static class DTNeoFaults implements DTMotorFaults { private static final short OTHER_FAULTS_MASK = 0b00001101_11110110; private final short internal; @@ -194,11 +194,6 @@ public static class DTNeoFaults implements DTMotorFaults { this.internal = internal; } - @Override - public Short getFaultsImpl() { - return Short.valueOf(internal); - } - @Override public boolean hasAnyFault() { return internal != 0;