From 65f7c215ff00af82fe1f98879cc3e4f5ea316042 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Thu, 22 Jan 2026 19:18:26 -0600 Subject: [PATCH 1/8] Intake Rollers done Ask why the IO classes autologged inputs is as doubles and not specific Angle or Voltage units. --- src/main/java/frc/robot/Constants.java | 61 ++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 22 +++++- .../robot/subsystems/intake/pivot/Pivot.java | 36 +++++++++ .../subsystems/intake/pivot/PivotIO.java | 23 ++++++ .../subsystems/intake/pivot/PivotIOSim.java | 59 +++++++++++++++ .../intake/pivot/PivotIOSparkFlex.java | 56 ++++++++++++++ .../intake/pivot/PivotIOTalonFX.java | 74 +++++++++++++++++++ .../subsystems/intake/roller/Roller.java | 14 ++-- .../subsystems/intake/roller/RollerIO.java | 2 +- .../subsystems/intake/roller/RollerIOSim.java | 17 +++-- .../intake/roller/RollerIOSparkFlex.java | 17 +++-- .../intake/roller/RollerIOTalonFX.java | 34 +++++---- .../titan/core/subsystem/CTREMechanism.java | 2 +- 13 files changed, 371 insertions(+), 46 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java create mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8cfef36..4d966c3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.revrobotics.spark.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; @@ -126,7 +127,7 @@ public enum IndexerModes { } } - public static final class IntakeRollerIOConstants { + public static final class IntakeRollerConstants { public static final boolean attached = true; public static final boolean useRpm = false; @@ -168,7 +169,7 @@ public static final class IntakeRollerIOConstants { public static final AngularVelocity outtakeSpeed = Units.RPM.of(0); public static final AngularVelocity idleSpeed = Units.RPM.of(0); - public enum RollerIOModes { + public enum IntakeRollerModes { IDLE(idleSpeed, 0.0), INTAKE(rollerIOSpeed, 8.4), OUTTAKE(outtakeSpeed, -4.8); @@ -177,10 +178,62 @@ public enum RollerIOModes { // TODO: Make sure voltage is just output * 12 (for 12V) public double voltage; - RollerIOModes(AngularVelocity speed, double voltage) { + IntakeRollerModes(AngularVelocity speed, double voltage) { this.speed = speed; this.voltage = voltage; } } } -} + + public static class IntakePivotConstants { + 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 double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final Current forwardTorqueLimit = Units.Amps.of(80); + public static final Current reverseTorqueLimit = Units.Amps.of(80); + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + public static final IdleMode idleMode = IdleMode.kBrake; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = + FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = + FeedbackSensor.kPrimaryEncoder; + + public static final double maxForwardOutput = 0.5; + public static final double maxReverseOutput = -0.5; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final boolean useStallLimit = true; + public static final boolean useSupplyLimit = true; + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + + public static final Angle stow = Units.Rotation.of(0); + public static final Angle feed = Units.Rotation.of(0); + public static final Angle eject = Units.Rotation.of(0); + + public enum IntakePivotPositions { + STOW(stow), EJECT(eject), FEED(feed); + public Angle position; + + IntakePivotPositions (Angle position) { + this.position = position; + } + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c39a65..68a4ba0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,7 +17,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes; +import frc.robot.Constants.IntakePivotConstants.IntakePivotPositions; +import frc.robot.Constants.IntakeRollerConstants.IntakeRollerModes; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; @@ -26,6 +27,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.intake.pivot.Pivot; +import frc.robot.subsystems.intake.pivot.PivotIO; +import frc.robot.subsystems.intake.pivot.PivotIOSim; +import frc.robot.subsystems.intake.pivot.PivotIOSparkFlex; import frc.robot.subsystems.intake.roller.Roller; import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; @@ -49,6 +54,7 @@ public class RobotContainer { private final Drive drive; private final Vision vision; private final Roller roller; + private final Pivot pivot; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -78,10 +84,13 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOLimelight(camera0Name, drive::getRotation), new VisionIOLimelight(camera1Name, drive::getRotation)); + roller = new Roller( new RollerIOSparkFlex()); - + + pivot = + new Pivot(new PivotIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -125,8 +134,10 @@ public RobotContainer() { roller = new Roller( new RollerIOSim() {}); - break; + pivot = + new Pivot(new PivotIOSim() {}); + break; default: // Replayed robot, disable IO implementations drive = @@ -139,6 +150,7 @@ public RobotContainer() { vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); roller = new Roller(new RollerIO() {}); + pivot = new Pivot(new PivotIO() {}); break; @@ -224,7 +236,9 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true)); + operator.a().onTrue(roller.runIntakeCommand(IntakeRollerModes.INTAKE, true)); + operator.b().onTrue(pivot.runPivotCommand(IntakePivotPositions.FEED)); + operator.y().onTrue(pivot.runPivotCommand(IntakePivotPositions.STOW)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java new file mode 100644 index 0000000..1a8d90a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems.intake.pivot; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.IntakePivotConstants.IntakePivotPositions; + +public class Pivot extends SubsystemBase { + private final PivotIO pivotIO; + private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); + + private IntakePivotPositions mode; + + public Pivot(PivotIO PivotIO) { + this.pivotIO = PivotIO; + this.mode = IntakePivotPositions.STOW; + } + + @Override + public void periodic() { + pivotIO.updateInputs(inputs); + Logger.processInputs("Intake/Pivot", inputs); + Logger.recordOutput("Intake/Pivot/Mode", mode); + } + + public void runEnum(IntakePivotPositions pivotPosition) { + this.mode = pivotPosition; + pivotIO.setPosition(pivotPosition.position.magnitude()); + } + + public Command runPivotCommand(IntakePivotPositions pivotPosition) { + return new RunCommand(() -> this.runEnum(pivotPosition), this).withName("IntakePivot.runEnum"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java new file mode 100644 index 0000000..062c3db --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.intake.pivot; + +import org.littletonrobotics.junction.AutoLog; + +public interface PivotIO { + + @AutoLog + public static class PivotIOInputs { + public boolean pivotConnected = false; + public double appliedVoltage = 0.0; + public double position = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(PivotIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setPivotVoltage(double voltage) {} + + /** Run the motor to the specified position. */ + public default void setPosition(double positionAngle) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java new file mode 100644 index 0000000..154df7d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.intake.pivot; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class PivotIOSim implements PivotIO { + private DCMotorSim pivotMotorSim; + private PIDController pivotContpivot = new PIDController(PIVOT_KP, 0, PIVOT_KD); + private boolean pivotClosedLoop = false; + private double appliedVoltage = 0.0; + private double pivotFFVolts = 0.0; + + // From ModuleIOSim no clue tbh + private static final double pivot_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double PIVOT_KV = 1.0 / Units.rotationsToRadians(1.0 / pivot_KV_ROT); + private static final double PIVOT_KS = 0.0; + private static final double PIVOT_KP = 1.0; + private static final double PIVOT_KD = 0.0; + + + public PivotIOSim() { + this.pivotMotorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + + // Enable wrapping for turn PID --- TODO: NOT SURE WHAT THIS IS ASK + pivotContpivot.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + if (pivotClosedLoop) { + appliedVoltage = pivotFFVolts + pivotContpivot.calculate(pivotMotorSim.getAngularVelocityRadPerSec()); + } else { + pivotContpivot.reset(); + } + + pivotMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + pivotMotorSim.update(0.02); + + inputs.pivotConnected = true; + inputs.position = pivotMotorSim.getAngularPositionRad(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(pivotMotorSim.getCurrentDrawAmps()); + } + + @Override + // TODO + public void setPosition(double positionAngle) { + + } + + public void setAppliedVoltage(double appliedVoltage) { + this.appliedVoltage = MathUtil.clamp(appliedVoltage, -12.0, 12.0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java new file mode 100644 index 0000000..ba27df7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.intake.pivot; + +import static frc.robot.util.SparkUtil.*; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; + +import frc.robot.Constants.IntakePivotConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class PivotIOSparkFlex implements PivotIO { + private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class PivotSparkFlexConfig extends REVMechanism.Config { + public PivotSparkFlexConfig() { + super("PivotSparkFlex", IntakePivotConstants.id); + configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); + // configGear(PivotConstants.gearRat); + // configGravity(PivotConstants.gravityType); + configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); + configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); + } + } + + public PivotIOSparkFlex() { + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.position = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setPivotVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } + + @Override + public void setPosition(double positionAngle) { + sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, + ClosedLoopSlot.kSlot0); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java new file mode 100644 index 0000000..1feaab9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.intake.pivot; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +import static edu.wpi.first.units.Units.*; + +import frc.robot.Constants; +import frc.robot.Constants.IntakePivotConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class PivotIOTalonFX implements PivotIO { + private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); + + public static class PivotTalonFXConfig extends CTREMechanism.Config { + public PivotTalonFXConfig() { + super("PivotTalonFX", IntakePivotConstants.id, Constants.CANBUS); + configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + configNeutralBrakeMode(IntakePivotConstants.breakType); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); + configGearRatio(IntakePivotConstants.gearRatio); + configGravityType(IntakePivotConstants.gravityType); + configSupplyCurrentLimit(IntakePivotConstants.supplyLimit, IntakePivotConstants.useSupplyLimit); + configStatorCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.useStallLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation.in(Rotation), IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation.in(Rotation), IntakePivotConstants.useFMaxRotation); + } + } + + + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + private PivotTalonFXConfig config; + + public PivotIOTalonFX(TalonFX talon) { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.position = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setPivotVoltage(double voltage) { + talon.setVoltage(voltage); + } + + @Override + public void setPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } + +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java index 081fc95..47ae70e 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java @@ -1,29 +1,33 @@ package frc.robot.subsystems.intake.roller; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes; +import frc.robot.Constants.IntakeRollerConstants.IntakeRollerModes; public class Roller extends SubsystemBase { private final RollerIO rollerIO; private final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); - private RollerIOModes mode; + private IntakeRollerModes mode; public Roller(RollerIO rollerIO) { this.rollerIO = rollerIO; - this.mode = RollerIOModes.IDLE; + this.mode = IntakeRollerModes.IDLE; } @Override public void periodic() { rollerIO.updateInputs(inputs); + Logger.recordOutput("Intake/Roller/Mode", mode.toString()); + Logger.processInputs("Intake/Roller", inputs); } - public void runEnum(RollerIOModes rollerIOModes, boolean useRPM) { + public void runEnum(IntakeRollerModes rollerIOModes, boolean useRPM) { this.mode = rollerIOModes; if (useRPM) { rollerIO.setRPM(rollerIOModes.speed.magnitude()); @@ -33,7 +37,7 @@ public void runEnum(RollerIOModes rollerIOModes, boolean useRPM) { } } - public Command runIntakeCommand(RollerIOModes rollerIOModes, boolean useRPM) { + public Command runIntakeCommand(IntakeRollerModes rollerIOModes, boolean useRPM) { return new RunCommand(() -> this.runEnum(rollerIOModes, useRPM), this) .withName("Intake.runEnum"); } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java index 7428118..3a9364b 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java @@ -9,7 +9,7 @@ public static class RollerIOInputs { public boolean rollerConnected = false; public double appliedVoltage = 0.0; public double RPM = 0.0; - + public double currentAmps = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java index 331d3fa..0b3ee94 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java @@ -9,15 +9,17 @@ public class RollerIOSim implements RollerIO { private DCMotorSim rollerMotorSim; - private PIDController rollerController; + private PIDController rollerController = new PIDController(ROLLER_KP, 0, ROLLER_KD); private boolean rollerClosedLoop = false; private double appliedVoltage = 0.0; - private double driveFFVolts = 0.0; + private double rollerFFVolts = 0.0; // From ModuleIOSim no clue tbh - private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); - private static final double DRIVE_KS = 0.0; + private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); + private static final double ROLLER_KS = 0.0; + private static final double ROLLER_KP = 1.0; + private static final double ROLLER_KD = 0.0; public RollerIOSim() { @@ -31,7 +33,7 @@ public RollerIOSim() { @Override public void updateInputs(RollerIOInputs inputs) { if (rollerClosedLoop) { - appliedVoltage = driveFFVolts + rollerController.calculate(rollerMotorSim.getAngularVelocityRadPerSec()); + appliedVoltage = rollerFFVolts + rollerController.calculate(rollerMotorSim.getAngularVelocityRadPerSec()); } else { rollerController.reset(); } @@ -42,12 +44,13 @@ public void updateInputs(RollerIOInputs inputs) { inputs.rollerConnected = true; inputs.RPM = rollerMotorSim.getAngularVelocityRadPerSec(); inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(rollerMotorSim.getCurrentDrawAmps()); } @Override public void setRPM(double RPM) { rollerClosedLoop = true; - driveFFVolts = DRIVE_KS * Math.signum(RPM) + DRIVE_KV * RPM; + rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; rollerController.setSetpoint(RPM); } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index a7e0ff1..b7cd351 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -9,7 +9,7 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; -import frc.robot.Constants.IntakeRollerIOConstants; +import frc.robot.Constants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { @@ -17,17 +17,17 @@ public class RollerIOSparkFlex implements RollerIO { private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class RollerIOSparkFlexConfig extends REVMechanism.Config { public RollerIOSparkFlexConfig() { - super("RollerSparkFlex", IntakeRollerIOConstants.id); - configPIDGains(IntakeRollerIOConstants.p, IntakeRollerIOConstants.i, IntakeRollerIOConstants.d); - configFeedbackSensorSource(IntakeRollerIOConstants.feedbackSensorREV); + super("RollerSparkFlex", IntakeRollerConstants.id); + configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); + configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorREV); // configGear(RollerIOConstants.gearRatio); // configGravity(RollerIOConstants.gravityType); - configSmartCurrentLimit(IntakeRollerIOConstants.stallLimit, IntakeRollerIOConstants.supplyLimit); - configSmartStallCurrentLimit(IntakeRollerIOConstants.stallLimit); + configSmartCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.supplyLimit); + configSmartStallCurrentLimit(IntakeRollerConstants.stallLimit); configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation, IntakeRollerIOConstants.useRMaxRotation); + IntakeRollerConstants.maxReverseRotation, IntakeRollerConstants.useRMaxRotation); configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation, IntakeRollerIOConstants.useFMaxRotation); + IntakeRollerConstants.maxFowardRotation, IntakeRollerConstants.useFMaxRotation); } } @@ -39,6 +39,7 @@ public RollerIOSparkFlex() { public void updateInputs(RollerIOInputs inputs) { ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 1969919..a2ed2b1 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -8,52 +8,54 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.Constants.IntakeRollerIOConstants; +import frc.robot.Constants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class RollerIOTalonFX implements RollerIO { - private final TalonFX talon = new TalonFX(IntakeRollerIOConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.CANBUS); public static class RollerTalonFXConfig extends CTREMechanism.Config { public RollerTalonFXConfig() { - super("RollerTalonFX", IntakeRollerIOConstants.id, Constants.CANBUS); - configPIDGains(IntakeRollerIOConstants.p, IntakeRollerIOConstants.i, IntakeRollerIOConstants.d); - configNeutralBrakeMode(IntakeRollerIOConstants.breakType); - configFeedbackSensorSource(IntakeRollerIOConstants.feedbackSensorCTRE); - configGearRatio(IntakeRollerIOConstants.gearRatio); - configGravityType(IntakeRollerIOConstants.gravityType); - configSupplyCurrentLimit(IntakeRollerIOConstants.supplyLimit, IntakeRollerIOConstants.useSupplyLimit); - configStatorCurrentLimit(IntakeRollerIOConstants.stallLimit, IntakeRollerIOConstants.useStallLimit); + super("RollerTalonFX", IntakeRollerConstants.id, Constants.CANBUS); + configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); + configNeutralBrakeMode(IntakeRollerConstants.breakType); + configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorCTRE); + configGearRatio(IntakeRollerConstants.gearRatio); + configGravityType(IntakeRollerConstants.gravityType); + configSupplyCurrentLimit(IntakeRollerConstants.supplyLimit, IntakeRollerConstants.useSupplyLimit); + configStatorCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.useStallLimit); configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation.in(Rotation), IntakeRollerIOConstants.useRMaxRotation); + IntakeRollerConstants.maxReverseRotation.in(Rotation), IntakeRollerConstants.useRMaxRotation); configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation.in(Rotation), IntakeRollerIOConstants.useFMaxRotation); + IntakeRollerConstants.maxFowardRotation.in(Rotation), IntakeRollerConstants.useFMaxRotation); } } private StatusSignal appliedVoltage; private StatusSignal rollerRPM; - private StatusSignal rollerOutput; + private StatusSignal currentAmps; private RollerTalonFXConfig config; public RollerIOTalonFX(TalonFX talon) { appliedVoltage = talon.getMotorVoltage(); rollerRPM = talon.getVelocity(); - rollerOutput = talon.getClosedLoopOutput(); + currentAmps = talon.getStatorCurrent(); config.applyTalonConfig(talon); - BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, rollerOutput, rollerRPM); + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); } @Override public void updateInputs(RollerIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, rollerOutput, rollerRPM); + BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.RPM = rollerRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); } @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 db26ebb..54dc59e 100644 --- a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java +++ b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java @@ -239,7 +239,7 @@ public static class Config { 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 + // voltage //Should // normally use VoltageOut public Config(String title, int id, CANBus canbus) { From 7d01c748d05a116b6325e11843e835705b8d156c Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sat, 31 Jan 2026 10:39:32 -0600 Subject: [PATCH 2/8] changes 1/31 pivot code finish, and shooter got modify. --- src/main/java/frc/robot/Constants.java | 152 ------------------ src/main/java/frc/robot/RobotContainer.java | 66 ++++---- .../frc/robot/subsystems/indexer/Indexer.java | 5 +- .../frc/robot/subsystems/intake/Intake.java | 57 +++++++ .../subsystems/intake/IntakeConstants.java | 84 ++++++++++ .../robot/subsystems/intake/pivot/Pivot.java | 36 ----- .../subsystems/intake/pivot/PivotIOSim.java | 14 +- .../intake/pivot/PivotIOSparkFlex.java | 6 +- .../intake/pivot/PivotIOTalonFX.java | 9 +- .../subsystems/intake/roller/Roller.java | 44 ----- .../intake/roller/RollerIOSparkFlex.java | 2 +- .../intake/roller/RollerIOTalonFX.java | 19 +-- .../frc/robot/subsystems/shooter/Shooter.java | 127 +++++++-------- .../subsystems/shooter/ShooterConstants.java | 62 +++++++ .../subsystems/shooter/angler/AnglerIO.java | 19 +++ .../shooter/angler/AnglerIOSim.java | 5 + .../shooter/angler/AnglerIOTalonFX.java | 5 + .../shooter/flywheel/FlywheelIO.java | 19 +++ .../shooter/flywheel/FlywheelIOSim.java | 54 +++++++ .../shooter/flywheel/FlywheelIOTalonFX.java | 63 ++++++++ .../titan/core/subsystem/CTREMechanism.java | 12 +- 21 files changed, 484 insertions(+), 376 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/IntakeConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java delete mode 100644 src/main/java/frc/robot/subsystems/intake/roller/Roller.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4d966c3..bf1df40 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -40,50 +40,6 @@ public static enum Mode { /** Replaying from a log file. */ REPLAY } - - public static class ShooterConstants { - - public enum ShooterState { - SHOOTER, - REVERSE, - IDLE - } - - public static final boolean attached = true; - public static final int id = 50; - public static final boolean inverted = false; - public static final boolean breakType = false; - public static final double gearRatio = 1 / 1; - - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; - // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might Add later - - public static final Current stallLimit = Units.Amps.of(60); - public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 1; - 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 idleSpeed = Units.RPM.of(0); - - public enum ShooterModes { - SHOOTER(shooterSpeed, 1.0), - REVERSE(reverseSpeed, -0.5), - IDLE(idleSpeed, 0.0); - - public AngularVelocity speed; - public double output; - - ShooterModes(AngularVelocity speed, double output) { - this.speed = speed; - this.output = output; - } - } - } - public static class IndexerConstants { public enum IndexerState { @@ -127,113 +83,5 @@ public enum IndexerModes { } } - public static final class IntakeRollerConstants { - public static final boolean attached = true; - public static final boolean useRpm = false; - - 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 double maxIAccum = 0.2; - - public static final double gearRatio = 1 / 1; - - public static final Current forwardTorqueLimit = Units.Amps.of(80); - public static final Current reverseTorqueLimit = Units.Amps.of(80); - - public static final boolean invert = false; - public static final boolean gravityType = false; - public static final boolean breakType = false; - public static final FeedbackSensorSourceValue feedbackSensorCTRE = - FeedbackSensorSourceValue.FusedCANcoder; - public static final FeedbackSensor feedbackSensorREV = - FeedbackSensor.kPrimaryEncoder; - - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; - - public static final boolean useFMaxRotation = true; - public static final boolean useRMaxRotation = true; - public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); - public static final Angle maxFowardRotation = Units.Rotation.of(5); - - public static final boolean useStallLimit = true; - public static final boolean useSupplyLimit = true; - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); - - public static final AngularVelocity rollerIOSpeed = Units.RPM.of(3000); - public static final AngularVelocity outtakeSpeed = Units.RPM.of(0); - public static final AngularVelocity idleSpeed = Units.RPM.of(0); - - public enum IntakeRollerModes { - IDLE(idleSpeed, 0.0), - INTAKE(rollerIOSpeed, 8.4), - OUTTAKE(outtakeSpeed, -4.8); - - public AngularVelocity speed; - // TODO: Make sure voltage is just output * 12 (for 12V) - public double voltage; - - IntakeRollerModes(AngularVelocity speed, double voltage) { - this.speed = speed; - this.voltage = voltage; - } - } - } - - public static class IntakePivotConstants { - 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 double maxIAccum = 0.2; - - public static final double gearRatio = 1 / 1; - - public static final Current forwardTorqueLimit = Units.Amps.of(80); - public static final Current reverseTorqueLimit = Units.Amps.of(80); - - public static final boolean invert = false; - public static final boolean gravityType = false; - public static final boolean breakType = false; - public static final IdleMode idleMode = IdleMode.kBrake; - - public static final FeedbackSensorSourceValue feedbackSensorCTRE = - FeedbackSensorSourceValue.FusedCANcoder; - public static final FeedbackSensor feedbackSensorREV = - FeedbackSensor.kPrimaryEncoder; - - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; - - public static final boolean useFMaxRotation = true; - public static final boolean useRMaxRotation = true; - public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); - public static final Angle maxFowardRotation = Units.Rotation.of(5); - - public static final boolean useStallLimit = true; - public static final boolean useSupplyLimit = true; - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); - - public static final Angle stow = Units.Rotation.of(0); - public static final Angle feed = Units.Rotation.of(0); - public static final Angle eject = Units.Rotation.of(0); - - public enum IntakePivotPositions { - STOW(stow), EJECT(eject), FEED(feed); - public Angle position; - - IntakePivotPositions (Angle position) { - this.position = position; - } - } - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 68a4ba0..f02e68b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,11 +14,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants.IntakePivotConstants.IntakePivotPositions; -import frc.robot.Constants.IntakeRollerConstants.IntakeRollerModes; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drive.Drive; @@ -27,11 +27,11 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.intake.pivot.Pivot; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; import frc.robot.subsystems.intake.pivot.PivotIOSim; import frc.robot.subsystems.intake.pivot.PivotIOSparkFlex; -import frc.robot.subsystems.intake.roller.Roller; import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; @@ -41,6 +41,7 @@ import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import frc.team5431.titan.core.joysticks.CommandXboxController; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -52,9 +53,8 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final Vision vision; - private final Roller roller; - private final Pivot pivot; +// private final Vision vision; + private final Intake intake; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -79,18 +79,13 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackRight)); // Real robot, instantiate hardware IO implementations - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOLimelight(camera0Name, drive::getRotation), - new VisionIOLimelight(camera1Name, drive::getRotation)); - - roller = - new Roller( - new RollerIOSparkFlex()); - - pivot = - new Pivot(new PivotIOSparkFlex()); + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOLimelight(camera0Name, drive::getRotation), + // new VisionIOLimelight(camera1Name, drive::getRotation)); + + intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -125,18 +120,14 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - vision = - new Vision( - drive::addVisionMeasurement, - new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), - new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); - - roller = - new Roller( - new RollerIOSim() {}); + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), + // new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); - pivot = - new Pivot(new PivotIOSim() {}); + intake = + new Intake(new RollerIOSim(), new PivotIOSim()); break; default: // Replayed robot, disable IO implementations @@ -148,10 +139,8 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); - roller = new Roller(new RollerIO() {}); - pivot = new Pivot(new PivotIO() {}); - + // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + intake = new Intake(new RollerIO() {}, new PivotIO() {}); break; } @@ -177,6 +166,10 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); + + + SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); + } /** @@ -236,9 +229,8 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().onTrue(roller.runIntakeCommand(IntakeRollerModes.INTAKE, true)); - operator.b().onTrue(pivot.runPivotCommand(IntakePivotPositions.FEED)); - operator.y().onTrue(pivot.runPivotCommand(IntakePivotPositions.STOW)); + operator.a().onTrue(intake.runIntakeCommand(IntakeMode.STOW)); + operator.b().onTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index fa5be02..b814934 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -23,11 +23,10 @@ public class Indexer extends CTREMechanism { public static class IndexerConfig extends Config { public IndexerConfig() { - super("Indexer", IndexerConstants.id, Constants.CANBUS); + super("Indexer", Constants.CANBUS); configNeutralBrakeMode(IndexerConstants.breakType); - configStatorCurrentLimit(IndexerConstants.stallLimit, true); - configSupplyCurrentLimit(IndexerConstants.supplyLimit, true); + configStatorCurrentLimit(IndexerConstants.stallLimit); configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..5cbaff5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.intake.pivot.PivotIO; +import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; +import frc.robot.subsystems.intake.roller.RollerIO; +import frc.robot.subsystems.intake.roller.RollerIOInputsAutoLogged; + +public class Intake extends SubsystemBase { + private final RollerIO rollerIO; + private final PivotIO pivotIO; + private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); + + private IntakeMode mode; + + + public Intake(RollerIO rollerIO, PivotIO pivotIO) { + this.rollerIO = rollerIO; + this.pivotIO = pivotIO; + this.mode = IntakeMode.STOW; + } + + @Override + public void periodic() { + rollerIO.updateInputs(rollerInputs); + Logger.processInputs("Intake/Roller", rollerInputs); + + pivotIO.updateInputs(pivotInputs); + Logger.processInputs("Intake/Pivot", pivotInputs); + + Logger.recordOutput("Intake/Mode", mode); + } + + public void runRollerEnum(IntakeMode intakeMode) { + this.mode = intakeMode; + rollerIO.setRollerVoltage(mode.voltage.baseUnitMagnitude()); + } + + public void runPivotEnum(IntakeMode intakeMode) { + this.mode = intakeMode; + pivotIO.setPosition(mode.position.magnitude()); + } + + public Command runIntakeCommand(IntakeMode intakeMode) { + return new RunCommand(() -> { + this.runRollerEnum(intakeMode); + this.runPivotEnum(intakeMode); + }, this).withName("Intake.runIntakeEnum"); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..852c6d8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems.intake; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public class IntakeConstants { + + public enum IntakeMode { + STOW(Units.Volts.of(0.0), Units.Rotation.of(0.0)), + INTAKE(Units.Volts.of(8.4), Units.Rotation.of(1.5)), + OUTTAKE(Units.Volts.of(-4.8), Units.Rotation.of(-1.5)); + + public Voltage voltage; + public Angle position; + + IntakeMode(Voltage voltage, Angle position) { + this.voltage = voltage; + this.position = position; + } + } + + public static final class IntakeRollerConstants { + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } + + public static final class IntakePivotConstants { + 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 double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kAbsoluteEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java deleted file mode 100644 index 1a8d90a..0000000 --- a/src/main/java/frc/robot/subsystems/intake/pivot/Pivot.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems.intake.pivot; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.IntakePivotConstants.IntakePivotPositions; - -public class Pivot extends SubsystemBase { - private final PivotIO pivotIO; - private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); - - private IntakePivotPositions mode; - - public Pivot(PivotIO PivotIO) { - this.pivotIO = PivotIO; - this.mode = IntakePivotPositions.STOW; - } - - @Override - public void periodic() { - pivotIO.updateInputs(inputs); - Logger.processInputs("Intake/Pivot", inputs); - Logger.recordOutput("Intake/Pivot/Mode", mode); - } - - public void runEnum(IntakePivotPositions pivotPosition) { - this.mode = pivotPosition; - pivotIO.setPosition(pivotPosition.position.magnitude()); - } - - public Command runPivotCommand(IntakePivotPositions pivotPosition) { - return new RunCommand(() -> this.runEnum(pivotPosition), this).withName("IntakePivot.runEnum"); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java index 154df7d..bcef348 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java @@ -9,7 +9,6 @@ public class PivotIOSim implements PivotIO { private DCMotorSim pivotMotorSim; - private PIDController pivotContpivot = new PIDController(PIVOT_KP, 0, PIVOT_KD); private boolean pivotClosedLoop = false; private double appliedVoltage = 0.0; private double pivotFFVolts = 0.0; @@ -21,21 +20,22 @@ public class PivotIOSim implements PivotIO { private static final double PIVOT_KP = 1.0; private static final double PIVOT_KD = 0.0; + private PIDController pivotController = new PIDController(PIVOT_KP, 0, PIVOT_KD); public PivotIOSim() { this.pivotMotorSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); - // Enable wrapping for turn PID --- TODO: NOT SURE WHAT THIS IS ASK - pivotContpivot.enableContinuousInput(-Math.PI, Math.PI); + // pivotMotorSim.set + pivotController.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(PivotIOInputs inputs) { if (pivotClosedLoop) { - appliedVoltage = pivotFFVolts + pivotContpivot.calculate(pivotMotorSim.getAngularVelocityRadPerSec()); + appliedVoltage = pivotFFVolts + pivotController.calculate(pivotMotorSim.getAngularVelocityRadPerSec()); } else { - pivotContpivot.reset(); + pivotController.reset(); } pivotMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); @@ -48,9 +48,9 @@ public void updateInputs(PivotIOInputs inputs) { } @Override - // TODO public void setPosition(double positionAngle) { - + pivotClosedLoop = true; + pivotController.setSetpoint(positionAngle); } public void setAppliedVoltage(double appliedVoltage) { diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index ba27df7..ab17ef4 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -9,7 +9,7 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; -import frc.robot.Constants.IntakePivotConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class PivotIOSparkFlex implements PivotIO { @@ -21,8 +21,8 @@ public PivotSparkFlexConfig() { super("PivotSparkFlex", IntakePivotConstants.id); configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); - // configGear(PivotConstants.gearRat); - // configGravity(PivotConstants.gravityType); + // configGear(IntakePivotConstants.gearRatio); + // configGravity(IntakePivotConstants.gravityType); configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); configReverseSoftLimit( diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 1feaab9..4e1e5b3 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -12,7 +12,7 @@ import static edu.wpi.first.units.Units.*; import frc.robot.Constants; -import frc.robot.Constants.IntakePivotConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class PivotIOTalonFX implements PivotIO { @@ -20,14 +20,13 @@ public class PivotIOTalonFX implements PivotIO { public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { - super("PivotTalonFX", IntakePivotConstants.id, Constants.CANBUS); + super("PivotTalonFX", Constants.CANBUS); configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); configNeutralBrakeMode(IntakePivotConstants.breakType); configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); configGearRatio(IntakePivotConstants.gearRatio); configGravityType(IntakePivotConstants.gravityType); - configSupplyCurrentLimit(IntakePivotConstants.supplyLimit, IntakePivotConstants.useSupplyLimit); - configStatorCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.useStallLimit); + configSupplyCurrentLimit(IntakePivotConstants.supplyLimit); configReverseSoftLimit( IntakePivotConstants.maxReverseRotation.in(Rotation), IntakePivotConstants.useRMaxRotation); configForwardSoftLimit( @@ -40,7 +39,7 @@ public PivotTalonFXConfig() { private StatusSignal pivotPosition; private StatusSignal currentAmps; - private PivotTalonFXConfig config; + private PivotTalonFXConfig config = new PivotTalonFXConfig(); public PivotIOTalonFX(TalonFX talon) { appliedVoltage = talon.getMotorVoltage(); diff --git a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java deleted file mode 100644 index 47ae70e..0000000 --- a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java +++ /dev/null @@ -1,44 +0,0 @@ -package frc.robot.subsystems.intake.roller; - - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.IntakeRollerConstants.IntakeRollerModes; - -public class Roller extends SubsystemBase { - - private final RollerIO rollerIO; - private final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); - - private IntakeRollerModes mode; - - public Roller(RollerIO rollerIO) { - this.rollerIO = rollerIO; - this.mode = IntakeRollerModes.IDLE; - } - - @Override - public void periodic() { - rollerIO.updateInputs(inputs); - Logger.recordOutput("Intake/Roller/Mode", mode.toString()); - Logger.processInputs("Intake/Roller", inputs); - } - - public void runEnum(IntakeRollerModes rollerIOModes, boolean useRPM) { - this.mode = rollerIOModes; - if (useRPM) { - rollerIO.setRPM(rollerIOModes.speed.magnitude()); - } - else { - rollerIO.setRollerVoltage(rollerIOModes.voltage); - } - } - - public Command runIntakeCommand(IntakeRollerModes rollerIOModes, boolean useRPM) { - return new RunCommand(() -> this.runEnum(rollerIOModes, useRPM), this) - .withName("Intake.runEnum"); - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index b7cd351..ad19054 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -9,7 +9,7 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; -import frc.robot.Constants.IntakeRollerConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index a2ed2b1..55a52e4 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -11,7 +11,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.Constants.IntakeRollerConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class RollerIOTalonFX implements RollerIO { @@ -19,14 +19,13 @@ public class RollerIOTalonFX implements RollerIO { public static class RollerTalonFXConfig extends CTREMechanism.Config { public RollerTalonFXConfig() { - super("RollerTalonFX", IntakeRollerConstants.id, Constants.CANBUS); + super("RollerTalonFX",Constants.CANBUS); configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); configNeutralBrakeMode(IntakeRollerConstants.breakType); configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorCTRE); - configGearRatio(IntakeRollerConstants.gearRatio); - configGravityType(IntakeRollerConstants.gravityType); - configSupplyCurrentLimit(IntakeRollerConstants.supplyLimit, IntakeRollerConstants.useSupplyLimit); - configStatorCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.useStallLimit); + // configGearRatio(IntakeRollerConstants.gearRatio); + // configGravityType(IntakeRollerConstants.gravityType); + configSupplyCurrentLimit(IntakeRollerConstants.supplyLimit); configReverseSoftLimit( IntakeRollerConstants.maxReverseRotation.in(Rotation), IntakeRollerConstants.useRMaxRotation); configForwardSoftLimit( @@ -38,7 +37,7 @@ public RollerTalonFXConfig() { private StatusSignal rollerRPM; private StatusSignal currentAmps; - private RollerTalonFXConfig config; + private RollerTalonFXConfig config = new RollerTalonFXConfig(); public RollerIOTalonFX(TalonFX talon) { appliedVoltage = talon.getMotorVoltage(); @@ -62,10 +61,4 @@ public void updateInputs(RollerIOInputs inputs) { public void setRollerVoltage(double voltage) { talon.setVoltage(voltage); } - - @Override - public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - talon.setControl(output); - } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index da5d41e..9423190 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,70 +1,61 @@ -package frc.robot.subsystems.shooter; - -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.subsystem.CTREMechanism; -import lombok.Getter; -import lombok.Setter; - -public class Shooter extends CTREMechanism { - - @Getter @Setter private ShooterState shooterState; - @Getter @Setter private ShooterModes shooterMode; - private boolean attached; - private TalonFX motor; - - public static class ShooterConfig extends Config { - public ShooterConfig() { - super("Shooter", ShooterConstants.id, Constants.CANBUS); - configNeutralBrakeMode(ShooterConstants.breakType); - configStatorCurrentLimit(ShooterConstants.stallLimit, true); - configSupplyCurrentLimit(ShooterConstants.supplyLimit, true); - configForwardSoftLimit(ShooterConstants.maxForwardOutput, true); - configReverseSoftLimit(ShooterConstants.maxReverseOutput, true); - configPIDGains(ShooterConstants.p, ShooterConstants.i, ShooterConstants.d); - configPeakOutput(ShooterConstants.maxForwardOutput, ShooterConstants.maxReverseOutput); - configGearRatio(ShooterConstants.gearRatio); - configMotorInverted(ShooterConstants.inverted); - } - } - - public Shooter(TalonFX motor, boolean attached, ShooterConfig config) { - super(motor, attached, config); - this.attached = attached; - this.motor = motor; - this.shooterMode = ShooterModes.IDLE; - this.shooterState = ShooterState.IDLE; - - config.applyTalonConfig(motor); - } - - @Override - public void periodic() { - SmartDashboard.putString("Shooter Mode", getShooterMode().toString()); +// package frc.robot.subsystems.shooter; + +// 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.subsystem.CTREMechanism; +// import lombok.Getter; +// import lombok.Setter; + +// public class Shooter extends CTREMechanism { +// private + +// @Getter @Setter private ShooterModes shooterMode; +// private boolean attached; +// private TalonFX motor; + +// public static class ShooterConfig extends Config { +// public ShooterConfig() { + +// } +// } + +// public Shooter(TalonFX motor, boolean attached, ShooterConfig config) { +// super(motor, attached, config); +// this.attached = attached; +// this.motor = motor; +// this.shooterMode = ShooterModes.IDLE; +// this.shooterState = ShooterState.IDLE; + +// config.applyTalonConfig(motor); +// } + +// @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"); - } -} +// // 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"); +// } +// } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java new file mode 100644 index 0000000..17688a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,62 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; + +public class ShooterConstants { + + // public enum ShooterModes { + // SHOOT( Units.RPM.of(2000)), + // IDLE( Units.RPM.of(0)); + + // public AngularVelocity speed; + + // ShooterModes(AngularVelocity speed) { + // this.speed = speed; + // } + // } + + public static class ShooterFlywheelConstants { + public static final boolean attached = true; + public static final int followerId = 50; + public static final int leaderId = 50; + public static final boolean inverted = false; + public static final boolean breakType = false; + public static final double gearRatio = 1 / 1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might + // Add later + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + + } + + public static class ShooterAnglerConstants { + public static final boolean attached = true; + public static final int id = 50; + public static final boolean inverted = false; + public static final boolean breakType = false; + public static final double gearRatio = 1 / 1; + + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might + // Add later + + public static final Current stallLimit = Units.Amps.of(60); + public static final Current supplyLimit = Units.Amps.of(80); + public static final double maxForwardOutput = 1; + public static final double maxReverseOutput = 0.5; + + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java new file mode 100644 index 0000000..3803a35 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter.angler; + +import org.littletonrobotics.junction.AutoLog; + +public interface AnglerIO { + @AutoLog + public static class AnglerIOInputs { + public boolean anglerConnected = false; + public double appliedVoltage = 0.0; + public double positionAngle = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(AnglerIOInputs inputs) {} + + /** Run the motor to the specified rotation per minute. */ + public default void setRPM(double rpm) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java new file mode 100644 index 0000000..47d428d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter.angler; + +public class AnglerIOSim implements AnglerIO { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java new file mode 100644 index 0000000..5181ff7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.shooter.angler; + +public class AnglerIOTalonFX implements AnglerIO { + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java new file mode 100644 index 0000000..9124318 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + @AutoLog + public static class FlywheelIOInputs { + public boolean flywheelConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FlywheelIOInputs inputs) {} + + /** Run the motor to the specified rotation per minute. */ + public default void setRPM(double rpm) {} +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..98d3e43 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.shooter.flywheel; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class FlywheelIOSim implements FlywheelIO { + private DCMotorSim flywheelMotorSim; + private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); + private boolean flywheelClosedLoop = false; + private double appliedVoltage = 0.0; + private double rollerFFVolts = 0.0; + + // From ModuleIOSim no clue tbh + private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); + private static final double ROLLER_KS = 0.0; + private static final double ROLLER_KP = 1.0; + private static final double ROLLER_KD = 0.0; + + + public FlywheelIOSim() { + this.flywheelMotorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + if (flywheelClosedLoop) { + appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); + } else { + flywheelController.reset(); + } + + flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + flywheelMotorSim.update(0.02); + + inputs.flywheelConnected = true; + inputs.RPM = flywheelMotorSim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + } + + @Override + public void setRPM(double RPM) { + flywheelClosedLoop = true; + rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; + flywheelController.setSetpoint(RPM); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java new file mode 100644 index 0000000..858cdac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.shooter.flywheel; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class FlywheelIOTalonFX implements FlywheelIO { + private final TalonFX followerFX = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); + private final TalonFX LeaderFX = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); + + public static class FlywheelTalonFXConfig extends CTREMechanism.Config { + public FlywheelTalonFXConfig() { + super("FlywheelTalonFX", Constants.CANBUS); + configNeutralBrakeMode(ShooterFlywheelConstants.breakType); + configFeedbackSensorSource(ShooterFlywheelConstants.feedbackSensorCTRE); + configNeutralBrakeMode(ShooterFlywheelConstants.breakType); + configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); + configGearRatio(ShooterFlywheelConstants.gearRatio); + configMotorInverted(ShooterFlywheelConstants.inverted); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal rollerRPM; + private StatusSignal currentAmps; + + private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); + + public FlywheelIOTalonFX(TalonFX talon) { + appliedVoltage = talon.getMotorVoltage(); + rollerRPM = talon.getVelocity(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = rollerRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setRPM(double rpm) { + VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); + LeaderFX.setControl(output); + } +} 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 54dc59e..b3a94d5 100644 --- a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java +++ b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java @@ -224,7 +224,6 @@ public void toggleTorqueCurrentLimit(Current enabledLimit, boolean enabled) { public static class Config { public String title; - public int id; public TalonFXConfiguration talonConfig; public double voltageCompSaturation; // 12V by default @@ -242,10 +241,9 @@ public static class Config { // voltage //Should // normally use VoltageOut - public Config(String title, int id, CANBus canbus) { + public Config(String title, CANBus canbus) { this.title = title; this.voltageCompSaturation = 12.0; - this.id = id; talonConfig = new TalonFXConfiguration(); /* Put default config settings for all mechanisms here */ @@ -284,14 +282,14 @@ public void configMotorInverted(boolean isInverted) { } } - public void configSupplyCurrentLimit(Current supplyLimit, boolean enabled) { + public void configSupplyCurrentLimit(Current supplyLimit) { talonConfig.CurrentLimits.SupplyCurrentLimit = supplyLimit.in(Units.Amps); - talonConfig.CurrentLimits.SupplyCurrentLimitEnable = enabled; + talonConfig.CurrentLimits.SupplyCurrentLimitEnable = true; } - public void configStatorCurrentLimit(Current statorLimit, boolean enabled) { + public void configStatorCurrentLimit(Current statorLimit) { talonConfig.CurrentLimits.StatorCurrentLimit = statorLimit.in(Units.Amps); - talonConfig.CurrentLimits.StatorCurrentLimitEnable = enabled; + talonConfig.CurrentLimits.StatorCurrentLimitEnable = true; } public void configForwardTorqueCurrentLimit(Current currentLimit) { From 24b0851b7b11140801ae37370e6b5de5eb392471 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sat, 31 Jan 2026 14:20:40 -0600 Subject: [PATCH 3/8] changes cont. shooter implemnted and magic carpet hopper started --- src/main/java/frc/robot/RobotContainer.java | 20 ++- .../frc/robot/subsystems/hopper/CarpetIO.java | 20 +++ .../robot/subsystems/hopper/CarpetIOSim.java | 5 + .../frc/robot/subsystems/hopper/carpet.java | 5 + .../subsystems/intake/IntakeConstants.java | 6 +- .../subsystems/intake/pivot/PivotIO.java | 2 +- .../subsystems/intake/pivot/PivotIOSim.java | 2 +- .../intake/pivot/PivotIOSparkFlex.java | 2 +- .../intake/pivot/PivotIOTalonFX.java | 4 +- .../intake/roller/RollerIOTalonFX.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 128 +++++++++--------- .../subsystems/shooter/ShooterConstants.java | 2 + .../subsystems/shooter/angler/AnglerIO.java | 5 +- .../shooter/angler/AnglerIOTalonFX.java | 60 +++++++- .../shooter/flywheel/FlywheelIO.java | 10 +- .../shooter/flywheel/FlywheelIOSim.java | 84 ++++++------ .../shooter/flywheel/FlywheelIOTalonFX.java | 51 ++++--- 17 files changed, 267 insertions(+), 143 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/hopper/CarpetIO.java create mode 100644 src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/hopper/carpet.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f02e68b..6bc7faa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,8 +7,6 @@ package frc.robot; -import static frc.robot.subsystems.vision.VisionConstants.*; - import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -35,13 +33,15 @@ import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIOLimelight; -import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.angler.AnglerIO; +import frc.robot.subsystems.shooter.angler.AnglerIOSim; +import frc.robot.subsystems.shooter.angler.AnglerIOTalonFX; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; import frc.team5431.titan.core.joysticks.CommandXboxController; -import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -55,6 +55,7 @@ public class RobotContainer { private final Drive drive; // private final Vision vision; private final Intake intake; + private final Shooter shooter; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -86,6 +87,7 @@ public RobotContainer() { // new VisionIOLimelight(camera1Name, drive::getRotation)); intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); + shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -128,6 +130,8 @@ public RobotContainer() { intake = new Intake(new RollerIOSim(), new PivotIOSim()); + shooter = + new Shooter(new AnglerIOSim(), new FlywheelIOSim()); break; default: // Replayed robot, disable IO implementations @@ -141,6 +145,8 @@ public RobotContainer() { // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); intake = new Intake(new RollerIO() {}, new PivotIO() {}); + shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); + break; } diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java new file mode 100644 index 0000000..7ce81f1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.hopper; + +import org.littletonrobotics.junction.AutoLog; + +public interface CarpetIO { + @AutoLog + public static class CarpetIOInputs { + public boolean rollerConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(CarpetIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setRollerVoltage(double voltage) {} + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java new file mode 100644 index 0000000..57e07d2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.hopper; + +public class CarpetIOSim { + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/carpet.java new file mode 100644 index 0000000..dc4c97e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/carpet.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.hopper; + +public class carpet { + +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 852c6d8..119e79f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -12,9 +12,9 @@ public class IntakeConstants { public enum IntakeMode { - STOW(Units.Volts.of(0.0), Units.Rotation.of(0.0)), - INTAKE(Units.Volts.of(8.4), Units.Rotation.of(1.5)), - OUTTAKE(Units.Volts.of(-4.8), Units.Rotation.of(-1.5)); + STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), + INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0)), + OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(0)); public Voltage voltage; public Angle position; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java index 062c3db..d9d6c6b 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java @@ -8,7 +8,7 @@ public interface PivotIO { public static class PivotIOInputs { public boolean pivotConnected = false; public double appliedVoltage = 0.0; - public double position = 0.0; + public double positionAngle = 0.0; public double currentAmps = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java index bcef348..c88dbfb 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSim.java @@ -42,7 +42,7 @@ public void updateInputs(PivotIOInputs inputs) { pivotMotorSim.update(0.02); inputs.pivotConnected = true; - inputs.position = pivotMotorSim.getAngularPositionRad(); + inputs.positionAngle = pivotMotorSim.getAngularPositionRad(); inputs.appliedVoltage = appliedVoltage; inputs.currentAmps = Math.abs(pivotMotorSim.getCurrentDrawAmps()); } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index ab17ef4..7839dd0 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -38,7 +38,7 @@ public PivotIOSparkFlex() { @Override public void updateInputs(PivotIOInputs inputs) { - ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.position = value); + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 4e1e5b3..56536ef 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -41,7 +41,7 @@ public PivotTalonFXConfig() { private PivotTalonFXConfig config = new PivotTalonFXConfig(); - public PivotIOTalonFX(TalonFX talon) { + public PivotIOTalonFX() { appliedVoltage = talon.getMotorVoltage(); pivotPosition = talon.getPosition(); currentAmps = talon.getStatorCurrent(); @@ -55,7 +55,7 @@ public void updateInputs(PivotIOInputs inputs) { BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - inputs.position = pivotPosition.getValue().in(Rotation); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); inputs.currentAmps = currentAmps.getValueAsDouble(); } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 55a52e4..906f6f3 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -4,9 +4,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -39,7 +37,7 @@ public RollerTalonFXConfig() { private RollerTalonFXConfig config = new RollerTalonFXConfig(); - public RollerIOTalonFX(TalonFX talon) { + public RollerIOTalonFX() { appliedVoltage = talon.getMotorVoltage(); rollerRPM = talon.getVelocity(); currentAmps = talon.getStatorCurrent(); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9423190..01c54b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,61 +1,67 @@ -// package frc.robot.subsystems.shooter; - -// 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.subsystem.CTREMechanism; -// import lombok.Getter; -// import lombok.Setter; - -// public class Shooter extends CTREMechanism { -// private - -// @Getter @Setter private ShooterModes shooterMode; -// private boolean attached; -// private TalonFX motor; - -// public static class ShooterConfig extends Config { -// public ShooterConfig() { - -// } -// } - -// public Shooter(TalonFX motor, boolean attached, ShooterConfig config) { -// super(motor, attached, config); -// this.attached = attached; -// this.motor = motor; -// this.shooterMode = ShooterModes.IDLE; -// this.shooterState = ShooterState.IDLE; - -// config.applyTalonConfig(motor); -// } - -// @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"); -// } -// } +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.Logger; + +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 edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.intake.pivot.PivotIO; +import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; +import frc.robot.subsystems.intake.roller.RollerIO; +import frc.robot.subsystems.intake.roller.RollerIOInputsAutoLogged; +import frc.robot.subsystems.shooter.angler.AnglerIO; +import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO; +import frc.robot.subsystems.shooter.flywheel.FlywheelIO.FlywheelIOInputs; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; +import frc.team5431.titan.core.subsystem.CTREMechanism; +import lombok.Getter; +import lombok.Setter; + +public class Shooter extends SubsystemBase { + private final AnglerIO anglerIO; + private final FlywheelIO flywheelIO; + + private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); + private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + + private IntakeMode mode; + + + public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { + this.anglerIO = anglerIO; + this.flywheelIO = flywheelIO; + } + + @Override + public void periodic() { + anglerIO.updateInputs(anglerInputs); + Logger.processInputs("Shooter/Angler", anglerInputs); + + flywheelIO.updateInputs(flywheelInputs); + Logger.processInputs("Shooter/Flywheel", flywheelInputs); + // Logger.recordOutput("Intake/Mode", mode); + } + + public void runFlywheelEnum(IntakeMode intakeMode) { + this.mode = intakeMode; + flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); + } + + public void runAnglerEnum(IntakeMode intakeMode) { + this.mode = intakeMode; + anglerIO.setPosition(mode.position.magnitude()); + } + + // public Command runIntakeCommand(IntakeMode intakeMode) { + // return new RunCommand(() -> { + // this.runFlywheelEnum(intakeMode); + // this.runAnglerEnum(intakeMode); + // }, this).withName("Shooter.runIntakeEnum"); + // } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 17688a5..28fb363 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -53,6 +53,8 @@ public static class ShooterAnglerConstants { // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); public static final double maxForwardOutput = 1; diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java index 3803a35..d732bc9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -14,6 +14,7 @@ public static class AnglerIOInputs { /** Updates the set of loggable inputs. */ public default void updateInputs(AnglerIOInputs inputs) {} - /** Run the motor to the specified rotation per minute. */ - public default void setRPM(double rpm) {} + /** Run the motor to the specified position. */ + public default void setPosition(double positionAngle) { + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 5181ff7..16632e8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -1,5 +1,63 @@ package frc.robot.subsystems.shooter.angler; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.intake.pivot.PivotIO.PivotIOInputs; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + public class AnglerIOTalonFX implements AnglerIO { + private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANBUS); + + public static class PivotTalonFXConfig extends CTREMechanism.Config { + public PivotTalonFXConfig() { + super("AnglerTalonFX", Constants.CANBUS); + configPIDGains(ShooterAnglerConstants.p, ShooterAnglerConstants.i, ShooterAnglerConstants.d); + configNeutralBrakeMode(ShooterAnglerConstants.breakType); + configFeedbackSensorSource(ShooterAnglerConstants.feedbackSensorCTRE); + configGearRatio(ShooterAnglerConstants.gearRatio); + configSupplyCurrentLimit(ShooterAnglerConstants.supplyLimit); + } + } + -} + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + private PivotTalonFXConfig config = new PivotTalonFXConfig(); + + public AnglerIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + + } + + @Override + public void updateInputs(AnglerIOInputs inputs) { + BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index 9124318..31cee28 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -6,9 +6,13 @@ public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { public boolean flywheelConnected = false; - public double appliedVoltage = 0.0; - public double RPM = 0.0; - public double currentAmps = 0.0; + public double leaderAppliedVoltage = 0.0; + public double leaderRPM = 0.0; + public double leaderAmps = 0.0; + + public double followerAppliedVoltage = 0.0; + public double followerRPM = 0.0; + public double followerAmps = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 98d3e43..c692a8d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -8,47 +8,47 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class FlywheelIOSim implements FlywheelIO { - private DCMotorSim flywheelMotorSim; - private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); - private boolean flywheelClosedLoop = false; - private double appliedVoltage = 0.0; - private double rollerFFVolts = 0.0; - - // From ModuleIOSim no clue tbh - private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); - private static final double ROLLER_KS = 0.0; - private static final double ROLLER_KP = 1.0; - private static final double ROLLER_KD = 0.0; - - - public FlywheelIOSim() { - this.flywheelMotorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + // private DCMotorSim flywheelMotorSim; + // private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); + // private boolean flywheelClosedLoop = false; + // private double appliedVoltage = 0.0; + // private double rollerFFVolts = 0.0; + + // // From ModuleIOSim no clue tbh + // private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + // private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); + // private static final double ROLLER_KS = 0.0; + // private static final double ROLLER_KP = 1.0; + // private static final double ROLLER_KD = 0.0; + + + // public FlywheelIOSim() { + // this.flywheelMotorSim = new DCMotorSim( + // LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - if (flywheelClosedLoop) { - appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); - } else { - flywheelController.reset(); - } - - flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); - flywheelMotorSim.update(0.02); - - inputs.flywheelConnected = true; - inputs.RPM = flywheelMotorSim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); - } - - @Override - public void setRPM(double RPM) { - flywheelClosedLoop = true; - rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; - flywheelController.setSetpoint(RPM); - } + // } + + // @Override + // public void updateInputs(FlywheelIOInputs inputs) { + // if (flywheelClosedLoop) { + // appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); + // } else { + // flywheelController.reset(); + // } + + // flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + // flywheelMotorSim.update(0.02); + + // inputs.flywheelConnected = true; + // inputs.l = flywheelMotorSim.getAngularVelocityRadPerSec(); + // inputs.appliedVoltage = appliedVoltage; + // inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + // } + + // @Override + // public void setRPM(double RPM) { + // flywheelClosedLoop = true; + // rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; + // flywheelController.setSetpoint(RPM); + // } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 858cdac..9461f6d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -4,8 +4,10 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; @@ -16,8 +18,8 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX followerFX = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); - private final TalonFX LeaderFX = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); + private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); + private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); public static class FlywheelTalonFXConfig extends CTREMechanism.Config { public FlywheelTalonFXConfig() { @@ -31,33 +33,50 @@ public FlywheelTalonFXConfig() { } } - private StatusSignal appliedVoltage; - private StatusSignal rollerRPM; - private StatusSignal currentAmps; + private StatusSignal leaderAppliedVoltage; + private StatusSignal leaderFlywheelRPM; + private StatusSignal leaderAmps; + + private StatusSignal followerAppliedVoltage; + private StatusSignal followerFlywheelRPM; + private StatusSignal followerAmps; private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); - public FlywheelIOTalonFX(TalonFX talon) { - appliedVoltage = talon.getMotorVoltage(); - rollerRPM = talon.getVelocity(); - currentAmps = talon.getStatorCurrent(); - config.applyTalonConfig(talon); + public FlywheelIOTalonFX() { + leaderAppliedVoltage = leader.getMotorVoltage(); + leaderFlywheelRPM = leader.getVelocity(); + leaderAmps = leader.getStatorCurrent(); + + followerAppliedVoltage = follower.getMotorVoltage(); + followerFlywheelRPM = follower.getVelocity(); + followerAmps = follower.getStatorCurrent(); + + config.applyTalonConfig(leader); + config.applyTalonConfig(follower); + + // will need to config whether aligned or inverted later + follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Aligned)); - BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); + BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); } @Override public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + + inputs.leaderAppliedVoltage = leaderAppliedVoltage.getValueAsDouble(); + inputs.leaderRPM = leaderFlywheelRPM.getValue().in(RPM); + inputs.leaderAmps = leaderAmps.getValueAsDouble(); - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - inputs.RPM = rollerRPM.getValue().in(RPM); - inputs.currentAmps = currentAmps.getValueAsDouble(); + inputs.followerAppliedVoltage = followerAppliedVoltage.getValueAsDouble(); + inputs.followerRPM = followerFlywheelRPM.getValue().in(RPM); + inputs.followerAmps = followerAmps.getValueAsDouble(); } @Override public void setRPM(double rpm) { VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - LeaderFX.setControl(output); + leader.setControl(output); } } From f6f3bfc967c5c596c091f9d0fd93f6b2ae9a6ab8 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Tue, 3 Feb 2026 19:47:46 -0600 Subject: [PATCH 4/8] Did the mockup of flywheel and of the carpet. Climber TODO --- src/main/java/frc/robot/RobotContainer.java | 34 +++++---- .../frc/robot/subsystems/climber/Climber.java | 5 ++ .../robot/subsystems/climber/ClimberIO.java | 19 +++++ .../subsystems/climber/ClimberIOSim.java | 5 ++ .../climber/ClimberIOSparkFlex.java | 5 ++ .../subsystems/climber/ClimberIOTalonFX.java | 5 ++ .../subsystems/hopper/CarpetConstants.java | 53 ++++++++++++++ .../robot/subsystems/hopper/CarpetIOSim.java | 2 +- .../subsystems/hopper/CarpetIOSparkFlex.java | 50 ++++++++++++++ .../subsystems/hopper/CarpetIOTalonFX.java | 69 +++++++++++++++++++ .../frc/robot/subsystems/hopper/carpet.java | 41 ++++++++++- .../intake/pivot/PivotIOTalonFX.java | 9 ++- .../intake/roller/RollerIOTalonFX.java | 9 ++- .../shooter/angler/AnglerIOTalonFX.java | 9 ++- .../shooter/flywheel/FlywheelIOTalonFX.java | 11 ++- 15 files changed, 305 insertions(+), 21 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIO.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java create mode 100644 src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java create mode 100644 src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java create mode 100644 src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bc7faa..385e98a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetIO; +import frc.robot.subsystems.hopper.CarpetIOSim; +import frc.robot.subsystems.hopper.CarpetIOSparkFlex; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; @@ -56,6 +60,7 @@ public class RobotContainer { // private final Vision vision; private final Intake intake; private final Shooter shooter; + private final Carpet carpet; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -88,6 +93,7 @@ public RobotContainer() { intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); + carpet = new Carpet(new CarpetIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -132,6 +138,8 @@ public RobotContainer() { new Intake(new RollerIOSim(), new PivotIOSim()); shooter = new Shooter(new AnglerIOSim(), new FlywheelIOSim()); + carpet = + new Carpet(new CarpetIOSim()); break; default: // Replayed robot, disable IO implementations @@ -146,7 +154,7 @@ public RobotContainer() { // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); intake = new Intake(new RollerIO() {}, new PivotIO() {}); shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); - + carpet = new Carpet(new CarpetIO() {}); break; } @@ -193,18 +201,18 @@ private void configureDriverBindings() { () -> -driver.getLeftX(), () -> -driver.getRightX())); - // Lock to 0° when A button is held - driver - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> Rotation2d.kZero)); - - // Switch to X pattern when X button is pressed - driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + // // Lock to 0° when A button is held + // driver + // .a() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, + // () -> -driver.getLeftY(), + // () -> -driver.getLeftX(), + // () -> Rotation2d.kZero)); + + // // Switch to X pattern when X button is pressed + // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed driver diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..0aa8bef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class Climber { + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..d2fdc26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + public boolean climberConnected = false; + public double appliedVoltage = 0.0; + public double positionAngle = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(ClimberIOInputs inputs) {} + + /** Run the motor to the specified position. */ + public default void setClimberPosition(double positionAngle) {} +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..266e503 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class ClimberIOSim implements ClimberIO { + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java new file mode 100644 index 0000000..3ad82c1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class ClimberIOSparkFlex implements ClimberIO { + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java new file mode 100644 index 0000000..486a69b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.climber; + +public class ClimberIOTalonFX implements ClimberIO { + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java new file mode 100644 index 0000000..e8b6f20 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.hopper; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public class CarpetConstants { + public enum CarpetModes { + IDLE(Units.Volts.of(0.0)), + INTAKE(Units.Volts.of(8.4)); + + public Voltage voltage; + + CarpetModes(Voltage voltage) { + this.voltage = voltage; + } + } + + public static final class CarpetRollerConstants { + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + } + +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java index 57e07d2..98a07c8 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java @@ -1,5 +1,5 @@ package frc.robot.subsystems.hopper; -public class CarpetIOSim { +public class CarpetIOSim implements CarpetIO { } diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java new file mode 100644 index 0000000..2fe55ba --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -0,0 +1,50 @@ +package frc.robot.subsystems.hopper; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; + +import static frc.robot.util.SparkUtil.*; + +import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class CarpetIOSparkFlex extends CarpetIOTalonFX { + private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class CarpetIOSparkFlexConfig extends REVMechanism.Config { + public CarpetIOSparkFlexConfig() { + super("CarpetSparkFlex", CarpetRollerConstants.id); + configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); + configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorREV); + // configGear(CarpetIOConstants.gearRatio); + // configGravity(CarpetIOConstants.gravityType); + configSmartCurrentLimit(CarpetRollerConstants.stallLimit, CarpetRollerConstants.supplyLimit); + configSmartStallCurrentLimit(CarpetRollerConstants.stallLimit); + configReverseSoftLimit( + CarpetRollerConstants.maxReverseRotation, CarpetRollerConstants.useRMaxRotation); + configForwardSoftLimit( + CarpetRollerConstants.maxFowardRotation, CarpetRollerConstants.useFMaxRotation); + } + } + + public CarpetIOSparkFlex() { + sparkFlex.configure(new CarpetIOSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(CarpetIOInputs inputs) { + ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setRollerVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java new file mode 100644 index 0000000..fe2c65a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.hopper; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class CarpetIOTalonFX implements CarpetIO { + private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.CANBUS); + + public static class CarpetIOTalonFXConfig extends CTREMechanism.Config { + public CarpetIOTalonFXConfig() { + super("RollerTalonFX",Constants.CANBUS); + configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); + configNeutralBrakeMode(CarpetRollerConstants.breakType); + configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorCTRE); + // configGearRatio(CarpetRoller.gearRatio); + // configGravityType(CarpetRoller.gravityType); + configSupplyCurrentLimit(CarpetRollerConstants.supplyLimit); + configReverseSoftLimit( + CarpetRollerConstants.maxReverseRotation.in(Rotation), CarpetRollerConstants.useRMaxRotation); + configForwardSoftLimit( + CarpetRollerConstants.maxFowardRotation.in(Rotation), CarpetRollerConstants.useFMaxRotation); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal rollerRPM; + private StatusSignal currentAmps; + + // No clue what this means copied from ModuleIO + private final Debouncer carpetConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private CarpetIOTalonFXConfig config = new CarpetIOTalonFXConfig(); + + public CarpetIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + rollerRPM = talon.getVelocity(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, rollerRPM); + } + + @Override + public void updateInputs(CarpetIOInputs inputs) { + var carpetStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + + inputs.rollerConnected = carpetConnectedDebounce.calculate(carpetStatus.isOK()); + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = rollerRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setRollerVoltage(double voltage) { + talon.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/carpet.java index dc4c97e..c79ffcd 100644 --- a/src/main/java/frc/robot/subsystems/hopper/carpet.java +++ b/src/main/java/frc/robot/subsystems/hopper/carpet.java @@ -1,5 +1,42 @@ package frc.robot.subsystems.hopper; -public class carpet { - +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.hopper.CarpetIO.CarpetIOInputs; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; + +public class Carpet extends SubsystemBase { + private final CarpetIO carpetIO; + private final CarpetIOInputsAutoLogged carpetInputs = new CarpetIOInputsAutoLogged(); + + private CarpetModes mode; + + public Carpet(CarpetIO carpetIO) { + this.carpetIO = carpetIO; + this.mode = CarpetModes.IDLE; + } + + @Override + public void periodic() { + carpetIO.updateInputs(carpetInputs); + Logger.processInputs("Carpet/", carpetInputs); + + Logger.recordOutput("Carpet/Mode", mode); + } + + public void runRollerEnum(CarpetModes carpetMode) { + this.mode = carpetMode; + carpetIO.setRollerVoltage(mode.voltage.baseUnitMagnitude()); + } + + public Command runCarpetCommand(CarpetModes carpetMode) { + return new RunCommand(() -> { + this.runRollerEnum(carpetMode); + }, this).withName("Carpet.runCarpetEnum"); + } + } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 56536ef..132efab 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -5,6 +5,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -39,6 +40,10 @@ public PivotTalonFXConfig() { private StatusSignal pivotPosition; private StatusSignal currentAmps; + // No clue what this means copied from ModuleIO + private final Debouncer pivotConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private PivotTalonFXConfig config = new PivotTalonFXConfig(); public PivotIOTalonFX() { @@ -52,8 +57,10 @@ public PivotIOTalonFX() { @Override public void updateInputs(PivotIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + var pivotStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + inputs.pivotConnected = pivotConnectedDebounce.calculate(pivotStatus.isOK()); + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.positionAngle = pivotPosition.getValue().in(Rotation); inputs.currentAmps = currentAmps.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 906f6f3..2cf47f2 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -5,6 +5,8 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -35,6 +37,10 @@ public RollerTalonFXConfig() { private StatusSignal rollerRPM; private StatusSignal currentAmps; + // No clue what this means copied from ModuleIO + private final Debouncer rollerConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private RollerTalonFXConfig config = new RollerTalonFXConfig(); public RollerIOTalonFX() { @@ -48,8 +54,9 @@ public RollerIOTalonFX() { @Override public void updateInputs(RollerIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + var rollerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, rollerRPM); + inputs.rollerConnected = rollerConnectedDebounce.calculate(rollerStatus.isOK()); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.RPM = rollerRPM.getValue().in(RPM); inputs.currentAmps = currentAmps.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 16632e8..2d1508d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; @@ -34,6 +35,10 @@ public PivotTalonFXConfig() { private StatusSignal pivotPosition; private StatusSignal currentAmps; + // No clue stole from ModuleIO + private final Debouncer anglerConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private PivotTalonFXConfig config = new PivotTalonFXConfig(); public AnglerIOTalonFX() { @@ -48,7 +53,9 @@ public AnglerIOTalonFX() { @Override public void updateInputs(AnglerIOInputs inputs) { - BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.anglerConnected = anglerConnectedDebounce.calculate(anglerStatus.isOK()); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.positionAngle = pivotPosition.getValue().in(Rotation); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 9461f6d..d37be19 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -9,6 +9,7 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -41,7 +42,11 @@ public FlywheelTalonFXConfig() { private StatusSignal followerFlywheelRPM; private StatusSignal followerAmps; - private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); + // No clue stole from ModuleIO + private final Debouncer flywheelConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); public FlywheelIOTalonFX() { leaderAppliedVoltage = leader.getMotorVoltage(); @@ -63,7 +68,9 @@ public FlywheelIOTalonFX() { @Override public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + + inputs.flywheelConnected = flywheelConnectedDebounce.calculate(flywheelStatus.isOK()); inputs.leaderAppliedVoltage = leaderAppliedVoltage.getValueAsDouble(); inputs.leaderRPM = leaderFlywheelRPM.getValue().in(RPM); From 3df6a5bbd3e68cbbec098843e23106d76702b257 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Wed, 4 Feb 2026 19:49:31 -0600 Subject: [PATCH 5/8] Climber but needs to have a softlimit as well as angler needs a softlimit. --- src/main/java/frc/robot/RobotContainer.java | 9 +++ .../frc/robot/subsystems/climber/Climber.java | 39 +++++++++++- .../subsystems/climber/ClimberConstants.java | 51 +++++++++++++++ .../climber/ClimberIOSparkFlex.java | 47 +++++++++++++- .../subsystems/climber/ClimberIOTalonFX.java | 63 +++++++++++++++++++ .../subsystems/hopper/CarpetIOSparkFlex.java | 4 +- .../intake/pivot/PivotIOSparkFlex.java | 2 +- .../intake/pivot/PivotIOTalonFX.java | 2 +- .../intake/roller/RollerIOSparkFlex.java | 2 +- 9 files changed, 210 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 385e98a..e02ce03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,10 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.climber.Climber; +import frc.robot.subsystems.climber.ClimberIO; +import frc.robot.subsystems.climber.ClimberIOSim; +import frc.robot.subsystems.climber.ClimberIOSparkFlex; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -61,6 +65,7 @@ public class RobotContainer { private final Intake intake; private final Shooter shooter; private final Carpet carpet; + private final Climber climber; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -94,6 +99,7 @@ public RobotContainer() { intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); + climber = new Climber(new ClimberIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -140,6 +146,8 @@ public RobotContainer() { new Shooter(new AnglerIOSim(), new FlywheelIOSim()); carpet = new Carpet(new CarpetIOSim()); + climber = + new Climber(new ClimberIOSim()); break; default: // Replayed robot, disable IO implementations @@ -155,6 +163,7 @@ public RobotContainer() { intake = new Intake(new RollerIO() {}, new PivotIO() {}); shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); carpet = new Carpet(new CarpetIO() {}); + climber = new Climber(new ClimberIO() {}); break; } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 0aa8bef..ea6267c 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,5 +1,40 @@ package frc.robot.subsystems.climber; -public class Climber { - +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.climber.ClimberConstants.ClimberModes; + +public class Climber extends SubsystemBase { + private final ClimberIO climberIO; + private final ClimberIOInputsAutoLogged carpetInputs = new ClimberIOInputsAutoLogged(); + + private ClimberModes mode; + + public Climber(ClimberIO climberIO) { + this.climberIO = climberIO; + this.mode = ClimberModes.STOW; + } + + @Override + public void periodic() { + climberIO.updateInputs(carpetInputs); + Logger.processInputs("Climber/", carpetInputs); + + Logger.recordOutput("Climber/Mode", mode); + } + + public void runClimberEnum(ClimberModes climberMode) { + this.mode = climberMode; + climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); + } + + public Command runCarpetCommand(ClimberModes climberModes) { + return new RunCommand(() -> { + this.runClimberEnum(climberModes); + }, this).withName("Carpet.runCarpetEnum"); + } + } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..1a60361 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.pathplanner.lib.config.PIDConstants; +import com.revrobotics.spark.FeedbackSensor; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; + +public class ClimberConstants { + public enum ClimberModes { + STOW(Units.Rotation.of(0.0)), + CLIMB(Units.Rotation.of(5)); + + public Angle positionAngle; + + ClimberModes(Angle positionAngle) { + this.positionAngle = positionAngle; + } + } + + public static final boolean attached = true; + + public static final int id = -1; + + PIDConstants pidConstants = new PIDConstants(1, 0, 0); + public static final double p = 1; + public static final double i = 0; + public static final double d = 0; + public static final double maxIAccum = 0.2; + + public static final double gearRatio = 1 / 1; + + public static final boolean invert = false; + public static final boolean gravityType = false; + public static final boolean breakType = false; + + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kPrimaryEncoder; + + public static final boolean useFMaxRotation = true; + public static final boolean useRMaxRotation = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(5); + + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); + + +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java index 3ad82c1..2de40a0 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -1,5 +1,50 @@ package frc.robot.subsystems.climber; +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkFlex; + +import static frc.robot.util.SparkUtil.*; + +import frc.team5431.titan.core.subsystem.REVMechanism; + public class ClimberIOSparkFlex implements ClimberIO { - + private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, null); + private final RelativeEncoder encoder = sparkFlex.getEncoder(); + + public static class PivotSparkFlexConfig extends REVMechanism.Config { + public PivotSparkFlexConfig() { + super("PivotSparkFlex", ClimberConstants.id); + configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); + configFeedbackSensorSource(ClimberConstants.feedbackSensorREV); + // configGear(ClimberConstants.gearRatio); + // configGravity(ClimberConstants.gravityType); + configSmartCurrentLimit(ClimberConstants.stallLimit, ClimberConstants.supplyLimit); + configSmartStallCurrentLimit(ClimberConstants.stallLimit); + configReverseSoftLimit( + ClimberConstants.maxReverseRotation, ClimberConstants.useRMaxRotation); + configForwardSoftLimit( + ClimberConstants.maxFowardRotation, ClimberConstants.useFMaxRotation); + } + } + + public ClimberIOSparkFlex() { + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setClimberPosition(double positionAngle) { + sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, + ClosedLoopSlot.kSlot0); + } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java index 486a69b..1dd4b97 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -1,5 +1,68 @@ package frc.robot.subsystems.climber; +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + public class ClimberIOTalonFX implements ClimberIO { + private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANBUS); + + public static class ClimberTalonFXConfig extends CTREMechanism.Config { + public ClimberTalonFXConfig() { + super("ClimberTalonFX", Constants.CANBUS); + configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); + configNeutralBrakeMode(ClimberConstants.breakType); + configFeedbackSensorSource(ClimberConstants.feedbackSensorCTRE); + configGearRatio(ClimberConstants.gearRatio); + configSupplyCurrentLimit(ClimberConstants.supplyLimit); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal pivotPosition; + private StatusSignal currentAmps; + + // No clue stole from ModuleIO + private final Debouncer climberConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private ClimberTalonFXConfig config = new ClimberTalonFXConfig(); + + public ClimberIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + var climberStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.climberConnected = climberConnectedDebounce.calculate(climberStatus.isOK()); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.positionAngle = pivotPosition.getValue().in(Rotation); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setClimberPosition(double positionAngle) { + PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + talon.setControl(mm); + } } diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java index 2fe55ba..9e8a5c3 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -3,8 +3,6 @@ import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; import static frc.robot.util.SparkUtil.*; @@ -13,7 +11,7 @@ import frc.team5431.titan.core.subsystem.REVMechanism; public class CarpetIOSparkFlex extends CarpetIOTalonFX { - private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, null); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class CarpetIOSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index 7839dd0..5e7485a 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -13,7 +13,7 @@ import frc.team5431.titan.core.subsystem.REVMechanism; public class PivotIOSparkFlex implements PivotIO { - private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, null); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class PivotSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 132efab..92baf0b 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -17,7 +17,7 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class PivotIOTalonFX implements PivotIO { - private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index ad19054..0124b4e 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -13,7 +13,7 @@ import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { - private final SparkFlex sparkFlex = new SparkFlex(0, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, null); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class RollerIOSparkFlexConfig extends REVMechanism.Config { public RollerIOSparkFlexConfig() { From 775b669996aad93f7a92621972b994464c3d723b Mon Sep 17 00:00:00 2001 From: AlbedoLinguine Date: Fri, 6 Feb 2026 19:01:46 -0600 Subject: [PATCH 6/8] feeder done except feederIO --- src/main/java/frc/robot/Constants.java | 50 ------------- .../frc/robot/subsystems/drive/Module.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 65 +++++++++++++++++ .../subsystems/feeder/FeederConstants.java | 48 +++++++++++++ .../frc/robot/subsystems/feeder/FeederIO.java | 23 ++++++ .../robot/subsystems/feeder/FeederIOSim.java | 5 ++ .../subsystems/feeder/FeederIOSparkFlex.java | 42 +++++++++++ .../subsystems/feeder/FeederIOTalonFX.java | 72 +++++++++++++++++++ .../frc/robot/subsystems/indexer/Indexer.java | 71 ------------------ 9 files changed, 256 insertions(+), 122 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/feeder/Feeder.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederConstants.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIO.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java create mode 100644 src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java delete 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 bf1df40..16a2bd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,14 +8,6 @@ package frc.robot; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.revrobotics.spark.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import edu.wpi.first.units.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.RobotBase; /** @@ -40,48 +32,6 @@ public static enum Mode { /** Replaying from a log file. */ REPLAY } - public static class IndexerConstants { - - 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; - } - } - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 8f9781f..9993e78 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -20,7 +20,7 @@ public class Module { private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + // private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java new file mode 100644 index 0000000..43d6c7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -0,0 +1,65 @@ +package frc.robot.subsystems.feeder; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.feeder.FeederConstants.FeederState; +import frc.robot.subsystems.feeder.FeederIO.FeederIOInputs; +import lombok.Getter; +import lombok.Setter; + +public class Feeder extends SubsystemBase { + + private final FeederIO feederIO; + private final FeederIOInputs feederInputs = new FeederIOInputs(); + + @Getter @Setter private FeederModes feederMode; + @Getter @Setter private FeederState feederState; + + public Feeder(FeederIO feederIO) { + this.feederIO = feederIO; + this.feederMode = FeederModes.IDLE; + this.feederState = FeederState.IDLE; + } + + @Override + public void periodic() { + feederIO.updateInputs(feederInputs); + // TODO: Add Logger.processInputs once auto-logged classes are generated + // Logger.processInputs("Feeder", feederInputs); + + SmartDashboard.putString("Feeder Mode", feederMode.toString()); + SmartDashboard.putBoolean("Feeder Connected", feederInputs.feederConnected); + SmartDashboard.putNumber("Feeder Voltage", feederInputs.appliedVoltage); + SmartDashboard.putNumber("Feeder RPM", feederInputs.RPM); + SmartDashboard.putNumber("Feeder Current", feederInputs.currentAmps); + + Logger.recordOutput("Feeder/Mode", feederMode); + Logger.recordOutput("Feeder/State", feederState); + + switch (this.feederMode) { + case IDLE: + this.feederState = FeederState.IDLE; + break; + case FEEDER: + this.feederState = FeederState.FEEDER; + break; + case REVERSE: + this.feederState = FeederState.REVERSE; + break; + } + } + + public void runFeederEnum(FeederModes feederMode) { + this.feederMode = feederMode; + feederIO.setPercentOutput(feederMode.output); + } + + public Command runFeederCommand(FeederModes feederMode) { + return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum"); + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java new file mode 100644 index 0000000..7b8d5e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.feeder; + +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; + +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Current; + +public class FeederConstants { + + public enum FeederState { + FEEDER, + 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 FeederSpeed = 0.5; + public static final double reverseSpeed = -0.5; + public static final double idleSpeed = 0.0; + + public enum FeederModes { + FEEDER(FeederSpeed), + REVERSE(reverseSpeed), + IDLE(idleSpeed); + + public double output; + + FeederModes(double output) { + this.output = output; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java new file mode 100644 index 0000000..ab06013 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.feeder; + +import org.littletonrobotics.junction.AutoLog; + +public interface FeederIO { + @AutoLog + public static class FeederIOInputs { + public boolean feederConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + public double currentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(FeederIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setFeederVoltage(double voltage) {} + + /** Run the motor at the specified percent output. */ + public default void setPercentOutput(double percent) {} + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java new file mode 100644 index 0000000..04992e1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.feeder; + +public class FeederIOSim implements FeederIO { + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java new file mode 100644 index 0000000..c9d6dac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.feeder; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import static frc.robot.util.SparkUtil.*; + + +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class FeederIOSparkFlex implements FeederIO { + private final SparkFlex sparkFlex = new SparkFlex(FeederConstants.id, MotorType.kBrushless); + + + public static class FeederSparkFlexConfig extends REVMechanism.Config { + public FeederSparkFlexConfig() { + super("PivotSparkFlex", FeederConstants.id); + configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); + configSmartCurrentLimit(FeederConstants.stallLimit, FeederConstants.supplyLimit); + configSmartStallCurrentLimit(FeederConstants.stallLimit); + } + } + + public FeederIOSparkFlex() { + + sparkFlex.configure(new FeederSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + } + + @Override + public void setFeederVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } + +} diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java new file mode 100644 index 0000000..dc90de4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -0,0 +1,72 @@ +package frc.robot.subsystems.feeder; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class FeederIOTalonFX implements FeederIO { + private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.CANBUS); + + public static class FeederIOTalonFXConfig extends CTREMechanism.Config { + public FeederIOTalonFXConfig() { + super("Feeder", Constants.CANBUS); + + configNeutralBrakeMode(FeederConstants.breakType); + configStatorCurrentLimit(FeederConstants.stallLimit); + configSupplyCurrentLimit(FeederConstants.supplyLimit); + configForwardSoftLimit(FeederConstants.maxForwardOutput, true); + configReverseSoftLimit(FeederConstants.maxReverseOutput, true); + configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); + configPeakOutput(FeederConstants.maxForwardOutput, FeederConstants.maxReverseOutput); + configGearRatio(FeederConstants.gearRatio); + configMotorInverted(FeederConstants.invert); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal feederRPM; + private StatusSignal currentAmps; + + private final Debouncer feederConnectedDebounce = + new Debouncer(0.5, Debouncer.DebounceType.kFalling); + + private FeederIOTalonFXConfig config = new FeederIOTalonFXConfig(); + + public FeederIOTalonFX() { + appliedVoltage = talon.getMotorVoltage(); + feederRPM = talon.getVelocity(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, feederRPM); + } + + @Override + public void updateInputs(FeederIOInputs inputs) { + var feederStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, feederRPM); + + inputs.feederConnected = feederConnectedDebounce.calculate(feederStatus.isOK()); + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = feederRPM.getValue().in(RPM); + inputs.currentAmps = currentAmps.getValueAsDouble(); + } + + @Override + public void setFeederVoltage(double voltage) { + talon.setVoltage(voltage); + } + + @Override + public void setPercentOutput(double percent) { + talon.set(percent); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java deleted file mode 100644 index b814934..0000000 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ /dev/null @@ -1,71 +0,0 @@ -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.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", Constants.CANBUS); - - configNeutralBrakeMode(IndexerConstants.breakType); - configStatorCurrentLimit(IndexerConstants.stallLimit); - configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); - configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); - configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); - configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); - configGearRatio(IndexerConstants.gearRatio); - configMotorInverted(IndexerConstants.invert); - } - } - - public Indexer(TalonFX motor, boolean attached, IndexerConfig config) { - super(motor, attached, config); - 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"); - } -} From 1d7cb915ca5b6e31be1fe14c9a80f727f9699369 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sat, 7 Feb 2026 11:22:45 -0600 Subject: [PATCH 7/8] Simed the rollers for intake, saw values move!!! Finsihed other misc, made some buttons ontrue -Will need to go and make default commands for all the subsystems -Figure out visualization --- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../subsystems/climber/ClimberConstants.java | 2 +- .../subsystems/hopper/CarpetIOSparkFlex.java | 4 --- .../subsystems/hopper/CarpetIOTalonFX.java | 4 --- .../frc/robot/subsystems/indexer/Indexer.java | 3 -- .../frc/robot/subsystems/intake/Intake.java | 35 +++++++++++-------- .../subsystems/intake/IntakeConstants.java | 29 +++++++++++++-- .../subsystems/intake/roller/RollerIOSim.java | 6 ++-- .../intake/roller/RollerIOSparkFlex.java | 6 ++-- .../intake/roller/RollerIOTalonFX.java | 4 --- .../subsystems/shooter/ShooterConstants.java | 10 ++++-- .../shooter/angler/AnglerIOTalonFX.java | 1 + 12 files changed, 64 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e02ce03..d87da8a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -252,8 +252,8 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().onTrue(intake.runIntakeCommand(IntakeMode.STOW)); - operator.b().onTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.STOW)); + operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index 1a60361..f3b35d3 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -30,7 +30,7 @@ public enum ClimberModes { public static final double d = 0; public static final double maxIAccum = 0.2; - public static final double gearRatio = 1 / 1; + public static final double gearRatio = (42.0 / 11.0) * (42.0 / 18.0); public static final boolean invert = false; public static final boolean gravityType = false; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java index 9e8a5c3..a6265a1 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -23,10 +23,6 @@ public CarpetIOSparkFlexConfig() { // configGravity(CarpetIOConstants.gravityType); configSmartCurrentLimit(CarpetRollerConstants.stallLimit, CarpetRollerConstants.supplyLimit); configSmartStallCurrentLimit(CarpetRollerConstants.stallLimit); - configReverseSoftLimit( - CarpetRollerConstants.maxReverseRotation, CarpetRollerConstants.useRMaxRotation); - configForwardSoftLimit( - CarpetRollerConstants.maxFowardRotation, CarpetRollerConstants.useFMaxRotation); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java index fe2c65a..563ba28 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -26,10 +26,6 @@ public CarpetIOTalonFXConfig() { // configGearRatio(CarpetRoller.gearRatio); // configGravityType(CarpetRoller.gravityType); configSupplyCurrentLimit(CarpetRollerConstants.supplyLimit); - configReverseSoftLimit( - CarpetRollerConstants.maxReverseRotation.in(Rotation), CarpetRollerConstants.useRMaxRotation); - configForwardSoftLimit( - CarpetRollerConstants.maxFowardRotation.in(Rotation), CarpetRollerConstants.useFMaxRotation); } } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index b814934..168070e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -24,11 +24,8 @@ public class Indexer extends CTREMechanism { public static class IndexerConfig extends Config { public IndexerConfig() { super("Indexer", Constants.CANBUS); - configNeutralBrakeMode(IndexerConstants.breakType); configStatorCurrentLimit(IndexerConstants.stallLimit); - configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); - configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); configGearRatio(IndexerConstants.gearRatio); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 5cbaff5..774c300 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -7,6 +7,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants.IntakePivotModes; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants.IntakeRollerModes; import frc.robot.subsystems.intake.pivot.PivotIO; import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; import frc.robot.subsystems.intake.roller.RollerIO; @@ -18,40 +21,42 @@ public class Intake extends SubsystemBase { private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); - private IntakeMode mode; + private IntakeRollerModes rollerMode; + private IntakePivotModes pivotMode; public Intake(RollerIO rollerIO, PivotIO pivotIO) { this.rollerIO = rollerIO; this.pivotIO = pivotIO; - this.mode = IntakeMode.STOW; + this.rollerMode = IntakeRollerModes.IDLE; + this.pivotMode = IntakePivotModes.STOW; } @Override public void periodic() { rollerIO.updateInputs(rollerInputs); Logger.processInputs("Intake/Roller", rollerInputs); - + Logger.recordOutput("Intake/Roller/Mode", rollerMode); + pivotIO.updateInputs(pivotInputs); Logger.processInputs("Intake/Pivot", pivotInputs); - - Logger.recordOutput("Intake/Mode", mode); + Logger.recordOutput("Intake/Pivot/Mode", pivotMode); } - public void runRollerEnum(IntakeMode intakeMode) { - this.mode = intakeMode; - rollerIO.setRollerVoltage(mode.voltage.baseUnitMagnitude()); + public void runRollerEnum(IntakeRollerModes rollerMode) { + this.rollerMode = rollerMode; + rollerIO.setRollerVoltage(rollerMode.voltage.baseUnitMagnitude()); } - public void runPivotEnum(IntakeMode intakeMode) { - this.mode = intakeMode; - pivotIO.setPosition(mode.position.magnitude()); + public void runPivotEnum(IntakePivotModes pivotMode) { + this.pivotMode = pivotMode; + pivotIO.setPosition(pivotMode.position.magnitude()); } - public Command runIntakeCommand(IntakeMode intakeMode) { + public Command runIntakeCommand(IntakePivotModes pivotMode, IntakeRollerModes rollerMode) { return new RunCommand(() -> { - this.runRollerEnum(intakeMode); - this.runPivotEnum(intakeMode); - }, this).withName("Intake.runIntakeEnum"); + this.runRollerEnum(rollerMode); + this.runPivotEnum(pivotMode); + }, this).withName("Intake.runIntakeEnum" + rollerMode.toString()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 119e79f..d7f636d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -13,8 +13,8 @@ public class IntakeConstants { public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), - INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0)), - OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(0)); + INTAKE(Units.Volts.of(8.4), Units.Degrees.of(300)), + OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(300)); public Voltage voltage; public Angle position; @@ -26,6 +26,19 @@ public enum IntakeMode { } public static final class IntakeRollerConstants { + + public enum IntakeRollerModes { + IDLE(Units.Volts.of(0.0)), + INTAKE(Units.Volts.of(8.4)), + OUTTAKE(Units.Volts.of(-4.8)); + + public Voltage voltage; + + IntakeRollerModes(Voltage voltage) { + this.voltage = voltage; + } + } + public static final boolean attached = true; public static final int id = -1; @@ -55,6 +68,18 @@ public static final class IntakeRollerConstants { } public static final class IntakePivotConstants { + + public enum IntakePivotModes { + STOW(Units.Degrees.of(0.0)), + INTAKE(Units.Degrees.of(300)); + + public Angle position; + + IntakePivotModes(Angle position) { + this.position = position; + } + } + public static final boolean attached = true; public static final int id = -1; diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java index 0b3ee94..fa4f894 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java @@ -45,6 +45,7 @@ public void updateInputs(RollerIOInputs inputs) { inputs.RPM = rollerMotorSim.getAngularVelocityRadPerSec(); inputs.appliedVoltage = appliedVoltage; inputs.currentAmps = Math.abs(rollerMotorSim.getCurrentDrawAmps()); + } @Override @@ -54,7 +55,8 @@ public void setRPM(double RPM) { rollerController.setSetpoint(RPM); } - public void setAppliedVoltage(double appliedVoltage) { - this.appliedVoltage = MathUtil.clamp(appliedVoltage, -12.0, 12.0); + @Override + public void setRollerVoltage(double voltage) { + this.appliedVoltage = MathUtil.clamp(voltage, -12.0, 12.0); } } diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index 0124b4e..488d0a8 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -24,10 +24,6 @@ public RollerIOSparkFlexConfig() { // configGravity(RollerIOConstants.gravityType); configSmartCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.supplyLimit); configSmartStallCurrentLimit(IntakeRollerConstants.stallLimit); - configReverseSoftLimit( - IntakeRollerConstants.maxReverseRotation, IntakeRollerConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerConstants.maxFowardRotation, IntakeRollerConstants.useFMaxRotation); } } @@ -40,6 +36,8 @@ public void updateInputs(RollerIOInputs inputs) { ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); + + // figure out } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 2cf47f2..53b06b2 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -26,10 +26,6 @@ public RollerTalonFXConfig() { // configGearRatio(IntakeRollerConstants.gearRatio); // configGravityType(IntakeRollerConstants.gravityType); configSupplyCurrentLimit(IntakeRollerConstants.supplyLimit); - configReverseSoftLimit( - IntakeRollerConstants.maxReverseRotation.in(Rotation), IntakeRollerConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerConstants.maxFowardRotation.in(Rotation), IntakeRollerConstants.useFMaxRotation); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 28fb363..72be9c4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -45,7 +46,7 @@ public static class ShooterAnglerConstants { public static final int id = 50; public static final boolean inverted = false; public static final boolean breakType = false; - public static final double gearRatio = 1 / 1; + public static final double gearRatio = 3.0 / 1; public static final double p = 1; public static final double i = 0; @@ -57,8 +58,11 @@ public static class ShooterAnglerConstants { public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 1; - public static final double maxReverseOutput = 0.5; + // public static final double maxForwardOutput = 1; + // public static final double maxReverseOutput = 0.5; + + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); + public static final Angle maxFowardRotation = Units.Rotation.of(2); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 2d1508d..37cc884 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -27,6 +27,7 @@ public PivotTalonFXConfig() { configFeedbackSensorSource(ShooterAnglerConstants.feedbackSensorCTRE); configGearRatio(ShooterAnglerConstants.gearRatio); configSupplyCurrentLimit(ShooterAnglerConstants.supplyLimit); + configForwardSoftLimit(voltageCompSaturation, false); } } From 728dea3d748606c1a21ff73d5b6bae25af201b31 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Sun, 8 Feb 2026 11:07:44 -0600 Subject: [PATCH 8/8] will need to decide what I want to do intkae later. Tuner constants from swerve gen with our drivebase. --- src/main/java/frc/robot/RobotContainer.java | 17 +- .../frc/robot/generated/TunerConstants.java | 561 ++++++++---------- .../frc/robot/subsystems/drive/Module.java | 2 +- .../frc/robot/subsystems/hopper/carpet.java | 1 - .../frc/robot/subsystems/intake/Intake.java | 45 +- .../subsystems/intake/IntakeConstants.java | 34 +- .../frc/robot/subsystems/shooter/Shooter.java | 35 +- .../shooter/angler/AnglerIOTalonFX.java | 1 - 8 files changed, 313 insertions(+), 383 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d87da8a..de14205 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,10 +96,10 @@ public RobotContainer() { // new VisionIOLimelight(camera0Name, drive::getRotation), // new VisionIOLimelight(camera1Name, drive::getRotation)); - intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); - shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); - carpet = new Carpet(new CarpetIOSparkFlex()); - climber = new Climber(new ClimberIOSparkFlex()); + intake = new Intake(new RollerIOSim(), new PivotIOSim()); + shooter = new Shooter(new AnglerIOSim(), new FlywheelIOSim()); + carpet = new Carpet(new CarpetIOSim()); + climber = new Climber(new ClimberIOSim()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -252,8 +252,13 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.STOW)); - operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + // Default Commands + intake.setDefaultCommand(intake.runIntakeeCommand(IntakeMode.OUT_IDLE)); + + + + operator.a().whileTrue(intake.runIntakeeCommand(IntakeMode.INTAKE)); + operator.b().whileTrue(intake.runIntakeeCommand(IntakeMode.OUTTAKE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index d41c38b..bd1daa8 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -1,321 +1,284 @@ -// Copyright (c) 2021-2026 Littleton Robotics -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by a BSD -// license that can be found in the LICENSE file -// at the root directory of this project. - package frc.robot.generated; import static edu.wpi.first.units.Units.*; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; import com.ctre.phoenix6.hardware.*; import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; -import frc.robot.Constants; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with - // the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(1.91) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to - // RemoteCANcoder - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these - // cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API - // documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a - // relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.8181818181818183; - - private static final double kDriveGearRatio = 7.363636363636365; - private static final double kSteerGearRatio = 15.42857142857143; - private static final Distance kWheelRadius = Inches.of(2.167); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 1; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(Constants.CANBUS.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 3; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10); - private static final Distance kFrontLeftYPos = Inches.of(10); - - // Front Right - private static final int kFrontRightDriveMotorId = 1; - private static final int kFrontRightSteerMotorId = 0; - private static final int kFrontRightEncoderId = 0; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10); - private static final Distance kFrontRightYPos = Inches.of(-10); - - // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10); - private static final Distance kBackLeftYPos = Inches.of(10); - - // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 4; - private static final int kBackRightEncoderId = 2; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10); - private static final Distance kBackRightYPos = Inches.of(-10); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - // } - - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.33).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.125; + + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 18.75; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 49; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 1; + private static final int kFrontLeftSteerMotorId = 2; + private static final int kFrontLeftEncoderId = 9; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.301513671875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = true; + + private static final Distance kFrontLeftXPos = Inches.of(11); + private static final Distance kFrontLeftYPos = Inches.of(11.5); + + // Front Right + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 3; + private static final int kFrontRightEncoderId = 10; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.318603515625); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = true; + + private static final Distance kFrontRightXPos = Inches.of(11); + private static final Distance kFrontRightYPos = Inches.of(-11.5); + + // Back Left + private static final int kBackLeftDriveMotorId = 7; + private static final int kBackLeftSteerMotorId = 8; + private static final int kBackLeftEncoderId = 12; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181396484375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = true; + + private static final Distance kBackLeftXPos = Inches.of(-11); + private static final Distance kBackLeftYPos = Inches.of(11.5); + + // Back Right + private static final int kBackRightDriveMotorId = 5; + private static final int kBackRightSteerMotorId = 6; + private static final int kBackRightEncoderId = 11; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.458984375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = true; + + private static final Distance kBackRightXPos = Inches.of(-11); + private static final Distance kBackRightYPos = Inches.of(-11.5); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); + + // /** + // * Creates a CommandSwerveDrivetrain instance. + // * This should only be called once in your robot program,. + // */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + // ); + // } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]ᵀ, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]ᵀ, with units in meters and radians - * @param modules Constants for each specific module + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } } - } } diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 9993e78..8f9781f 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -20,7 +20,7 @@ public class Module { private final ModuleIO io; - // private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/carpet.java index c79ffcd..f147957 100644 --- a/src/main/java/frc/robot/subsystems/hopper/carpet.java +++ b/src/main/java/frc/robot/subsystems/hopper/carpet.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; import frc.robot.subsystems.hopper.CarpetIO.CarpetIOInputs; -import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; public class Carpet extends SubsystemBase { private final CarpetIO carpetIO; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 774c300..e3ce7ce 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -7,9 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; -import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; -import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants.IntakePivotModes; -import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants.IntakeRollerModes; import frc.robot.subsystems.intake.pivot.PivotIO; import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; import frc.robot.subsystems.intake.roller.RollerIO; @@ -21,42 +18,48 @@ public class Intake extends SubsystemBase { private final RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); private final PivotIOInputsAutoLogged pivotInputs = new PivotIOInputsAutoLogged(); - private IntakeRollerModes rollerMode; - private IntakePivotModes pivotMode; + private IntakeMode intakeMode; + // private IntakePivotModes pivotMode; public Intake(RollerIO rollerIO, PivotIO pivotIO) { this.rollerIO = rollerIO; this.pivotIO = pivotIO; - this.rollerMode = IntakeRollerModes.IDLE; - this.pivotMode = IntakePivotModes.STOW; + this.intakeMode = IntakeMode.STOW; } @Override public void periodic() { rollerIO.updateInputs(rollerInputs); Logger.processInputs("Intake/Roller", rollerInputs); - Logger.recordOutput("Intake/Roller/Mode", rollerMode); pivotIO.updateInputs(pivotInputs); Logger.processInputs("Intake/Pivot", pivotInputs); - Logger.recordOutput("Intake/Pivot/Mode", pivotMode); + + Logger.recordOutput("Intake/Mode", intakeMode); } - public void runRollerEnum(IntakeRollerModes rollerMode) { - this.rollerMode = rollerMode; - rollerIO.setRollerVoltage(rollerMode.voltage.baseUnitMagnitude()); + public void runIntakeEnum(IntakeMode intakeMode) { + this.intakeMode = intakeMode; + rollerIO.setRollerVoltage(intakeMode.voltage.baseUnitMagnitude()); + pivotIO.setPosition(intakeMode.position.magnitude()); } - public void runPivotEnum(IntakePivotModes pivotMode) { - this.pivotMode = pivotMode; - pivotIO.setPosition(pivotMode.position.magnitude()); - } - - public Command runIntakeCommand(IntakePivotModes pivotMode, IntakeRollerModes rollerMode) { + public Command runIntakeeCommand(IntakeMode intakeMode) { return new RunCommand(() -> { - this.runRollerEnum(rollerMode); - this.runPivotEnum(pivotMode); - }, this).withName("Intake.runIntakeEnum" + rollerMode.toString()); + this.runIntakeEnum(intakeMode); + }, this).withName("Intake.runIntakeeCommand" + intakeMode.toString()); } + + // public Command runPivotCommand(IntakeMode intakeMode) { + // return new RunCommand(() -> { + // this.runPivotEnum(intakeMode); + // }, this).withName("Intake.runPivotCommand" + intakeMode.toString()); + // } + + // public Command runRollerCommand(IntakeMode rollerMode) { + // return new RunCommand(() -> { + // this.runRollerEnum(rollerMode); + // }, this).withName("Intake.runRollerCommand" + rollerMode.toString()); + // } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index d7f636d..a33f971 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -12,32 +12,22 @@ public class IntakeConstants { public enum IntakeMode { + STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), - INTAKE(Units.Volts.of(8.4), Units.Degrees.of(300)), - OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(300)); + OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), + INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0.0)), + OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(180.0)); public Voltage voltage; public Angle position; - + IntakeMode(Voltage voltage, Angle position) { this.voltage = voltage; this.position = position; } } - - public static final class IntakeRollerConstants { - public enum IntakeRollerModes { - IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(8.4)), - OUTTAKE(Units.Volts.of(-4.8)); - - public Voltage voltage; - - IntakeRollerModes(Voltage voltage) { - this.voltage = voltage; - } - } + public static final class IntakeRollerConstants { public static final boolean attached = true; @@ -68,18 +58,6 @@ public enum IntakeRollerModes { } public static final class IntakePivotConstants { - - public enum IntakePivotModes { - STOW(Units.Degrees.of(0.0)), - INTAKE(Units.Degrees.of(300)); - - public Angle position; - - IntakePivotModes(Angle position) { - this.position = position; - } - } - public static final boolean attached = true; public static final int id = -1; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 01c54b1..d455c1d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,26 +2,11 @@ import org.littletonrobotics.junction.Logger; -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 edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; -import frc.robot.subsystems.intake.pivot.PivotIO; -import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; -import frc.robot.subsystems.intake.roller.RollerIO; -import frc.robot.subsystems.intake.roller.RollerIOInputsAutoLogged; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; -import frc.robot.subsystems.shooter.flywheel.FlywheelIO.FlywheelIOInputs; import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; -import frc.team5431.titan.core.subsystem.CTREMechanism; -import lombok.Getter; -import lombok.Setter; public class Shooter extends SubsystemBase { private final AnglerIO anglerIO; @@ -29,9 +14,7 @@ public class Shooter extends SubsystemBase { private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - - private IntakeMode mode; - + public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; @@ -48,15 +31,15 @@ public void periodic() { // Logger.recordOutput("Intake/Mode", mode); } - public void runFlywheelEnum(IntakeMode intakeMode) { - this.mode = intakeMode; - flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); - } + // public void runFlywheelEnum(IntakeMode intakeMode) { + // this.mode = intakeMode; + // flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); + // } - public void runAnglerEnum(IntakeMode intakeMode) { - this.mode = intakeMode; - anglerIO.setPosition(mode.position.magnitude()); - } + // public void runAnglerEnum(IntakeMode intakeMode) { + // this.mode = intakeMode; + // anglerIO.setPosition(mode.position.magnitude()); + // } // public Command runIntakeCommand(IntakeMode intakeMode) { // return new RunCommand(() -> { diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 37cc884..f7041a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -12,7 +12,6 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; -import frc.robot.subsystems.intake.pivot.PivotIO.PivotIOInputs; import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; import frc.team5431.titan.core.subsystem.CTREMechanism;