diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 7675772..b5365a9 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -43,6 +43,7 @@ public Controls(Subsystems subsystems) { s = subsystems; configureIntake(); // Configure the trigger bindings + configureFlywheelBindings(); configureDrivebaseBindings(); } @@ -75,6 +76,14 @@ private double getDriveRotate() { return input * MaxSpeed; } + private void configureFlywheelBindings() { + if (s.flywheels == null) { + return; + } else { + driverController.rightTrigger().whileTrue(s.flywheels.runFlywheels()); + } + } + /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() diff --git a/src/main/java/frc/robot/Flywheels.java b/src/main/java/frc/robot/Flywheels.java new file mode 100644 index 0000000..09ade98 --- /dev/null +++ b/src/main/java/frc/robot/Flywheels.java @@ -0,0 +1,110 @@ +package frc.robot; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; + +public class Flywheels extends SubsystemBase { + private final TalonFX flywheelOne; + private final TalonFX flywheelTwo; + + public static double speed = 1; // placeholder + + // Configs: PIDS and limits + double kP = 0.0; + double kI = 0.0; + double kD = 0.0; + double kA = 0.0; + double kV = 0.0; + double kS = 0.0; + double kG = 0.0; + + private static final int supplyCurrentLimit = 20; + private static final int statorCurrentLimit = 40; + private static final boolean supplyCurrentLimitEnable = true; + private static final boolean statorCurrentLimitEnable = true; + private static final int motionMagicAcceleration = 40; + private final MotionMagicVelocityVoltage request = new MotionMagicVelocityVoltage(0); + private final Follower follow = + new Follower(Hardware.FLYWHEEL_ONE_ID, MotorAlignmentValue.Opposed); + public final double FLYWHEEL_TOLERANCE = 5; // TODO: Tune tolerance as well + + public Flywheels() { + flywheelOne = new TalonFX(Hardware.FLYWHEEL_ONE_ID); + flywheelTwo = new TalonFX(Hardware.FLYWHEEL_TWO_ID); + + configureMotors(); + } + + private void configureMotors() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + // TODO: tune configs and PIDS, currently placeholder + config.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = statorCurrentLimit; + config.CurrentLimits.SupplyCurrentLimitEnable = supplyCurrentLimitEnable; + config.CurrentLimits.StatorCurrentLimitEnable = statorCurrentLimitEnable; + + config.MotorOutput.NeutralMode = NeutralModeValue.Coast; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + config.Slot0.kP = kP; + config.Slot0.kI = kI; + config.Slot0.kD = kD; + config.Slot0.kA = kA; + config.Slot0.kV = kV; + config.Slot0.kS = kS; + config.Slot0.kG = kG; + + config.MotionMagic.MotionMagicAcceleration = motionMagicAcceleration; // RPS^2 + + flywheelOne.getConfigurator().apply(config); + flywheelTwo.getConfigurator().apply(config); + } + + public Command runFlywheels() { + return startEnd( + () -> { + setFlywheelVelocity(speed); + }, + () -> { + stopCommand(); + }) + .withName("Run flywheels"); + } + + private void setFlywheelVelocity(double rps) { + request.Velocity = rps; + flywheelOne.setControl(request); + flywheelTwo.setControl(follow); + } + + public Command suppliedSetVelocity(DoubleSupplier rps) { + return startEnd( + () -> { + request.Velocity = rps.getAsDouble(); + flywheelOne.setControl(request); + flywheelTwo.setControl(follow); + }, + () -> { + stopCommand(); + }) + .withName("Supplied velocity command"); + } + + public boolean isAtSpeed() { + return Math.abs(flywheelOne.getVelocity().getValueAsDouble() - request.Velocity) + <= FLYWHEEL_TOLERANCE; + } + + private void stopCommand() { + flywheelOne.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java index 81b53c4..08f32b9 100644 --- a/src/main/java/frc/robot/Hardware.java +++ b/src/main/java/frc/robot/Hardware.java @@ -10,4 +10,7 @@ public final class Hardware { // TODO: better number when get robot public static final int IntakeMotor = 0; + // TODO: make this accurate to the actual robot + public static final int FLYWHEEL_ONE_ID = 13; + public static final int FLYWHEEL_TWO_ID = 14; } diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index c20d12d..728a4aa 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -1,6 +1,7 @@ package frc.robot; import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED; +import static frc.robot.Subsystems.SubsystemConstants.FLYWHEELS_ENABLED; import static frc.robot.Subsystems.SubsystemConstants.INTAKE_ENABLED; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -14,10 +15,12 @@ public class Subsystems { public static class SubsystemConstants { public static final boolean DRIVEBASE_ENABLED = true; public static final boolean INTAKE_ENABLED = true; + public static final boolean FLYWHEELS_ENABLED = true; } public final CommandSwerveDrivetrain drivebaseSubsystem; public final IntakeSubsystem intakeSubsystem; + public final Flywheels flywheels; public Subsystems(Mechanism2d mechanism2d) { // Initialize subsystems here (don't forget to check if they're enabled!) @@ -43,5 +46,10 @@ public Subsystems(Mechanism2d mechanism2d) { } else { intakeSubsystem = null; } + if (FLYWHEELS_ENABLED) { + flywheels = new Flywheels(); + } else { + flywheels = null; + } } }