Skip to content

Commit

Permalink
Merge pull request #8 from Team1559/dtmotor
Browse files Browse the repository at this point in the history 
Changes from previous PR
  • Loading branch information
@MathNerd28
MathNerd28 authored Mar 28, 2023
2 parents 631a437 + 9f1ed54 commit 99ec95e
Show file tree
Hide file tree
Showing 5 changed files with 113 additions and 76 deletions.
61 changes: 28 additions & 33 deletions src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotor.java
View file
Original file line number Diff line number Diff line change
@@ -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<MOTORTYPE, CURRENTLIMITTYPE> extends DTSendable, AutoCloseable {
public interface DTMotor<MOTORTYPE> extends DTSendable {
@Override
void close();

Expand All @@ -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::getVelocityRPM, 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();

Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -127,9 +118,13 @@ default void configPIDFIZ(double proportional, double integral, double derivativ

double getEncoderPosition();

double getEncoderVelocity();
double getVelocityRPM();

DTMotorFaults getFaults();

String getFirmwareVersion();

double getMaxVelocity();

double getStallTorque();
}
4 changes: 1 addition & 3 deletions src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/DTMotorFaults.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
package org.victorrobotics.frc.dtlib.actuator.motor;

public interface DTMotorFaults<FAULT_TYPE> {
FAULT_TYPE internal();

public interface DTMotorFaults {
boolean hasAnyFault();

boolean lowVoltage();
Expand Down
40 changes: 28 additions & 12 deletions src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/ctre/DTTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -12,9 +13,11 @@
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;

public class DTTalonFX implements DTMotor<WPI_TalonFX, SupplyCurrentLimitConfiguration> {
public class DTTalonFX implements DTMotor<WPI_TalonFX> {
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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -101,8 +105,15 @@ 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(double baseCurrentLimit, double peakCurrentLimit,
double peakDuration) {
internal.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true,
baseCurrentLimit, peakCurrentLimit, peakDuration));
}

public void configAllSettings(TalonFXConfiguration config) {
Expand Down Expand Up @@ -196,8 +207,8 @@ public double getEncoderPosition() {
}

@Override
public double getEncoderVelocity() {
return internal.getSelectedSensorVelocity() / TICKS_PER_REV * 10;
public double getVelocityRPM() {
return internal.getSelectedSensorVelocity() / TICKS_PER_REV * 10 * SECONDS_PER_MINUTE;
}

public DTTalonFXFaults getFaults() {
Expand Down Expand Up @@ -235,7 +246,7 @@ public double getInputVoltage() {
return internal.getBusVoltage();
}

public static class DTTalonFXFaults implements DTMotorFaults<Faults> {
public static class DTTalonFXFaults implements DTMotorFaults {
private static final int OTHER_FAULTS_MASK = 0b00111111_10000000;

private final Faults internal;
Expand All @@ -244,11 +255,6 @@ public static class DTTalonFXFaults implements DTMotorFaults<Faults> {
this.internal = internal;
}

@Override
public Faults internal() {
return internal;
}

@Override
public boolean hasAnyFault() {
return internal.hasAnyFault();
Expand Down Expand Up @@ -294,4 +300,14 @@ public boolean hardwareFailure() {
return internal.HardwareFailure;
}
}

@Override
public double getMaxVelocity() {
return MAX_VELOCITY_RPM;
}

@Override
public double getStallTorque() {
return STALL_TORQUE;
}
}
47 changes: 20 additions & 27 deletions src/main/java/org/victorrobotics/frc/dtlib/actuator/motor/revrobotics/DTNeo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<CANSparkMax, DTNeo.DTNeoCurrentLimit> {
public class DTNeo implements DTMotor<CANSparkMax> {
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;
Expand All @@ -28,7 +31,7 @@ public void close() {
}

@Override
public CANSparkMax internal() {
public CANSparkMax getMotorImpl() {
return internal;
}

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -168,7 +171,7 @@ public double getEncoderPosition() {
}

@Override
public double getEncoderVelocity() {
public double getVelocityRPM() {
return encoder.getVelocity();
}

Expand All @@ -182,21 +185,7 @@ 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<Short> {
public static class DTNeoFaults implements DTMotorFaults {
private static final short OTHER_FAULTS_MASK = 0b00001101_11110110;

private final short internal;
Expand All @@ -205,11 +194,6 @@ public static class DTNeoFaults implements DTMotorFaults<Short> {
this.internal = internal;
}

@Override
public Short internal() {
return Short.valueOf(internal);
}

@Override
public boolean hasAnyFault() {
return internal != 0;
Expand Down Expand Up @@ -254,6 +238,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;
}
}
37 changes: 36 additions & 1 deletion src/main/java/org/victorrobotics/frc/dtlib/network/DTSendable.java
View file
Original file line number Diff line number Diff line change
@@ -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 <T> DTLimitedSupplier<T> limitRate(Supplier<T> supplier, double rateHz) {
return new DTLimitedSupplier<>(supplier, rateHz);
}
}

0 comments on commit 99ec95e

Please sign in to comment.