From 7d4f0da966b3673b6dd8063dc47cb6f1782083f4 Mon Sep 17 00:00:00 2001 From: AlbedoLinguine Date: Sat, 17 Jan 2026 13:50:15 -0600 Subject: [PATCH 1/3] Added Indexer(might need changes), created config inverted in titan util, fixed some shooter bugs --- src/main/java/frc/robot/Constants.java | 64 ++++++++++++--- .../frc/robot/subsystems/indexer/Indexer.java | 80 +++++++++++++++++++ .../frc/robot/subsystems/intake/Intake.java | 1 + .../frc/robot/subsystems/shooter/Shooter.java | 37 ++++++--- .../titan/core/subsystem/CTREMechanism.java | 74 +++++++++-------- 5 files changed, 200 insertions(+), 56 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6f96e29..3722907 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,7 +38,7 @@ public static enum Mode { public static class ShooterConstants { public enum ShooterState { - SHOOTING, + SHOOTER, REVERSE, IDLE } @@ -60,25 +60,65 @@ public enum ShooterState { public static final double maxReverseOutput = 0.5; public static final AngularVelocity shooterSpeed = Units.RPM.of(4000); - public static final AngularVelocity reverseSpeed = Units.RPM.of(-1000); + public static final AngularVelocity reverseSpeed = Units.RPM.of(-1000); public static final AngularVelocity idleSpeed = Units.RPM.of(0); public enum ShooterModes { - SHOOTER(shooterSpeed, 1.0), - REVERSE(reverseSpeed, -0.5), - IDLE(idleSpeed, 0.0); + SHOOTER(shooterSpeed, 1.0), + REVERSE(reverseSpeed, -0.5), + IDLE(idleSpeed, 0.0); - public AngularVelocity speed; - public double output; + public AngularVelocity speed; + public double output; + + ShooterModes(AngularVelocity speed, double output) { + this.speed = speed; + this.output = output; + } + } + } + + public static class IndexerConstants { - ShooterModes(AngularVelocity speed, double output) { - this.speed = speed; - this.output = output; - } + public enum IndexerState { + INDEXER, + REVERSE, + IDLE + } - } + public static final boolean attached = true; + public static final int id = -1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final boolean invert = false; + public static final boolean breakType = false; + public static final double gearRatio = 1 / 1; + + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + public static final double maxForwardOutput = 0.5; + public static final double maxReverseOutput = -0.5; + + public static final double indexerSpeed = 0.5; + public static final double reverseSpeed = -0.5; + public static final double idleSpeed = 0.0; + + public enum IndexerModes { + INDEXER(indexerSpeed), + REVERSE(reverseSpeed), + IDLE(idleSpeed); + + public double output; + + IndexerModes(double output) { + this.output = output; + } + } } public static final class IntakeConstants { diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..4e9132f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems.indexer; + +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.Constants; +import frc.robot.Constants.IndexerConstants; +import frc.robot.Constants.IndexerConstants.IndexerModes; +import frc.robot.Constants.IndexerConstants.IndexerState; +import frc.robot.Constants.IntakeConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; +import lombok.Getter; +import lombok.Setter; + +public class Indexer extends CTREMechanism { + + @Getter @Setter private IndexerModes indexerModes; + @Getter @Setter private IndexerState indexerState; + + private boolean attached; + private TalonFX motor; + + public static class IndexerConfig extends Config { + public IndexerConfig() { + super("Indexer", IndexerConstants.id, Constants.canbus); + + configNeutralBrakeMode(IndexerConstants.breakType); + configStatorCurrentLimit(IndexerConstants.stallLimit, true); + configSupplyCurrentLimit(IndexerConstants.supplyLimit, true); + configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); + configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); + configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); + configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); + configGearRatio(IndexerConstants.gearRatio); + configMotorInverted(IntakeConstants.invert); + } + } + + public Indexer(TalonFX motor, boolean attached) { + super(motor, attached); + this.attached = attached; + this.motor = motor; + this.indexerModes = IndexerModes.IDLE; + this.indexerState = IndexerState.IDLE; + + config.applyTalonConfig(motor); + } + + @Override + public void periodic() { + SmartDashboard.putString("Indexer Mode", getIndexerModes().toString()); + + + switch (this.indexerModes) { + case IDLE: + setIndexerState(IndexerState.IDLE); + break; + case INDEXER: + setIndexerState(IndexerState.INDEXER); + break; + case REVERSE: + setIndexerState(IndexerState.REVERSE); + break; + } + + } + + public Command runIndexerCommand(IndexerModes indexerModes) { + return new RunCommand(() -> setPercentOutput(indexerModes.output), this).withName("Indexer.runEnum"); + } + + @Override + public Config setConfig() { + // configure motors/sensors here + this.config = new IndexerConfig(); + return this.config; + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 1019237..c9d09bb 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -28,6 +28,7 @@ public IntakeConfig() { IntakeConstants.maxReverseRotation.in(Rotation), IntakeConstants.useRMaxRotation); configForwardSoftLimit( IntakeConstants.maxFowardRotation.in(Rotation), IntakeConstants.useFMaxRotation); + configMotorInverted(IntakeConstants.invert); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 318c974..793c698 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,24 +2,23 @@ import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.Constants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterConstants.ShooterModes; import frc.robot.Constants.ShooterConstants.ShooterState; -import frc.team5431.titan.core.misc.Logger; import frc.team5431.titan.core.subsystem.CTREMechanism; import lombok.Getter; import lombok.Setter; public class Shooter extends CTREMechanism { - @Getter - private ShooterState shooterState; - @Getter - @Setter - private ShooterModes shooterMode; + @Getter @Setter private ShooterState shooterState; + @Getter @Setter private ShooterModes shooterMode; + private boolean attached; + private TalonFX motor; public static class ShooterConfig extends Config { public ShooterConfig() { @@ -32,7 +31,8 @@ public ShooterConfig() { configReverseSoftLimit(ShooterConstants.maxReverseOutput, true); configPIDGains(ShooterConstants.p, ShooterConstants.i, ShooterConstants.d); configPeakOutput(ShooterConstants.maxForwardOutput, ShooterConstants.maxReverseOutput); - + configGearRatio(ShooterConstants.gearRatio); + configMotorInverted(ShooterConstants.inverted); } } @@ -44,14 +44,29 @@ public Shooter(TalonFX motor, boolean attached) { this.shooterState = ShooterState.IDLE; config.applyTalonConfig(motor); - this.setConfig(); } - + @Override + public void periodic() { + SmartDashboard.putString("Shooter Mode", getShooterMode().toString()); + + + switch (this.shooterMode) { + case IDLE: + setShooterState(ShooterState.IDLE); + break; + case SHOOTER: + setShooterState(ShooterState.SHOOTER); + break; + case REVERSE: + setShooterState(ShooterState.REVERSE); + break; + } + + } public Command runShooterCommand(ShooterModes shooterModes) { - return new RunCommand(() -> setVelocity(shooterModes.speed), this) - .withName("Shooter.runEnum"); + return new RunCommand(() -> setVelocity(shooterModes.speed), this).withName("Shooter.runEnum"); } @Override diff --git a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java index c2b74c4..6284d39 100644 --- a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java +++ b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java @@ -32,10 +32,12 @@ public abstract class CTREMechanism implements Subsystem { public Config config; /** - * Multi-Purpose subsystem class based on 3847 Spectrum's Mechanism class utlizing PID Closed-Loop + * Multi-Purpose subsystem class based on 3847 Spectrum's Mechanism class + * utlizing PID Closed-Loop * and Motion Magic positional or velocity control, WPILib Units, and TorqueFOC * - *

Only use one control method per mechanism, PLEASE! + *

+ * Only use one control method per mechanism, PLEASE! * * @param attached for if the motor is in use */ @@ -49,8 +51,7 @@ public CTREMechanism(TalonFX motor, boolean attached) { protected void setConfig(Config config) { this.config = config; - } - ; + }; /** Sets the mechanism position of the motor to 0 */ public void resetPosition() { @@ -73,13 +74,14 @@ public void resetMotorPosition(Angle position) { /** * Closed-loop Velocity Motion Magic with torque control (requires Pro) * - * @param velocity Mesure of Velocity in Terms of Angle; Converted to Rotations/Revolutions Per - * Second + * @param velocity Mesure of Velocity in Terms of Angle; Converted to + * Rotations/Revolutions Per + * Second */ public void setMMVelocityFOC(AngularVelocity velocity) { if (attached) { - MotionMagicVelocityTorqueCurrentFOC mm = - config.mmVelocityFOC.withVelocity(velocity.in(Units.RevolutionsPerSecond)); + MotionMagicVelocityTorqueCurrentFOC mm = config.mmVelocityFOC + .withVelocity(velocity.in(Units.RevolutionsPerSecond)); motor.setControl(mm); } } @@ -87,12 +89,13 @@ public void setMMVelocityFOC(AngularVelocity velocity) { /** * Closed-loop Velocity with torque control (requires Pro) * - * @param velocity Mesure of Velocity in Terms of Angle; Converted to Rotations Per Second + * @param velocity Mesure of Velocity in Terms of Angle; Converted to Rotations + * Per Second */ public void setVelocityTorqueCurrentFOC(AngularVelocity velocity) { if (attached) { - VelocityTorqueCurrentFOC output = - config.velocityTorqueCurrentFOC.withVelocity(velocity.in(Units.RotationsPerSecond)); + VelocityTorqueCurrentFOC output = config.velocityTorqueCurrentFOC + .withVelocity(velocity.in(Units.RotationsPerSecond)); motor.setControl(output); } } @@ -100,12 +103,12 @@ public void setVelocityTorqueCurrentFOC(AngularVelocity velocity) { /** * Closed-loop velocity control with voltage compensation * - * @param velocity Mesure of Velocity in Terms of Angle; Converted to Rotations Per Second + * @param velocity Mesure of Velocity in Terms of Angle; Converted to Rotations + * Per Second */ public void setVelocity(AngularVelocity velocity) { if (attached) { - VelocityVoltage output = - config.velocityControl.withVelocity(velocity.in(Units.RotationsPerSecond)); + VelocityVoltage output = config.velocityControl.withVelocity(velocity.in(Units.RotationsPerSecond)); motor.setControl(output); } } @@ -141,8 +144,7 @@ public void setPositionFOC(Angle position) { */ public void setMMPositionFOC(Angle position) { if (attached) { - MotionMagicTorqueCurrentFOC mm = - config.mmPositionFOC.withPosition(position.in(Units.Rotations)); + MotionMagicTorqueCurrentFOC mm = config.mmPositionFOC.withPosition(position.in(Units.Rotations)); motor.setControl(mm); } } @@ -175,12 +177,11 @@ public void setMMPosition(DoubleSupplier position) { * Closed-loop Position Motion Magic using a slot other than 0 * * @param position rotations - * @param slot gains slot + * @param slot gains slot */ public void setMMPosition(Angle position, int slot) { if (attached) { - MotionMagicVoltage mm = - config.mmPositionVoltageSlot.withSlot(slot).withPosition(position.in(Units.Rotations)); + MotionMagicVoltage mm = config.mmPositionVoltageSlot.withSlot(slot).withPosition(position.in(Units.Rotations)); motor.setControl(mm); } } @@ -232,8 +233,7 @@ public static class Config { public TalonFXConfiguration talonConfig; public double voltageCompSaturation; // 12V by default - public MotionMagicVelocityTorqueCurrentFOC mmVelocityFOC = - new MotionMagicVelocityTorqueCurrentFOC(0); + public MotionMagicVelocityTorqueCurrentFOC mmVelocityFOC = new MotionMagicVelocityTorqueCurrentFOC(0); public MotionMagicTorqueCurrentFOC mmPositionFOC = new MotionMagicTorqueCurrentFOC(0); public MotionMagicVelocityVoltage mmVelocityVoltage = new MotionMagicVelocityVoltage(0); public MotionMagicVoltage mmPositionVoltage = new MotionMagicVoltage(0); @@ -243,8 +243,8 @@ public static class Config { public PositionVoltage positionVoltage = new PositionVoltage(0); public PositionTorqueCurrentFOC positionFOC = new PositionTorqueCurrentFOC(0); public VelocityTorqueCurrentFOC velocityTorqueCurrentFOC = new VelocityTorqueCurrentFOC(0); - public DutyCycleOut percentOutput = - new DutyCycleOut(0); // Percent Output control using percentage of supply voltage //Should + public DutyCycleOut percentOutput = new DutyCycleOut(0); // Percent Output control using percentage of supply + // voltage //Should // normally use VoltageOut public Config(String title, int id, String canbus) { @@ -270,12 +270,23 @@ public void configVoltageCompensation(Voltage voltageCompSaturation) { this.voltageCompSaturation = voltageCompSaturation.in(Units.Volts); } + public void configClockwise_Positive() { + talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } + public void configCounterClockwise_Positive() { talonConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; } - public void configClockwise_Positive() { - talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + /** + * @param isInverted Inverts the motor if needed + */ + public void configMotorInverted(boolean isInverted) { + if (isInverted) { + talonConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + talonConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } } public void configSupplyCurrentLimit(Current supplyLimit, boolean enabled) { @@ -323,8 +334,7 @@ public void configReverseSoftLimit(double threshold, boolean enabled) { // Configure optional motion magic velocity parameters public void configMotionMagicVelocity(double acceleration, double feedforward) { mmVelocityFOC = mmVelocityFOC.withAcceleration(acceleration).withFeedForward(feedforward); - mmVelocityVoltage = - mmVelocityVoltage.withAcceleration(acceleration).withFeedForward(feedforward); + mmVelocityVoltage = mmVelocityVoltage.withAcceleration(acceleration).withFeedForward(feedforward); } // Configure optional motion magic position parameters @@ -335,13 +345,12 @@ public void configMotionMagicPosition(double feedforward) { /** * @param cruiseVelocity defines the passive running velocity of the motor, rps - * @param acceleration acceleration in rps - * @param jerk will slow down curve, rps + * @param acceleration acceleration in rps + * @param jerk will slow down curve, rps */ public void configMotionMagic( AngularVelocity cruiseVelocity, double acceleration, double jerk) { - talonConfig.MotionMagic.MotionMagicCruiseVelocity = - cruiseVelocity.in(Units.RotationsPerSecond); + talonConfig.MotionMagic.MotionMagicCruiseVelocity = cruiseVelocity.in(Units.RotationsPerSecond); talonConfig.MotionMagic.MotionMagicAcceleration = acceleration; talonConfig.MotionMagic.MotionMagicJerk = jerk; } @@ -422,8 +431,7 @@ public void configGravityType(boolean isArm) { } public void configGravityType(int slot, boolean isArm) { - GravityTypeValue gravityType = - isArm ? GravityTypeValue.Arm_Cosine : GravityTypeValue.Elevator_Static; + GravityTypeValue gravityType = isArm ? GravityTypeValue.Arm_Cosine : GravityTypeValue.Elevator_Static; if (slot == 0) { talonConfig.Slot0.GravityType = gravityType; } else if (slot == 1) { From 9e7796850acbcda553229ad87bbb48665dbe4453 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 17 Jan 2026 20:01:05 +0000 Subject: [PATCH 2/3] Initial plan From d5cff84d0a2663ddb97405e4c6ec40afa5ec3625 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sat, 17 Jan 2026 20:03:27 +0000 Subject: [PATCH 3/3] Fix: Use IndexerConstants.invert instead of IntakeConstants.invert Co-authored-by: AlbedoLinguine <58450360+AlbedoLinguine@users.noreply.github.com> --- gradlew | 0 src/main/java/frc/robot/subsystems/indexer/Indexer.java | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 4e9132f..e4fafae 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -34,7 +34,7 @@ public IndexerConfig() { configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); configGearRatio(IndexerConstants.gearRatio); - configMotorInverted(IntakeConstants.invert); + configMotorInverted(IndexerConstants.invert); } }