Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
feat(shooter): Add serializer motor TalonFX.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 2, 2024
1 parent 29bd5aa commit f27f2cf
Show file tree
Hide file tree
Showing 3 changed files with 54 additions and 37 deletions.
36 changes: 0 additions & 36 deletions src/main/java/frc/robot/shooter/SerializerMotorIOSparkMax.java

This file was deleted.

53 changes: 53 additions & 0 deletions src/main/java/frc/robot/shooter/SerializerMotorIOTalonFX.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
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.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<Double> velocityRotationsPerSecond, statorCurrentAmps;

private final SimpleMotorFeedforward velocityFeedforward;

public SerializerMotorIOTalonFX() {
talonFX = new TalonFX(20);

velocityRotationsPerSecond = talonFX.getVelocity();
statorCurrentAmps = talonFX.getStatorCurrent();

velocityFeedforward = new SimpleMotorFeedforward(0, 0);
}

@Override
public void configure() {
final TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.NeutralMode = NeutralModeValue.Coast;

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);
}
}
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/shooter/ShooterFactory.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ public class ShooterFactory {
*/
public static SerializerMotorIO createSerializerMotor() {
if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER))
return new SerializerMotorIOSparkMax();
return new SerializerMotorIOTalonFX();

return new SerializerMotorIOSim();
}
Expand Down

0 comments on commit f27f2cf

Please sign in to comment.