diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index b9c8b39..f710e31 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -14,7 +14,7 @@ public class IntakeFactory { */ public static RollerMotorIO createRollerMotor() { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) - return new RollerMotorIOSparkMax(); + return new RollerMotorIOTalonFX(); return new RollerMotorIOSim(); } diff --git a/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java b/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java deleted file mode 100644 index 00dcdf8..0000000 --- a/src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java +++ /dev/null @@ -1,42 +0,0 @@ -package frc.robot.intake; - -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import frc.lib.Configurator; - -/** Roller motor using a Spark Max. */ -public class RollerMotorIOSparkMax implements RollerMotorIO { - - /** Hardware Spark Max. */ - private final CANSparkMax sparkMax; - - public RollerMotorIOSparkMax() { - sparkMax = new CANSparkMax(5, MotorType.kBrushless); - } - - @Override - public void configure() { - Configurator.configureREV(sparkMax::restoreFactoryDefaults); - - sparkMax.setInverted(false); - - Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(40)); - - Configurator.configureStatusFrames(sparkMax); - } - - @Override - public void update(RollerMotorIOValues values) { - values.velocityRotationsPerSecond = getVelocity(); - values.currentAmps = sparkMax.getOutputCurrent(); - } - - @Override - public void setSetpoint(double velocityRotationsPerSecond) { - // TODO Implement velocity setpoint following - } - - public double getVelocity() { - return sparkMax.getEncoder().getVelocity() / 60.0; - } -} diff --git a/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java new file mode 100644 index 0000000..2d94863 --- /dev/null +++ b/src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java @@ -0,0 +1,56 @@ +package frc.robot.intake; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.lib.Configurator; + +/** Roller motor using a TalonFX. */ +public class RollerMotorIOTalonFX implements RollerMotorIO { + + private final TalonFX topTalonFX, bottomTalonFX; + + private final SimpleMotorFeedforward topFeedforward, bottomFeedforward; + + public RollerMotorIOTalonFX() { + topTalonFX = new TalonFX(0); // TODO + bottomTalonFX = new TalonFX(0); // TODO + + topFeedforward = new SimpleMotorFeedforward(0, 0); // TODO + bottomFeedforward = new SimpleMotorFeedforward(0, 0); // TODO + } + + @Override + public void configure() { + final TalonFXConfiguration config = new TalonFXConfiguration(); + + config.Feedback.SensorToMechanismRatio = 1.0; // TODO + + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + + Configurator.configureTalonFX(topTalonFX.getConfigurator(), config); + Configurator.configureTalonFX(bottomTalonFX.getConfigurator(), config); + } + + @Override + public void update(RollerMotorIOValues values) { + values.velocityRotationsPerSecond = getVelocity(); + values.currentAmps = topTalonFX.getStatorCurrent().refresh().getValue(); + } + + @Override + public void setSetpoint(double velocityRotationsPerSecond) { + double topVolts = topFeedforward.calculate(velocityRotationsPerSecond); + + topTalonFX.setVoltage(topVolts); + + double bottomVolts = bottomFeedforward.calculate(velocityRotationsPerSecond); + + bottomTalonFX.setVoltage(bottomVolts); + } + + public double getVelocity() { + return topTalonFX.getVelocity().refresh().getValue(); + } +}