diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIO.java b/src/main/java/frc/robot/shooter/FlywheelMotorIO.java deleted file mode 100644 index e73303e..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIO.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.shooter; - -/** Flywheel motor hardware interface. */ -public interface FlywheelMotorIO { - - /** Values for the flywheel motor hardware interface. */ - public static class FlywheelMotorIOValues { - /** Velocity of the flywheel in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Current drawn by the flywheel motor in amps. */ - public double currentAmps = 0.0; - } - - /** Configures the flywheel motor. */ - public void configure(); - - /** - * Updates the flywheel motor's values. - * - * @param values - */ - public void update(FlywheelMotorIOValues values); - - /** - * Sets the setpoint of the flywheel motor. - * - * @param velocityRotationsPerSecond the velocity setpoint of the flywheel motor. - */ - public void setSetpoint(double velocityRotationsPerSecond); -} diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java deleted file mode 100644 index 23d3a39..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOSim.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.shooter; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.RobotConstants; - -/** Simulated flywheel motor. */ -public class FlywheelMotorIOSim implements FlywheelMotorIO { - - private final FlywheelSim flywheelSim; - - private final SimpleMotorFeedforward flywheelVelocityFeedforward; - - public FlywheelMotorIOSim() { - flywheelSim = new FlywheelSim(DCMotor.getKrakenX60(1), 2.25, 0.009272); - - flywheelVelocityFeedforward = new SimpleMotorFeedforward(0.0, 0.2685); - } - - @Override - public void configure() {} - - @Override - public void update(FlywheelMotorIOValues values) { - values.velocityRotationsPerSecond = getFlywheelVelocityRotationsPerSecond(); - values.currentAmps = flywheelSim.getCurrentDrawAmps(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = flywheelVelocityFeedforward.calculate(velocityRotationsPerSecond); - - flywheelSim.setInputVoltage(volts); - - flywheelSim.update(RobotConstants.PERIODIC_DURATION); - } - - private double getFlywheelVelocityRotationsPerSecond() { - return flywheelSim.getAngularVelocityRPM() / 60; - } -} diff --git a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java deleted file mode 100644 index db42d83..0000000 --- a/src/main/java/frc/robot/shooter/FlywheelMotorIOTalonFX.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.shooter; - -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 edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.lib.Configurator; - -/** Flywheel motor using TalonFX. */ -public class FlywheelMotorIOTalonFX implements FlywheelMotorIO { - - private final TalonFX talonFX; - - private final StatusSignal velocityRotationsPerSecond, statorCurrentAmps; - - private final SimpleMotorFeedforward velocityFeedforward; - - public FlywheelMotorIOTalonFX() { - talonFX = new TalonFX(44); - - velocityRotationsPerSecond = talonFX.getVelocity(); - statorCurrentAmps = talonFX.getStatorCurrent(); - - velocityFeedforward = new SimpleMotorFeedforward(0.14, 0.2539); - } - - @Override - public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - config.CurrentLimits.StatorCurrentLimit = 40; - config.CurrentLimits.StatorCurrentLimitEnable = true; - - Configurator.configureTalonFX(talonFX.getConfigurator(), config); - } - - @Override - public void update(FlywheelMotorIOValues values) { - velocityRotationsPerSecond.refresh(); - statorCurrentAmps.refresh(); - - values.velocityRotationsPerSecond = velocityRotationsPerSecond.getValue(); - values.currentAmps = statorCurrentAmps.getValue(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = velocityFeedforward.calculate(velocityRotationsPerSecond); - - talonFX.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIO.java b/src/main/java/frc/robot/shooter/SerializerMotorIO.java deleted file mode 100644 index eebbda2..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIO.java +++ /dev/null @@ -1,31 +0,0 @@ -package frc.robot.shooter; - -/** Serializer motor hardware interface. */ -public interface SerializerMotorIO { - - /** Values for the serializer motor hardware interface. */ - public static class SerializerMotorIOValues { - /** Velocity of the serializer in rotations per second. */ - public double velocityRotationsPerSecond = 0.0; - - /** Current drawn by the serializer motor in amps. */ - public double currentAmps = 0.0; - } - - /** Configures the serializer motor. */ - public void configure(); - - /** - * Updates the serializer motor's values. - * - * @param values - */ - public void update(SerializerMotorIOValues values); - - /** - * Sets the setpoint of the serializer motor. - * - * @param velocityRotationsPerSecond the velocity setpoint of the serializer motor. - */ - public void setSetpoint(double velocityRotationsPerSecond); -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java b/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java deleted file mode 100644 index 351eee9..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOSim.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.shooter; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.FlywheelSim; -import frc.robot.RobotConstants; - -/** Simulated serializer motor. */ -public class SerializerMotorIOSim implements SerializerMotorIO { - - private final FlywheelSim serializerSim; - - private final SimpleMotorFeedforward serializerVelocityFeedforward; - - public SerializerMotorIOSim() { - serializerSim = new FlywheelSim(DCMotor.getKrakenX60(1), 3.0, 0.06202); - - serializerVelocityFeedforward = new SimpleMotorFeedforward(0.0, 0.358); - } - - @Override - public void configure() {} - - @Override - public void update(SerializerMotorIOValues values) { - values.velocityRotationsPerSecond = getSerializerVelocityRotationsPerSecond(); - values.currentAmps = serializerSim.getCurrentDrawAmps(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = serializerVelocityFeedforward.calculate(velocityRotationsPerSecond); - - serializerSim.setInputVoltage(volts); - - serializerSim.update(RobotConstants.PERIODIC_DURATION); - } - - private double getSerializerVelocityRotationsPerSecond() { - return serializerSim.getAngularVelocityRPM() / 60; - } -} diff --git a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java b/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java deleted file mode 100644 index 15ff9a8..0000000 --- a/src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java +++ /dev/null @@ -1,60 +0,0 @@ -package frc.robot.shooter; - -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 edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.lib.Configurator; - -/** Serializer motor using TalonFX. */ -public class SerializerMotorIOTalonFX implements SerializerMotorIO { - - private final TalonFX talonFX; - - private final StatusSignal velocityRotationsPerSecond, statorCurrentAmps; - - private final SimpleMotorFeedforward velocityFeedforward; - - public SerializerMotorIOTalonFX() { - talonFX = new TalonFX(42); - - velocityRotationsPerSecond = talonFX.getVelocity(); - statorCurrentAmps = talonFX.getStatorCurrent(); - - velocityFeedforward = new SimpleMotorFeedforward(0.14, 0.2617); - } - - @Override - public void configure() { - final TalonFXConfiguration config = new TalonFXConfiguration(); - - config.Feedback.SensorToMechanismRatio = 36.0 / 16.0; - - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - - config.CurrentLimits.StatorCurrentLimit = 40; - config.CurrentLimits.StatorCurrentLimitEnable = true; - - Configurator.configureTalonFX(talonFX.getConfigurator(), config); - } - - @Override - public void update(SerializerMotorIOValues values) { - velocityRotationsPerSecond.refresh(); - statorCurrentAmps.refresh(); - - values.velocityRotationsPerSecond = velocityRotationsPerSecond.getValue(); - values.currentAmps = statorCurrentAmps.getValue(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - double volts = velocityFeedforward.calculate(velocityRotationsPerSecond); - - talonFX.setVoltage(volts); - } -}