From 360ebaf9f521f1802b4106b22c5b10a5091e0845 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Mon, 8 Apr 2024 02:36:53 -0400 Subject: [PATCH] refactor(controller): Work on velocity controllers. --- src/main/java/frc/lib/PIDFConstants.java | 10 +-- .../lib/controller/VelocityControllerIO.java | 57 ++++++++++++++ .../VelocityControllerIOTalonFX.java | 74 +++++++++++++++++++ .../VelocityControllerIOTalonFXPIDF.java | 47 ++++++++++++ 4 files changed, 183 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/lib/controller/VelocityControllerIO.java create mode 100644 src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java create mode 100644 src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java diff --git a/src/main/java/frc/lib/PIDFConstants.java b/src/main/java/frc/lib/PIDFConstants.java index df95c41..8dc6818 100644 --- a/src/main/java/frc/lib/PIDFConstants.java +++ b/src/main/java/frc/lib/PIDFConstants.java @@ -17,11 +17,8 @@ public class PIDFConstants { /** Feedback controller position tolerance. */ public double kPositionTolerance = 0.0; - /** Feedback controller velocity constraint. */ - public double kVelocityConstraint = 0.0; - - /** Feedback controller acceleration constraint. */ - public double kAccelerationConstraint = 0.0; + /** Feedback controller velocity tolerance. */ + public double kVelocityTolerance = 0.0; /** Feedforward controller static gain. */ public double kS = 0.0; @@ -29,6 +26,9 @@ public class PIDFConstants { /** Feedforward controller velocity gain. */ public double kV = 0.0; + /** Feedforward controller acceleration gain. */ + public double kA = 0.0; + /** * Creates a Phoenix PIDF configuration using the PIDF constants. * diff --git a/src/main/java/frc/lib/controller/VelocityControllerIO.java b/src/main/java/frc/lib/controller/VelocityControllerIO.java new file mode 100644 index 0000000..30f0526 --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIO.java @@ -0,0 +1,57 @@ +package frc.lib.controller; + +/** Velocity controller interface. */ +public interface VelocityControllerIO { + + /** Velocity controller constants. */ + public static class VelocityControllerIOConstants { + /** Interpret counterclockwise rotation on the motor face as having positive velocity, if set to true. */ + public boolean ccwPositive = true; + + /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */ + public boolean neutralBrake = false; + + /** Maximum amount of current, in amps, to provide to the motor. */ + public double currentLimitAmps = 40.0; + + /** Ratio between the velocity sensor and the controlled mechanism. */ + public double sensorToMechanismRatio = 1.0; + } + + /** Velocity controller values. */ + public static class VelocityControllerIOValues { + /** Velocity in rotations per second. */ + public double velocityRotationsPerSecond = 0.0; + + /** Acceleration in rotations per second per second. */ + public double accelerationRotationsPerSecondPerSecond = 0.0; + + /** Motor voltage in volts. */ + public double motorVolts = 0.0; + + /** Motor current in amps. */ + public double motorAmps = 0.0; + } + + /** + * Configures the velocity controller. + * + * @param constants + */ + public void configure(VelocityControllerIOConstants constants); + + /** + * Updates the velocity controller's values. + * + * @param values + */ + public void update(VelocityControllerIOValues values); + + /** + * Sets the velocity setpoint. + * + * @param velocityRotationsPerSecond + */ + public void setSetpoint(double velocityRotationsPerSecond); + +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java new file mode 100644 index 0000000..553ec2d --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFX.java @@ -0,0 +1,74 @@ +package frc.lib.controller; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import frc.lib.CAN; +import frc.lib.Configurator; + +/** Velocity controller using TalonFX. */ +public abstract class VelocityControllerIOTalonFX implements VelocityControllerIO { + + protected final TalonFX motor; + + protected final StatusSignal velocity, acceleration, volts, amps; + + /** + * Creates a new velocity controller using TalonFX. + * + * @param can + */ + protected VelocityControllerIOTalonFX(CAN can) { + motor = new TalonFX(can.id(), can.bus()); + + velocity = motor.getVelocity(); + acceleration = motor.getAcceleration(); + volts = motor.getMotorVoltage(); + amps = motor.getStatorCurrent(); + } + + @Override + public void configure(VelocityControllerIOConstants constants) { + BaseStatusSignal.setUpdateFrequencyForAll(100.0, velocity, acceleration, volts, amps); + + motor.optimizeBusUtilization(); + + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.Inverted = constants.ccwPositive ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; + config.MotorOutput.NeutralMode = constants.neutralBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast; + + // Stator current is a measure of the current inside of the motor and is typically higher than supply (breaker) current + config.CurrentLimits.StatorCurrentLimit = constants.currentLimitAmps * 2.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + + config.CurrentLimits.SupplyCurrentLimit = constants.currentLimitAmps; + // Allow higher current spikes (150%) for a brief duration (one second) + // REV 40A auto-resetting breakers typically trip when current exceeds 300% for one second + config.CurrentLimits.SupplyCurrentThreshold = constants.currentLimitAmps * 1.5; + config.CurrentLimits.SupplyTimeThreshold = 1; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + + config.Feedback.SensorToMechanismRatio = constants.sensorToMechanismRatio; + + Configurator.configureTalonFX(motor.getConfigurator(), config); + } + + @Override + public void update(VelocityControllerIOValues values) { + BaseStatusSignal.refreshAll(velocity, acceleration, volts, amps); + + values.velocityRotationsPerSecond = velocity.getValue(); + values.accelerationRotationsPerSecondPerSecond = acceleration.getValue(); + values.motorVolts = volts.getValue(); + values.motorAmps = volts.getValue(); + } + + @Override + public abstract void setSetpoint(double velocityRotationsPerSecond); + +} diff --git a/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java new file mode 100644 index 0000000..c63406c --- /dev/null +++ b/src/main/java/frc/lib/controller/VelocityControllerIOTalonFXPIDF.java @@ -0,0 +1,47 @@ +package frc.lib.controller; + +import com.ctre.phoenix6.controls.VoltageOut; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import frc.lib.CAN; +import frc.lib.PIDFConstants; + +/** Velocity controller using TalonFX and external PIDF. */ +public class VelocityControllerIOTalonFXPIDF extends VelocityControllerIOTalonFX { + + private final SimpleMotorFeedforward feedforward; + + private final PIDController feedback; + + private final VoltageOut voltage; + + /** + * Creates a new velocity controller using TalonFX and external PIDF. + * + * @param can + * @param pidf + * @param enableFOC + */ + public VelocityControllerIOTalonFXPIDF(CAN can, PIDFConstants pidf, boolean enableFOC) { + super(can); + + feedforward = new SimpleMotorFeedforward(pidf.kS, pidf.kV, pidf.kA); + + feedback = new PIDController(pidf.kP, pidf.kI, pidf.kD); + + voltage = new VoltageOut(0.0).withEnableFOC(enableFOC); + } + + @Override + public void setSetpoint(double velocityRotationsPerSecond) { + double feedforwardVolts = feedforward.calculate(velocityRotationsPerSecond); + + double measuredVelocityRotationsPerSecond = velocity.getValue(); + + double feedbackVolts = feedback.calculate(measuredVelocityRotationsPerSecond, velocityRotationsPerSecond); + + motor.setControl(voltage.withOutput(feedforwardVolts + feedbackVolts)); + } + +}