diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8cfef36..16a2bd6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,13 +8,6 @@ package frc.robot; import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.revrobotics.spark.FeedbackSensor; - -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,147 +33,5 @@ public static enum Mode { 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 { - INDEXER, - REVERSE, - IDLE - } - - public static final boolean attached = true; - - public static final int id = -1; - - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; - - public static final boolean invert = false; - public static final boolean breakType = false; - public static final double gearRatio = 1 / 1; - - - public static final Current stallLimit = Units.Amps.of(60); - public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; - - public static final double indexerSpeed = 0.5; - public static final double reverseSpeed = -0.5; - public static final double idleSpeed = 0.0; - public enum IndexerModes { - INDEXER(indexerSpeed), - REVERSE(reverseSpeed), - IDLE(idleSpeed); - - public double output; - - IndexerModes(double output) { - this.output = output; - } - } - } - - public static final class IntakeRollerIOConstants { - 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 RollerIOModes { - 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; - - RollerIOModes(AngularVelocity speed, double voltage) { - this.speed = speed; - this.voltage = voltage; - } - } - } -} +} \ 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..d87da8a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,33 +7,47 @@ 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; 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.IntakeRollerIOConstants.RollerIOModes; 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; import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; -import frc.robot.subsystems.intake.roller.Roller; +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; +import frc.robot.subsystems.intake.pivot.PivotIOSim; +import frc.robot.subsystems.intake.pivot.PivotIOSparkFlex; 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.networktables.LoggedDashboardChooser; @@ -47,8 +61,11 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final Vision vision; - private final Roller roller; +// private final Vision vision; + private final Intake intake; + private final Shooter shooter; + private final Carpet carpet; + private final Climber climber; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -73,15 +90,16 @@ 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()); - + // vision = + // new Vision( + // drive::addVisionMeasurement, + // 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()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -116,17 +134,21 @@ 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)); + // vision = + // new Vision( + // drive::addVisionMeasurement, + // new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), + // new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); - roller = - new Roller( - new RollerIOSim() {}); + intake = + new Intake(new RollerIOSim(), new PivotIOSim()); + shooter = + new Shooter(new AnglerIOSim(), new FlywheelIOSim()); + carpet = + new Carpet(new CarpetIOSim()); + climber = + new Climber(new ClimberIOSim()); break; - default: // Replayed robot, disable IO implementations drive = @@ -137,9 +159,11 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); - vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); - roller = new Roller(new RollerIO() {}); - + // 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() {}); + climber = new Climber(new ClimberIO() {}); break; } @@ -165,6 +189,10 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); + + + SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); + } /** @@ -182,18 +210,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)); + // // 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)); + // // 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 @@ -224,7 +252,8 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true)); + 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/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..ea6267c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.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..f3b35d3 --- /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 = (42.0 / 11.0) * (42.0 / 18.0); + + 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/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..2de40a0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -0,0 +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 new file mode 100644 index 0000000..1dd4b97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -0,0 +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/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/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/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..98a07c8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSim.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.hopper; + +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..a6265a1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.hopper; + +import com.revrobotics.PersistMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.ResetMode; +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(CarpetRollerConstants.id, 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); + } + } + + 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..563ba28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -0,0 +1,65 @@ +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); + } + } + + 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 new file mode 100644 index 0000000..c79ffcd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hopper/carpet.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.hopper; + +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/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java deleted file mode 100644 index fa5be02..0000000 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ /dev/null @@ -1,72 +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", IndexerConstants.id, Constants.CANBUS); - - configNeutralBrakeMode(IndexerConstants.breakType); - configStatorCurrentLimit(IndexerConstants.stallLimit, true); - configSupplyCurrentLimit(IndexerConstants.supplyLimit, true); - configForwardSoftLimit(IndexerConstants.maxForwardOutput, true); - configReverseSoftLimit(IndexerConstants.maxReverseOutput, true); - configPIDGains(IndexerConstants.p, IndexerConstants.i, IndexerConstants.d); - configPeakOutput(IndexerConstants.maxForwardOutput, IndexerConstants.maxReverseOutput); - configGearRatio(IndexerConstants.gearRatio); - configMotorInverted(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"); - } -} 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..774c300 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,62 @@ +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.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; +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 IntakeRollerModes rollerMode; + private IntakePivotModes pivotMode; + + + public Intake(RollerIO rollerIO, PivotIO pivotIO) { + this.rollerIO = rollerIO; + this.pivotIO = pivotIO; + 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/Pivot/Mode", pivotMode); + } + + public void runRollerEnum(IntakeRollerModes rollerMode) { + this.rollerMode = rollerMode; + rollerIO.setRollerVoltage(rollerMode.voltage.baseUnitMagnitude()); + } + + public void runPivotEnum(IntakePivotModes pivotMode) { + this.pivotMode = pivotMode; + pivotIO.setPosition(pivotMode.position.magnitude()); + } + + public Command runIntakeCommand(IntakePivotModes pivotMode, IntakeRollerModes rollerMode) { + return new RunCommand(() -> { + 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 new file mode 100644 index 0000000..d7f636d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,109 @@ +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.Degrees.of(0.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; + + 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 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 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; + + 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/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java new file mode 100644 index 0000000..d9d6c6b --- /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 positionAngle = 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..c88dbfb --- /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 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; + + 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)); + + // pivotMotorSim.set + pivotController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + if (pivotClosedLoop) { + appliedVoltage = pivotFFVolts + pivotController.calculate(pivotMotorSim.getAngularVelocityRadPerSec()); + } else { + pivotController.reset(); + } + + pivotMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + pivotMotorSim.update(0.02); + + inputs.pivotConnected = true; + inputs.positionAngle = pivotMotorSim.getAngularPositionRad(); + inputs.appliedVoltage = appliedVoltage; + inputs.currentAmps = Math.abs(pivotMotorSim.getCurrentDrawAmps()); + } + + @Override + public void setPosition(double positionAngle) { + pivotClosedLoop = true; + pivotController.setSetpoint(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..5e7485a --- /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.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class PivotIOSparkFlex implements PivotIO { + private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, 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(IntakePivotConstants.gearRatio); + // configGravity(IntakePivotConstants.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.positionAngle = 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..92baf0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -0,0 +1,80 @@ +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.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 static edu.wpi.first.units.Units.*; + +import frc.robot.Constants; +import frc.robot.subsystems.intake.IntakeConstants.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", Constants.CANBUS); + configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); + configNeutralBrakeMode(IntakePivotConstants.breakType); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); + configGearRatio(IntakePivotConstants.gearRatio); + configGravityType(IntakePivotConstants.gravityType); + configSupplyCurrentLimit(IntakePivotConstants.supplyLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation.in(Rotation), IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation.in(Rotation), IntakePivotConstants.useFMaxRotation); + } + } + + + private StatusSignal appliedVoltage; + 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() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + 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(); + } + + @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 deleted file mode 100644 index 081fc95..0000000 --- a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.robot.subsystems.intake.roller; - - -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; - -public class Roller extends SubsystemBase { - - private final RollerIO rollerIO; - private final RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); - - private RollerIOModes mode; - - public Roller(RollerIO rollerIO) { - this.rollerIO = rollerIO; - this.mode = RollerIOModes.IDLE; - } - - @Override - public void periodic() { - rollerIO.updateInputs(inputs); - } - - public void runEnum(RollerIOModes rollerIOModes, boolean useRPM) { - this.mode = rollerIOModes; - if (useRPM) { - rollerIO.setRPM(rollerIOModes.speed.magnitude()); - } - else { - rollerIO.setRollerVoltage(rollerIOModes.voltage); - } - } - - public Command runIntakeCommand(RollerIOModes 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..fa4f894 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,16 +44,19 @@ 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); } - 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 a7e0ff1..488d0a8 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -9,25 +9,21 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; -import frc.robot.Constants.IntakeRollerIOConstants; +import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; 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() { - 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); - configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation, IntakeRollerIOConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation, IntakeRollerIOConstants.useFMaxRotation); + configSmartCurrentLimit(IntakeRollerConstants.stallLimit, IntakeRollerConstants.supplyLimit); + configSmartStallCurrentLimit(IntakeRollerConstants.stallLimit); } } @@ -39,6 +35,9 @@ 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); + + // 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 1969919..53b06b2 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -4,66 +4,62 @@ 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.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.Constants.IntakeRollerIOConstants; +import frc.robot.subsystems.intake.IntakeConstants.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); - configReverseSoftLimit( - IntakeRollerIOConstants.maxReverseRotation.in(Rotation), IntakeRollerIOConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeRollerIOConstants.maxFowardRotation.in(Rotation), IntakeRollerIOConstants.useFMaxRotation); + 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); } } private StatusSignal appliedVoltage; private StatusSignal rollerRPM; - private StatusSignal rollerOutput; + 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; + private RollerTalonFXConfig config = new RollerTalonFXConfig(); - public RollerIOTalonFX(TalonFX talon) { + public RollerIOTalonFX() { 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); + 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(); } @Override 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..01c54b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,70 +1,67 @@ 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.Constants.ShooterConstants; -import frc.robot.Constants.ShooterConstants.ShooterModes; -import frc.robot.Constants.ShooterConstants.ShooterState; +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 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; +public class Shooter extends SubsystemBase { + private final AnglerIO anglerIO; + private final FlywheelIO flywheelIO; - config.applyTalonConfig(motor); - } + private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); + private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); + + private IntakeMode mode; + - @Override - public void periodic() { - SmartDashboard.putString("Shooter Mode", getShooterMode().toString()); - + 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); + } - switch (this.shooterMode) { - case IDLE: - setShooterState(ShooterState.IDLE); - break; - case SHOOTER: - setShooterState(ShooterState.SHOOTER); - break; - case REVERSE: - setShooterState(ShooterState.REVERSE); - break; - } + 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 runShooterCommand(ShooterModes shooterModes) { - return new RunCommand(() -> setVelocity(shooterModes.speed), this).withName("Shooter.runEnum"); - } + // 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 new file mode 100644 index 0000000..72be9c4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.shooter; + +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; + +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 = 3.0 / 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 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/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java new file mode 100644 index 0000000..d732bc9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -0,0 +1,20 @@ +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 position. */ + public default void setPosition(double positionAngle) { + } +} 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..37cc884 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -0,0 +1,71 @@ +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.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.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); + configForwardSoftLimit(voltageCompSaturation, false); + } + } + + + private StatusSignal appliedVoltage; + 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() { + appliedVoltage = talon.getMotorVoltage(); + pivotPosition = talon.getPosition(); + currentAmps = talon.getStatorCurrent(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, currentAmps, pivotPosition); + + } + + @Override + public void updateInputs(AnglerIOInputs inputs) { + var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); + + inputs.anglerConnected = anglerConnectedDebounce.calculate(anglerStatus.isOK()); + + 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 new file mode 100644 index 0000000..31cee28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -0,0 +1,23 @@ +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 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. */ + 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..c692a8d --- /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.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 new file mode 100644 index 0000000..d37be19 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -0,0 +1,89 @@ +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.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +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; +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 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() { + 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 leaderAppliedVoltage; + private StatusSignal leaderFlywheelRPM; + private StatusSignal leaderAmps; + + private StatusSignal followerAppliedVoltage; + private StatusSignal followerFlywheelRPM; + private StatusSignal followerAmps; + + // 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(); + 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, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + 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); + inputs.leaderAmps = leaderAmps.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)); + leader.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 db26ebb..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 @@ -239,13 +238,12 @@ 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) { + 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) {