diff --git a/.vscode/settings.json b/.vscode/settings.json index a42e75d..ca0e740 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -29,8 +29,8 @@ null ], "java.test.defaultConfig": "WPIlibUnitTests", - "spotlessGradle.format.enable": true, - "spotlessGradle.diagnostics.enable": false, + // "spotlessGradle.format.enable": true, + // "spotlessGradle.diagnostics.enable": false, "java.import.gradle.annotationProcessing.enabled": false, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", diff --git a/build.gradle b/build.gradle index e711096..127d21b 100644 --- a/build.gradle +++ b/build.gradle @@ -187,43 +187,43 @@ task(eventDeploy) { createVersionFile.dependsOn(eventDeploy) // Spotless formatting -project.compileJava.dependsOn(spotlessApply) -spotless { - java { - target fileTree(".") { - include "**/*.java" - exclude "**/build/**", "**/build-*/**" - } - toggleOffOn() - googleJavaFormat() - removeUnusedImports() - trimTrailingWhitespace() - endWithNewline() - } - groovyGradle { - target fileTree(".") { - include "**/*.gradle" - exclude "**/build/**", "**/build-*/**" - } - greclipse() - indentWithSpaces(4) - trimTrailingWhitespace() - endWithNewline() - } - json { - target fileTree(".") { - include "**/*.json" - exclude "**/build/**", "**/build-*/**", ".wpilib/**" - } - gson().indentWithSpaces(2) - } - format "misc", { - target fileTree(".") { - include "**/*.md", "**/.gitignore" - exclude "**/build/**", "**/build-*/**" - } - trimTrailingWhitespace() - indentWithSpaces(2) - endWithNewline() - } -} +// project.compileJava.dependsOn(spotlessApply) +// spotless { +// java { +// target fileTree(".") { +// include "**/*.java" +// exclude "**/build/**", "**/build-*/**" +// } +// toggleOffOn() +// googleJavaFormat() +// removeUnusedImports() +// trimTrailingWhitespace() +// endWithNewline() +// } +// groovyGradle { +// target fileTree(".") { +// include "**/*.gradle" +// exclude "**/build/**", "**/build-*/**" +// } +// greclipse() +// indentWithSpaces(4) +// trimTrailingWhitespace() +// endWithNewline() +// } +// json { +// target fileTree(".") { +// include "**/*.json" +// exclude "**/build/**", "**/build-*/**", ".wpilib/**" +// } +// gson().indentWithSpaces(2) +// } +// format "misc", { +// target fileTree(".") { +// include "**/*.md", "**/.gitignore" +// exclude "**/build/**", "**/build-*/**" +// } +// trimTrailingWhitespace() +// indentWithSpaces(2) +// endWithNewline() +// } +// } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3722907..8cfef36 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,7 +7,10 @@ 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; @@ -22,7 +25,9 @@ public final class Constants { public static final Mode simMode = Mode.SIM; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; - public static final String canbus = "Omnivore"; + + // CAN bus that the devices are located on; + public static final CANBus CANBUS = new CANBus("canivore", "./logs/example.hoot"); public static enum Mode { /** Running on a real robot. */ @@ -121,7 +126,7 @@ public enum IndexerModes { } } - public static final class IntakeConstants { + public static final class IntakeRollerIOConstants { public static final boolean attached = true; public static final boolean useRpm = false; @@ -141,8 +146,10 @@ public static final class IntakeConstants { public static final boolean gravityType = false; public static final boolean breakType = false; - public static final FeedbackSensorSourceValue feedbackSensor = + 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; @@ -157,21 +164,22 @@ public static final class IntakeConstants { public static final Current stallLimit = Units.Amps.of(80); public static final Current supplyLimit = Units.Amps.of(60); - public static final AngularVelocity intakeSpeed = Units.RPM.of(3000); + 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 IntakeModes { + public enum RollerIOModes { IDLE(idleSpeed, 0.0), - INTAKE(intakeSpeed, 0.7), - OUTTAKE(outtakeSpeed, -0.4); + INTAKE(rollerIOSpeed, 8.4), + OUTTAKE(outtakeSpeed, -4.8); public AngularVelocity speed; - public double output; + // TODO: Make sure voltage is just output * 12 (for 12V) + public double voltage; - IntakeModes(AngularVelocity speed, double output) { + RollerIOModes(AngularVelocity speed, double voltage) { this.speed = speed; - this.output = output; + this.voltage = voltage; } } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34a26c9..0c39a65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,8 +16,8 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; 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.drive.Drive; @@ -26,10 +26,16 @@ 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.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.team5431.titan.core.joysticks.CommandXboxController; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -42,9 +48,11 @@ public class RobotContainer { // Subsystems private final Drive drive; private final Vision vision; + private final Roller roller; // Controller - private final CommandXboxController controller = new CommandXboxController(0); + private final CommandXboxController driver = new CommandXboxController(0); + private final CommandXboxController operator = new CommandXboxController(1); // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -70,6 +78,10 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOLimelight(camera0Name, drive::getRotation), new VisionIOLimelight(camera1Name, drive::getRotation)); + roller = + new Roller( + new RollerIOSparkFlex()); + // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -109,6 +121,10 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose), new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose)); + + roller = + new Roller( + new RollerIOSim() {}); break; default: @@ -122,7 +138,10 @@ public RobotContainer() { new ModuleIO() {}); vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); + roller = new Roller(new RollerIO() {}); + break; + } // Set up auto routines @@ -144,8 +163,8 @@ public RobotContainer() { autoChooser.addOption( "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - // Configure the button bindings - configureButtonBindings(); + configureDriverBindings(); + configureOperatorBindings(); } /** @@ -154,30 +173,30 @@ public RobotContainer() { * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() { + private void configureDriverBindings() { // Default command, normal field-relative drive drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX())); + () -> -driver.getLeftY(), + () -> -driver.getLeftX(), + () -> -driver.getRightX())); // Lock to 0° when A button is held - controller + driver .a() .whileTrue( DriveCommands.joystickDriveAtAngle( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), + () -> -driver.getLeftY(), + () -> -driver.getLeftX(), () -> Rotation2d.kZero)); // Switch to X pattern when X button is pressed - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); + driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - // Reset gyro to 0° when B button is pressed - controller + // Reset gyro to 0° when B button is pressed + driver .b() .onTrue( Commands.runOnce( @@ -187,23 +206,26 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); - // // Auto aim command example; code from AKit template. - // @SuppressWarnings("resource") - // PIDController aimController = new PIDController(0.2, 0.0, 0.0); - // aimController.enableContinuousInput(-Math.PI, Math.PI); - // controller - // .button(1) - // .whileTrue( - // Commands.startRun( - // () -> { - // aimController.reset(); - // }, - // () -> { - // drive.run(0.0, aimController.calculate(vision.getTargetX(0).getRadians())); - // }, - // drive)); + // // Auto aim command example; code from AKit template. + // @SuppressWarnings("resource") + // PIDController aimController = new PIDController(0.2, 0.0, 0.0); + // aimController.enableContinuousInput(-Math.PI, Math.PI); + // controller + // .button(1) + // .whileTrue( + // Commands.startRun( + // () -> { + // aimController.reset(); + // }, + // () -> { + // drive.run(0.0, aimController.calculate(vision.getTargetX(0).getRadians())); + // }, + // drive)); } + private void configureOperatorBindings() { + operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true)); + } /** * 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 83ee48f..d41c38b 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -9,7 +9,6 @@ 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.*; @@ -19,6 +18,7 @@ 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 // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html @@ -84,10 +84,6 @@ public class TunerConstants { // 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(4.69); @@ -114,7 +110,7 @@ public class TunerConstants { public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) + .withCANBusName(Constants.CANBUS.getName()) .withPigeon2Id(kPigeonId) .withPigeon2Configs(pigeonConfigs); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 397917a..ec515c8 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,7 +50,7 @@ public class Drive extends SubsystemBase { // TunerConstants doesn't include these constants, so they are declared locally - static final double ODOMETRY_FREQUENCY = TunerConstants.kCANBus.isNetworkFD() ? 250.0 : 100.0; + static final double ODOMETRY_FREQUENCY = Constants.CANBUS.isNetworkFD() ? 250.0 : 100.0; public static final double DRIVE_BASE_RADIUS = Math.max( Math.max( diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 5dc0241..e3d3d20 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -16,13 +16,14 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.Constants; import frc.robot.generated.TunerConstants; import java.util.Queue; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = - new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, TunerConstants.kCANBus); + new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANBUS); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 606eb54..8cf20bb 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -34,14 +34,15 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; -import frc.robot.generated.TunerConstants; +import frc.robot.Constants; import java.util.Queue; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and * CANcoder. Configured using a set of module constants from Phoenix. * - *

