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

Commit

Permalink
feat(intake): Add TalonFX intake.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 2, 2024
1 parent ae9d860 commit 5815b70
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 43 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/intake/IntakeFactory.java
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
Expand Down
42 changes: 0 additions & 42 deletions src/main/java/frc/robot/intake/RollerMotorIOSparkMax.java

This file was deleted.

56 changes: 56 additions & 0 deletions src/main/java/frc/robot/intake/RollerMotorIOTalonFX.java
Original file line number Diff line number Diff line change
@@ -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();
}
}

0 comments on commit 5815b70

Please sign in to comment.