Device configuration and other behaviors not exposed by TunerConstants can be customized here. + *

Device configuration and other behaviors not exposed by frc.robot.Constants can be customized + * here. */ public class ModuleIOTalonFX implements ModuleIO { private final SwerveModuleConstants< @@ -95,9 +96,9 @@ public ModuleIOTalonFX( SwerveModuleConstants constants) { this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, TunerConstants.kCANBus); - turnTalon = new TalonFX(constants.SteerMotorId, TunerConstants.kCANBus); - cancoder = new CANcoder(constants.EncoderId, TunerConstants.kCANBus); + driveTalon = new TalonFX(constants.DriveMotorId, Constants.CANBUS); + turnTalon = new TalonFX(constants.SteerMotorId, Constants.CANBUS); + cancoder = new CANcoder(constants.EncoderId, Constants.CANBUS); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 684ffd1..5cc1fae 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -11,7 +11,7 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; -import frc.robot.generated.TunerConstants; +import frc.robot.Constants; import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -37,7 +37,7 @@ public class PhoenixOdometryThread extends Thread { private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private static boolean isCANFD = TunerConstants.kCANBus.isNetworkFD(); + private static boolean isCANFD = Constants.CANBUS.isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index e4fafae..fa5be02 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -9,7 +9,6 @@ import frc.robot.Constants.IndexerConstants; import frc.robot.Constants.IndexerConstants.IndexerModes; import frc.robot.Constants.IndexerConstants.IndexerState; -import frc.robot.Constants.IntakeConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; import lombok.Getter; import lombok.Setter; @@ -24,7 +23,7 @@ public class Indexer extends CTREMechanism { public static class IndexerConfig extends Config { public IndexerConfig() { - super("Indexer", IndexerConstants.id, Constants.canbus); + super("Indexer", IndexerConstants.id, Constants.CANBUS); configNeutralBrakeMode(IndexerConstants.breakType); configStatorCurrentLimit(IndexerConstants.stallLimit, true); @@ -38,8 +37,8 @@ public IndexerConfig() { } } - public Indexer(TalonFX motor, boolean attached) { - super(motor, attached); + public Indexer(TalonFX motor, boolean attached, IndexerConfig config) { + super(motor, attached, config); this.attached = attached; this.motor = motor; this.indexerModes = IndexerModes.IDLE; @@ -70,11 +69,4 @@ public void periodic() { public Command runIndexerCommand(IndexerModes indexerModes) { return new RunCommand(() -> setPercentOutput(indexerModes.output), this).withName("Indexer.runEnum"); } - - @Override - public Config setConfig() { - // configure motors/sensors here - this.config = new IndexerConfig(); - return this.config; - } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java deleted file mode 100644 index c9d09bb..0000000 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ /dev/null @@ -1,75 +0,0 @@ -package frc.robot.subsystems.intake; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import frc.robot.Constants; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.IntakeConstants.IntakeModes; -import frc.team5431.titan.core.subsystem.CTREMechanism; -import lombok.Getter; -import org.littletonrobotics.junction.Logger; - -public class Intake extends CTREMechanism { - - public static class IntakeConfig extends Config { - public IntakeConfig() { - super("Intake", IntakeConstants.id, Constants.canbus); - configPIDGains(IntakeConstants.p, IntakeConstants.i, IntakeConstants.d); - configNeutralBrakeMode(IntakeConstants.breakType); - configFeedbackSensorSource(IntakeConstants.feedbackSensor); - configGearRatio(IntakeConstants.gearRatio); - configGravityType(IntakeConstants.gravityType); - configSupplyCurrentLimit(IntakeConstants.supplyLimit, IntakeConstants.useSupplyLimit); - configStatorCurrentLimit(IntakeConstants.stallLimit, IntakeConstants.useStallLimit); - configReverseSoftLimit( - IntakeConstants.maxReverseRotation.in(Rotation), IntakeConstants.useRMaxRotation); - configForwardSoftLimit( - IntakeConstants.maxFowardRotation.in(Rotation), IntakeConstants.useFMaxRotation); - configMotorInverted(IntakeConstants.invert); - } - } - - private TalonFX motor; - @Getter private IntakeModes mode; - - public Intake(TalonFX motor, boolean attached) { - super(motor, attached); - - this.mode = IntakeModes.IDLE; - this.motor = motor; - this.attached = attached; - config.applyTalonConfig(motor); - } - - @Override - public void periodic() { - Logger.recordOutput("Intake/Setpoint", getMode().speed.in(RPM)); - } - - public void runEnum(IntakeModes intakemode, boolean rpm) { - this.mode = intakemode; - if (rpm) { - setVelocity(intakemode.speed); - } else { - setPercentOutput(intakemode.output); - } - } - - public Command runIntakeCommand(IntakeModes intakeModes) { - return new RunCommand(() -> this.runEnum(intakeModes, IntakeConstants.useRpm), this) - .withName("Intake.runEnum"); - } - - public Command runIntakeCommand(IntakeModes intakeModes, boolean rpm) { - return new RunCommand(() -> this.runEnum(intakeModes, rpm), this).withName("Intake.runEnum"); - } - - @Override - protected Config setConfig() { - this.config = new IntakeConfig(); - return config; - } -} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/Roller.java b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java new file mode 100644 index 0000000..081fc95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/Roller.java @@ -0,0 +1,40 @@ +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 new file mode 100644 index 0000000..7428118 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIO.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems.intake.roller; + +import org.littletonrobotics.junction.AutoLog; + +public interface RollerIO { + + @AutoLog + public static class RollerIOInputs { + public boolean rollerConnected = false; + public double appliedVoltage = 0.0; + public double RPM = 0.0; + + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(RollerIOInputs inputs) {} + + /** Run the motor at the specified voltage. */ + public default void setRollerVoltage(double voltage) {} + + /** Run the motor to the specified rotation per minute. */ + public default void setRPM(double rpm) {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java new file mode 100644 index 0000000..331d3fa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSim.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.intake.roller; + +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 RollerIOSim implements RollerIO { + private DCMotorSim rollerMotorSim; + private PIDController rollerController; + private boolean rollerClosedLoop = false; + private double appliedVoltage = 0.0; + private double driveFFVolts = 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; + + + public RollerIOSim() { + this.rollerMotorSim = 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 + rollerController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + if (rollerClosedLoop) { + appliedVoltage = driveFFVolts + rollerController.calculate(rollerMotorSim.getAngularVelocityRadPerSec()); + } else { + rollerController.reset(); + } + + rollerMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + rollerMotorSim.update(0.02); + + inputs.rollerConnected = true; + inputs.RPM = rollerMotorSim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + } + + @Override + public void setRPM(double RPM) { + rollerClosedLoop = true; + driveFFVolts = DRIVE_KS * Math.signum(RPM) + DRIVE_KV * RPM; + rollerController.setSetpoint(RPM); + } + + public void setAppliedVoltage(double appliedVoltage) { + this.appliedVoltage = MathUtil.clamp(appliedVoltage, -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 new file mode 100644 index 0000000..a7e0ff1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -0,0 +1,55 @@ +package frc.robot.subsystems.intake.roller; + +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.IntakeRollerIOConstants; +import frc.team5431.titan.core.subsystem.REVMechanism; + +public class RollerIOSparkFlex implements RollerIO { + private final SparkFlex sparkFlex = new SparkFlex(0, 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); + // configGear(RollerIOConstants.gearRatio); + // configGravity(RollerIOConstants.gravityType); + configSmartCurrentLimit(IntakeRollerIOConstants.stallLimit, IntakeRollerIOConstants.supplyLimit); + configSmartStallCurrentLimit(IntakeRollerIOConstants.stallLimit); + configReverseSoftLimit( + IntakeRollerIOConstants.maxReverseRotation, IntakeRollerIOConstants.useRMaxRotation); + configForwardSoftLimit( + IntakeRollerIOConstants.maxFowardRotation, IntakeRollerIOConstants.useFMaxRotation); + } + } + + public RollerIOSparkFlex() { + sparkFlex.configure(new RollerIOSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + ifOk(sparkFlex, encoder::getVelocity, (value) -> inputs.RPM = value); + ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); + } + + @Override + public void setRPM(double rpm) { + sparkFlex + .getClosedLoopController() + .setSetpoint(rpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + } + + @Override + public void setRollerVoltage(double voltage) { + sparkFlex.setVoltage(voltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java new file mode 100644 index 0000000..1969919 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.intake.roller; + +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.Voltage; +import frc.robot.Constants; +import frc.robot.Constants.IntakeRollerIOConstants; +import frc.team5431.titan.core.subsystem.CTREMechanism; + +public class RollerIOTalonFX implements RollerIO { + private final TalonFX talon = new TalonFX(IntakeRollerIOConstants.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); + } + } + + private StatusSignal appliedVoltage; + private StatusSignal rollerRPM; + private StatusSignal rollerOutput; + + private RollerTalonFXConfig config; + + public RollerIOTalonFX(TalonFX talon) { + appliedVoltage = talon.getMotorVoltage(); + rollerRPM = talon.getVelocity(); + rollerOutput = talon.getClosedLoopOutput(); + config.applyTalonConfig(talon); + + BaseStatusSignal.setUpdateFrequencyForAll(50, appliedVoltage, rollerOutput, rollerRPM); + } + + @Override + public void updateInputs(RollerIOInputs inputs) { + BaseStatusSignal.refreshAll(appliedVoltage, rollerOutput, rollerRPM); + + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.RPM = rollerRPM.getValue().in(RPM); + } + + @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 793c698..da5d41e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -20,10 +20,9 @@ public class Shooter extends CTREMechanism { private boolean attached; private TalonFX motor; - public static class ShooterConfig extends Config { - public ShooterConfig() { - super("Shooter", ShooterConstants.id, Constants.canbus); - + 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); @@ -36,8 +35,8 @@ public ShooterConfig() { } } - public Shooter(TalonFX motor, boolean attached) { - super(motor, attached); + public Shooter(TalonFX motor, boolean attached, ShooterConfig config) { + super(motor, attached, config); this.attached = attached; this.motor = motor; this.shooterMode = ShooterModes.IDLE; @@ -68,11 +67,4 @@ public void periodic() { public Command runShooterCommand(ShooterModes shooterModes) { return new RunCommand(() -> setVelocity(shooterModes.speed), this).withName("Shooter.runEnum"); } - - @Override - public Config setConfig() { - // configure motors/sensors here - this.config = new ShooterConfig(); - return this.config; - } } diff --git a/src/main/java/frc/robot/util/SparkUtil.java b/src/main/java/frc/robot/util/SparkUtil.java new file mode 100644 index 0000000..8dbe477 --- /dev/null +++ b/src/main/java/frc/robot/util/SparkUtil.java @@ -0,0 +1,56 @@ +// 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.util; + +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public class SparkUtil { + /** Stores whether any error was has been detected by other utility methods. */ + public static boolean sparkStickyFault = false; + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer consumer) { + double value = supplier.getAsDouble(); + if (spark.getLastError() == REVLibError.kOk) { + consumer.accept(value); + } else { + sparkStickyFault = true; + } + } + + /** Processes a value from a Spark only if the value is valid. */ + public static void ifOk( + SparkBase spark, DoubleSupplier[] suppliers, Consumer consumer) { + double[] values = new double[suppliers.length]; + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return; + } + } + consumer.accept(values); + } + + /** Attempts to run the command until no error is produced. */ + public static void tryUntilOk(SparkBase spark, int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error == REVLibError.kOk) { + break; + } else { + sparkStickyFault = true; + } + } + } +} 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 6284d39..db26ebb 100644 --- a/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java +++ b/src/main/java/frc/team5431/titan/core/subsystem/CTREMechanism.java @@ -1,5 +1,6 @@ package frc.team5431.titan.core.subsystem; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; @@ -41,15 +42,9 @@ public abstract class CTREMechanism implements Subsystem { * * @param attached for if the motor is in use */ - public CTREMechanism(TalonFX motor, boolean attached) { + public CTREMechanism(TalonFX motor, boolean attached, Config config) { this.attached = attached; this.motor = motor; - this.config = setConfig(); - } - - protected abstract Config setConfig(); - - protected void setConfig(Config config) { this.config = config; }; @@ -247,7 +242,7 @@ public static class Config { // voltage //Should // normally use VoltageOut - public Config(String title, int id, String canbus) { + public Config(String title, int id, CANBus canbus) { this.title = title; this.voltageCompSaturation = 12.0; this.id = id;