From 9941e6fae30c2d09b2f0a1aaf8b7d3643c55a645 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Wed, 15 Apr 2026 22:19:35 +0000 Subject: [PATCH] Reduce PR scope by removing formatting-only churn Agent-Logs-Url: https://github.com/iron-claw-972/FRC2026/sessions/2270b964-3a5d-443d-9b34-6cc53803192e Co-authored-by: Arnav814 <74715690+Arnav814@users.noreply.github.com> --- .../controls/PS5ControllerDriverConfig.java | 286 +++--- .../controls/PS5XboxModeDriverConfig.java | 370 ++++---- .../robot/subsystems/drivetrain/Module.java | 853 +++++++++--------- 3 files changed, 753 insertions(+), 756 deletions(-) diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index b2a9341..6449efe 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -30,24 +30,23 @@ import lib.controllers.PS5Controller.PS5Button; * Driver controls for the PS5 controller */ public class PS5ControllerDriverConfig extends BaseDriverConfig { - private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = () -> false; - private boolean intakeBoolean = true; - private Command autoShoot = null; - private Shooter shooter; - private Turret turret; - private Hood hood; - private Intake intake; - private Spindexer spindexer; - - public PS5ControllerDriverConfig( + private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; + private Shooter shooter; + private Turret turret; + private Hood hood; + private Intake intake; + private Spindexer spindexer; + + public PS5ControllerDriverConfig( Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, - Spindexer spindexer - ) { + Spindexer spindexer) { super(drive); this.shooter = shooter; this.turret = turret; @@ -56,148 +55,151 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { this.spindexer = spindexer; } - public void configureControls() { - // Reset the yaw. Mainly useful for testing/driver practice - controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); - - // Cancel commands - controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { - getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(() -> null); - CommandScheduler.getInstance().cancelAll(); - })); - - // Reverse motors - if (intake != null && spindexer != null) { - controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake)); - } + public void configureControls() { + // Reset the yaw. Mainly useful for testing/driver practice + controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); + })); + + // Reverse motors + if (intake != null && spindexer != null) { + controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake)); + } - // Intake - if (intake != null) { - // Toggle intake - controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { - if (intakeBoolean) { - intake.extend(); - intake.spinStart(); - intakeBoolean = false; - } else { - intake.intermediateExtend(); - intake.spinStop(); - intakeBoolean = true; + // Intake + if (intake != null) { + // Toggle intake + controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { + if (intakeBoolean) { + intake.extend(); + intake.spinStart(); + intakeBoolean = false; + } else { + intake.intermediateExtend(); + intake.spinStop(); + intakeBoolean = true; + } + }, intake)); + + // Retract if hold for 2 seconds + controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> { + intake.retract(); + intakeBoolean = true; + intake.spinStop(); + }, intake)); + + // Make the intake go in and out while shooting + controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake) + .alongWith(new InstantCommand(()-> intakeBoolean = true))); + + // Calibration: you can now calibrate easily using this button + if (hood != null && intake != null) { + controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { + intake.calibrate(); + hood.calibrate(); + }, intake, hood)).onFalse(new InstantCommand(() -> { + intake.stopCalibrating(); + hood.stopCalibrating(); + }, intake, hood)); + } + + // Stop intake roller + controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{ + if(intakeBoolean){ + intake.spinStart(); + intakeBoolean = false; + } else{ + intake.spinStop(); + intakeBoolean = true; + } + })); + } + + // Spindexer + if (spindexer != null && turret != null && hood != null && intake != null) { + + // Toggle spindexer + controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( + new RunSpindexer(spindexer, turret, hood, intake) + ); + } + + // Auto shoot + if (turret != null && hood != null && shooter != null && spindexer != null) { + autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); + controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot); + } + + + // Hood + if (hood != null) { + // Set the hood down -- for safety measures under trench + controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{ + hood.forceHoodDown(true); + }, hood)).onFalse(new InstantCommand(()->{ + hood.forceHoodDown(false); + })); + } + + // shoot focus mode: reduces drive current + if (spindexer != null) { + controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true))) + .onFalse(new InstantCommand(()-> shootFocus(false))); } - }, intake)); - - // Retract if hold for 2 seconds - controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> { - intake.retract(); - intakeBoolean = true; - intake.spinStop(); - }, intake)); - - // Make the intake go in and out while shooting - controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake) - .alongWith(new InstantCommand(() -> intakeBoolean = true))); - - // Calibration: you can now calibrate easily using this button - if (hood != null && intake != null) { - controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { - intake.calibrate(); - hood.calibrate(); - }, intake, hood)).onFalse(new InstantCommand(() -> { - intake.stopCalibrating(); - hood.stopCalibrating(); - }, intake, hood)); - } - - // Stop intake roller - controller.get(DPad.DOWN).onTrue(new InstantCommand(() -> { - if (intakeBoolean) { - intake.spinStart(); - intakeBoolean = false; + } + + private void shootFocus(boolean turnOn) { + if (turnOn) { + System.out.println("Shooting is now Focused"); + spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); + drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25); } else { - intake.spinStop(); - intakeBoolean = true; + System.out.println("Shooting back to normal (From focus)"); + spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); + drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, + DriveConstants.DRIVE_PEAK_CURRENT_LIMIT); } - })); } - // Spindexer - if (spindexer != null && turret != null && hood != null && intake != null) { + @Override + public double getRawSideTranslation() { + return controller.get(PS5Axis.LEFT_X); + } + + @Override + public double getRawForwardTranslation() { + return controller.get(PS5Axis.LEFT_Y); + } - // Toggle spindexer - controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( - new RunSpindexer(spindexer, turret, hood, intake)); + @Override + public double getRawRotation() { + return controller.get(PS5Axis.RIGHT_X); } - // Auto shoot - if (turret != null && hood != null && shooter != null && spindexer != null) { - autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); - controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot); + @Override + public double getRawHeadingAngle() { + return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; } - // Hood - if (hood != null) { - // Set the hood down -- for safety measures under trench - controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> { - hood.forceHoodDown(true); - }, hood)).onFalse(new InstantCommand(() -> { - hood.forceHoodDown(false); - })); + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y)); } - // shoot focus mode: reduces drive current - if (spindexer != null) { - controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> shootFocus(true))) - .onFalse(new InstantCommand(() -> shootFocus(false))); + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); } - } - - private void shootFocus(boolean turnOn) { - if (turnOn) { - System.out.println("Shooting is now Focused"); - spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); - drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, - DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25); - } else { - System.out.println("Shooting back to normal (From focus)"); - spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); - drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT); + + @Override + public boolean getIsAlign() { + return false; } - } - - @Override - public double getRawSideTranslation() { - return controller.get(PS5Axis.LEFT_X); - } - - @Override - public double getRawForwardTranslation() { - return controller.get(PS5Axis.LEFT_Y); - } - - @Override - public double getRawRotation() { - return controller.get(PS5Axis.RIGHT_X); - } - - @Override - public double getRawHeadingAngle() { - return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; - } - - @Override - public double getRawHeadingMagnitude() { - return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y)); - } - - @Override - public boolean getIsSlowMode() { - return slowModeSupplier.getAsBoolean(); - } - - @Override - public boolean getIsAlign() { - return false; - } } diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java index 84c9293..e0cc573 100644 --- a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -38,204 +38,202 @@ import lib.controllers.GameController.DPad; */ public class PS5XboxModeDriverConfig extends BaseDriverConfig { - private final GameController controller = new GameController(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = () -> false; - private boolean intakeBoolean = true; - private Command autoShoot = null; - private Command reverseMotors = null; - private Shooter shooter; - private Turret turret; - private Hood hood; - private Intake intake; - private Spindexer spindexer; - - // PS5 button aliases - private final Button CROSS = Button.A; - private final Button CIRCLE = Button.B; - private final Button SQUARE = Button.X; - private final Button TRIANGLE = Button.Y; - private final Button LB = Button.LB; - private final Button RB = Button.RB; - private final Button CREATE = Button.BACK; - private final Button OPTIONS = Button.START; - private final Button LEFT_JOY = Button.LEFT_JOY; - private final Button RIGHT_JOY = Button.RIGHT_JOY; - - // PS5 trigger buttons - private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON; - private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON; - - // PS5 axis aliases - private final Axis LEFT_X = Axis.LEFT_X; - private final Axis LEFT_Y = Axis.LEFT_Y; - private final Axis RIGHT_X = Axis.RIGHT_X; - private final Axis RIGHT_Y = Axis.RIGHT_Y; - private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER; - private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER; - - public PS5XboxModeDriverConfig( - Drivetrain drive, - Shooter shooter, - Turret turret, - Hood hood, - Intake intake, - Spindexer spindexer) { - super(drive); - this.shooter = shooter; - this.turret = turret; - this.hood = hood; - this.intake = intake; - this.spindexer = spindexer; - } - - public void configureControls() { - // Reset the yaw. Mainly useful for testing/driver practice - controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); - - // Cancel commands - controller.get(RB).onTrue(new InstantCommand(() -> { - getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(() -> null); - CommandScheduler.getInstance().cancelAll(); - })); - - // Align wheels - controller.get(DPad.RIGHT).onTrue(new FunctionalCommand( - () -> getDrivetrain().setStateDeadband(false), - getDrivetrain()::alignWheels, - interrupted -> getDrivetrain().setStateDeadband(true), - () -> false, getDrivetrain()).withTimeout(2)); - - // Trench align - controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> { - getDrivetrain().setTrenchAssist(true); - getDrivetrain().setTrenchAlign(true); - })) - .onFalse(new InstantCommand(() -> { - getDrivetrain().setTrenchAssist(false); - getDrivetrain().setTrenchAlign(false); + private final GameController controller = new GameController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; + private Command reverseMotors = null; + private Shooter shooter; + private Turret turret; + private Hood hood; + private Intake intake; + private Spindexer spindexer; + + // PS5 button aliases + private final Button CROSS = Button.A; + private final Button CIRCLE = Button.B; + private final Button SQUARE = Button.X; + private final Button TRIANGLE = Button.Y; + private final Button LB = Button.LB; + private final Button RB = Button.RB; + private final Button CREATE = Button.BACK; + private final Button OPTIONS = Button.START; + private final Button LEFT_JOY = Button.LEFT_JOY; + private final Button RIGHT_JOY = Button.RIGHT_JOY; + + // PS5 trigger buttons + private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON; + private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON; + + // PS5 axis aliases + private final Axis LEFT_X = Axis.LEFT_X; + private final Axis LEFT_Y = Axis.LEFT_Y; + private final Axis RIGHT_X = Axis.RIGHT_X; + private final Axis RIGHT_Y = Axis.RIGHT_Y; + private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER; + private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER; + + public PS5XboxModeDriverConfig( + Drivetrain drive, + Shooter shooter, + Turret turret, + Hood hood, + Intake intake, + Spindexer spindexer) { + super(drive); + this.shooter = shooter; + this.turret = turret; + this.hood = hood; + this.intake = intake; + this.spindexer = spindexer; + } + + public void configureControls() { + // Reset the yaw. Mainly useful for testing/driver practice + controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + controller.get(RB).onTrue(new InstantCommand(() -> { + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); })); - // Reverse motors - if (intake != null && spindexer != null && shooter != null) { - controller.get(CIRCLE).onTrue(new InstantCommand(() -> { - reverseMotors = new ReverseMotors(intake); - CommandScheduler.getInstance().schedule(reverseMotors); - })).onFalse(new InstantCommand(() -> { - if (reverseMotors != null) { - reverseMotors.cancel(); + // Align wheels + controller.get(DPad.RIGHT).onTrue(new FunctionalCommand( + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); + + // Trench align + controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> { + getDrivetrain().setTrenchAssist(true); + getDrivetrain().setTrenchAlign(true); + })) + .onFalse(new InstantCommand(() -> { + getDrivetrain().setTrenchAssist(false); + getDrivetrain().setTrenchAlign(false); + })); + + // Reverse motors + if (intake != null && spindexer != null && shooter != null) { + controller.get(CIRCLE).onTrue(new InstantCommand(() -> { + reverseMotors = new ReverseMotors(intake); + CommandScheduler.getInstance().schedule(reverseMotors); + })).onFalse(new InstantCommand(() -> { + if (reverseMotors != null) { + reverseMotors.cancel(); + } + })); + } + + // Intake + if (intake != null) { + // Toggle intake + controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { + if (intakeBoolean) { + intake.extend(); + intake.spinStart(); + intakeBoolean = false; + } else { + intake.intermediateExtend(); + intake.spinStop(); + intakeBoolean = true; + } + })); + + // Retract if hold for 3 seconds + controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> { + intake.retract(); + intakeBoolean = true; + })); + + // Calibration + controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { + intake.calibrate(); + })).onFalse(new InstantCommand(() -> { + intake.stopCalibrating(); + })); } - })); - } - // Intake - if (intake != null) { - // Toggle intake - controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { - if (intakeBoolean) { - intake.extend(); - intake.spinStart(); - intakeBoolean = false; - } else { - intake.intermediateExtend(); - intake.spinStop(); - intakeBoolean = true; + // Spindexer + if (spindexer != null) { + // Will only run if we are not calling default shoot command + controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) + .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); } - })); - - // Retract if hold for 3 seconds - controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> { - intake.retract(); - intakeBoolean = true; - })); - - // Calibration - controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { - intake.calibrate(); - })).onFalse(new InstantCommand(() -> { - intake.stopCalibrating(); - })); + + // Auto shoot + if (turret != null && hood != null && shooter != null) { + controller.get(SQUARE).onTrue( + new InstantCommand(() -> { + if (autoShoot != null && autoShoot.isScheduled()) { + autoShoot.cancel(); + } else { + autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); + CommandScheduler.getInstance().schedule(autoShoot); + } + })); + } + + // Hood + if (hood != null) { + controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { + hood.calibrate(); + })).onFalse(new InstantCommand(() -> { + hood.stopCalibrating(); + })); + } + + // Rumble test + controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup( + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)), + new WaitCommand(0.5), + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF)))); } - // Spindexer - if (spindexer != null) { - // Will only run if we are not calling default shoot command - controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) - .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); + @Override + public double getRawSideTranslation() { + return controller.get(LEFT_X); } - // Auto shoot - if (turret != null && hood != null && shooter != null) { - controller.get(SQUARE).onTrue( - new InstantCommand(() -> { - if (autoShoot != null && autoShoot.isScheduled()) { - autoShoot.cancel(); - } else { - autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer); - CommandScheduler.getInstance().schedule(autoShoot); - } - })); + @Override + public double getRawForwardTranslation() { + return controller.get(LEFT_Y); } - // Climb + @Override + public double getRawRotation() { + return controller.get(RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y)); + } - // Hood - if (hood != null) { - controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { - hood.calibrate(); - })).onFalse(new InstantCommand(() -> { - hood.stopCalibrating(); - })); + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); } - // Rumble test - controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup( - new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)), - new WaitCommand(0.5), - new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF)))); - } - - @Override - public double getRawSideTranslation() { - return controller.get(LEFT_X); - } - - @Override - public double getRawForwardTranslation() { - return controller.get(LEFT_Y); - } - - @Override - public double getRawRotation() { - return controller.get(RIGHT_X); - } - - @Override - public double getRawHeadingAngle() { - return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2; - } - - @Override - public double getRawHeadingMagnitude() { - return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y)); - } - - @Override - public boolean getIsSlowMode() { - return slowModeSupplier.getAsBoolean(); - } - - @Override - public boolean getIsAlign() { - return false; - } - - public void startRumble() { - controller.setRumble(GameController.RumbleStatus.RUMBLE_ON); - } - - public void endRumble() { - controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF); - } + @Override + public boolean getIsAlign() { + return false; + } + + public void startRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_ON); + } + + public void endRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF); + } } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Module.java b/src/main/java/frc/robot/subsystems/drivetrain/Module.java index 148785c..82d235d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Module.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Module.java @@ -41,441 +41,438 @@ import frc.robot.constants.swerve.ModuleType; import frc.robot.util.PhoenixOdometryThread; import lib.CTREModuleState; -public class Module implements ModuleIO { - private final ModuleType type; - - // Degrees - private final double angleOffset; - - private final TalonFX angleMotor; - private final TalonFX driveMotor; - private final CANcoder CANcoder; - private SwerveModuleState desiredState; - - protected boolean stateDeadband = true; - - private boolean optimizeStates = true; - - // Inputs from drive motor - private final StatusSignal drivePosition; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - // Inputs from turn motor - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Timestamp inputs from Phoenix thread - protected final Queue timestampQueue; - protected final Queue drivePositionQueue; - protected final Queue turnPositionQueue; - - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - - // Connection debouncers - private final Debouncer driveConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnConnectedDebounce = new Debouncer(0.5); - private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); - - private final Alert driveDisconnectedAlert; - private final Alert turnDisconnectedAlert; - private final Alert turnEncoderDisconnectedAlert; - - protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - - private ModuleConstants moduleConstants; - private final MotionMagicVelocityVoltage velocityRequest = new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); - - public Module(ModuleConstants moduleConstants) { - this.moduleConstants = moduleConstants; - - type = moduleConstants.getType(); - angleOffset = moduleConstants.getSteerOffset(); - - /* Angle Encoder Config */ - CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN); - /* Angle Motor Config */ - angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN); - driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN); - // Create drive status signals - drivePosition = driveMotor.getPosition(); - driveVelocity = driveMotor.getVelocity(); - driveAppliedVolts = driveMotor.getMotorVoltage(); - driveCurrent = driveMotor.getStatorCurrent(); - - // Create turn status signals - turnAbsolutePosition = CANcoder.getAbsolutePosition(); - turnPosition = angleMotor.getPosition(); - turnVelocity = angleMotor.getVelocity(); - turnAppliedVolts = angleMotor.getMotorVoltage(); - turnCurrent = angleMotor.getStatorCurrent(); - - // Create timestamp queue - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); - drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition()); - turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition()); - updateInputs(); - - configCANcoder(); - configAngleMotor(); - configDriveMotor(); - - driveDisconnectedAlert = new Alert( - "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", - AlertType.kError); - turnDisconnectedAlert = new Alert( - "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError); - turnEncoderDisconnectedAlert = new Alert( - "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".", - AlertType.kError); - - // Configure periodic frames - BaseStatusSignal.setUpdateFrequencyForAll( - 250, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - setDesiredState(new SwerveModuleState(0, getAngle()), false); - } - - public void close() { - angleMotor.close(); - driveMotor.close(); - CANcoder.close(); - } - - @Override - public void updateInputs() { - // Refresh all signals - var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); - var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); - - // Update drive inputs - inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); - inputs.drivePositionRad = Units - .rotationsToRadians(drivePosition.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO); - inputs.driveVelocityRadPerSec = Units - .rotationsToRadians(driveVelocity.getValueAsDouble() / DriveConstants.DRIVE_GEAR_RATIO); - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); - - // Update turn inputs - inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); - inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); - inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); - inputs.turnPosition = Rotation2d - .fromRotations(turnPosition.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio); - inputs.turnVelocityRadPerSec = Units - .rotationsToRadians(turnVelocity.getValueAsDouble() / DriveConstants.MODULE_CONSTANTS.angleGearRatio); - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); - - // Update encoder inputs - inputs.encoderOffset = inputs.turnAbsolutePosition.getDegrees(); + +public class Module implements ModuleIO{ + private final ModuleType type; + + // Degrees + private final double angleOffset; + + private final TalonFX angleMotor; + private final TalonFX driveMotor; + private final CANcoder CANcoder; + private SwerveModuleState desiredState; + + protected boolean stateDeadband = true; + + private boolean optimizeStates = true; + + // Inputs from drive motor + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + // Inputs from turn motor + private final StatusSignal turnAbsolutePosition; + private final StatusSignal turnPosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + + // Timestamp inputs from Phoenix thread + protected final Queue timestampQueue; + protected final Queue drivePositionQueue; + protected final Queue turnPositionQueue; + + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + + // Connection debouncers + private final Debouncer driveConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnConnectedDebounce = new Debouncer(0.5); + private final Debouncer turnEncoderConnectedDebounce = new Debouncer(0.5); + + private final Alert driveDisconnectedAlert; + private final Alert turnDisconnectedAlert; + private final Alert turnEncoderDisconnectedAlert; + + protected final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); + + private ModuleConstants moduleConstants; + private final MotionMagicVelocityVoltage velocityRequest = + new MotionMagicVelocityVoltage(0.0).withUpdateFreqHz(0); + + + public Module(ModuleConstants moduleConstants) { + this.moduleConstants = moduleConstants; + + type = moduleConstants.getType(); + angleOffset = moduleConstants.getSteerOffset(); + + /* Angle Encoder Config */ + CANcoder = new CANcoder(moduleConstants.getEncoderPort(), DriveConstants.STEER_ENCODER_CAN); + /* Angle Motor Config */ + angleMotor = new TalonFX(moduleConstants.getSteerPort(), DriveConstants.STEER_ENCODER_CAN); + driveMotor = new TalonFX(moduleConstants.getDrivePort(), DriveConstants.DRIVE_MOTOR_CAN); + // Create drive status signals + drivePosition = driveMotor.getPosition(); + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveCurrent = driveMotor.getStatorCurrent(); + + // Create turn status signals + turnAbsolutePosition = CANcoder.getAbsolutePosition(); + turnPosition = angleMotor.getPosition(); + turnVelocity = angleMotor.getVelocity(); + turnAppliedVolts = angleMotor.getMotorVoltage(); + turnCurrent = angleMotor.getStatorCurrent(); + + // Create timestamp queue + timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveMotor.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(angleMotor.getPosition()); + updateInputs(); + + configCANcoder(); + configAngleMotor(); + configDriveMotor(); + + driveDisconnectedAlert = + new Alert( + "Disconnected drive motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + turnDisconnectedAlert = + new Alert( + "Disconnected turn motor on module " + Integer.toString(moduleConstants.ordinal()) + ".", AlertType.kError); + turnEncoderDisconnectedAlert = + new Alert( + "Disconnected turn encoder on module " + Integer.toString(moduleConstants.ordinal()) + ".", + AlertType.kError); + + + // Configure periodic frames + BaseStatusSignal.setUpdateFrequencyForAll( + 250, drivePosition, turnPosition); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnAbsolutePosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + setDesiredState(new SwerveModuleState(0, getAngle()), false); + } + + public void close() { + angleMotor.close(); + driveMotor.close(); + CANcoder.close(); + } + + @Override + public void updateInputs() { + // Refresh all signals + var driveStatus = + BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + var turnStatus = + BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); + + // Update drive inputs + inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK()); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()/DriveConstants.DRIVE_GEAR_RATIO); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + + // Update turn inputs + inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); + inputs.turnEncoderConnected = turnEncoderConnectedDebounce.calculate(turnEncoderStatus.isOK()); + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()/DriveConstants.MODULE_CONSTANTS.angleGearRatio); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + + // Update encoder inputs + inputs.encoderOffset = inputs.turnAbsolutePosition.getDegrees(); // Update odometry inputs - inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) - .toArray(); - inputs.odometryTurnPositions = turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value)) - .toArray(Rotation2d[]::new); + inputs.odometryTimestamps = + timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value)) + .toArray(Rotation2d[]::new); timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); - } - - public void periodic() { - updateInputs(); - Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); - - // Calculate positions for odometry - int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together - odometryPositions = new SwerveModulePosition[sampleCount]; - for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] / DriveConstants.DRIVE_GEAR_RATIO - * DriveConstants.WHEEL_RADIUS; - Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio); - odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); - } - // Update alerts - driveDisconnectedAlert.set(!inputs.driveConnected); - turnDisconnectedAlert.set(!inputs.turnConnected); - turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Angle " + moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); - } - } - - public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { - // Separate if here and in setAngle() to avoid warning - if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { - /* - * This is a custom optimize function, since default WPILib optimize assumes - * continuous controller which CTRE and Rev onboard is not - */ - desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState; - } else { - desiredState = wantedState; - } - setAngle(); - setSpeed(isOpenLoop); - } - - public void setSpeed(boolean isOpenLoop) { - if (desiredState == null) { - return; - } - if (isOpenLoop) { - double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED; - driveMotor.setControl(new DutyCycleOut(percentOutput)); - } else { - double velocity = desiredState.speedMetersPerSecond / DriveConstants.WHEEL_RADIUS / 2 / Math.PI - * DriveConstants.DRIVE_GEAR_RATIO; - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); - } + } + + public void periodic() { + updateInputs(); + Logger.processInputs("Drive/Module" + Integer.toString(moduleConstants.ordinal()), inputs); + + // Calculate positions for odometry + int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = inputs.odometryDrivePositionsRad[i]/DriveConstants.DRIVE_GEAR_RATIO * DriveConstants.WHEEL_RADIUS; + Rotation2d angle = inputs.odometryTurnPositions[i].div(DriveConstants.MODULE_CONSTANTS.angleGearRatio); + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + // Update alerts + driveDisconnectedAlert.set(!inputs.driveConnected); + turnDisconnectedAlert.set(!inputs.turnConnected); + turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("Angle "+ moduleConstants.ordinal(), MathUtil.inputModulus(getAngle().getDegrees(), 0, 360)); + } + } + + public void setDesiredState(SwerveModuleState wantedState, boolean isOpenLoop) { + // Separate if here and in setAngle() to avoid warning + if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ + /* + * This is a custom optimize function, since default WPILib optimize assumes + * continuous controller which CTRE and Rev onboard is not + */ + desiredState = optimizeStates ? CTREModuleState.optimize(wantedState, getState().angle) : wantedState; + }else{ + desiredState = wantedState; + } + setAngle(); + setSpeed(isOpenLoop); + } + + public void setSpeed(boolean isOpenLoop) { + if(desiredState == null){ + return; + } + if (isOpenLoop) { + double percentOutput = desiredState.speedMetersPerSecond / DriveConstants.MAX_SPEED; + driveMotor.setControl(new DutyCycleOut(percentOutput)); + } else { + double velocity = desiredState.speedMetersPerSecond/DriveConstants.WHEEL_RADIUS/2/Math.PI*DriveConstants.DRIVE_GEAR_RATIO; + if (!Constants.DISABLE_LOGGING) { + Logger.recordOutput("desired vel" + moduleConstants.ordinal(), velocity); + } + + double feedforward = velocity * moduleConstants.getDriveV(); + driveMotor.setControl( + velocityRequest + .withVelocity(velocity) + .withFeedForward(feedforward)); + } + } + + private void setAngle() { + if(!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION){ + // Prevent rotating module if desired speed < 1%. Prevents jittering and unnecessary movement. + if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) { + stop(); + return; + } + } + if(desiredState == null){ + return; + } + angleMotor.setControl(new PositionDutyCycle(desiredState.angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setDriveVoltage(Voltage voltage){ + driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude())); + } + public void setAngle(Rotation2d angle){ + angleMotor.setControl(new PositionDutyCycle(angle.getRotations()*DriveConstants.MODULE_CONSTANTS.angleGearRatio)); + } + + public void setOptimize(boolean enable) { + optimizeStates = enable; + } + + public byte getModuleIndex() { + return type.id; + } + + public Rotation2d getAngle() { + return inputs.turnPosition; + } + + public Rotation2d getCANcoder() { + return inputs.turnAbsolutePosition; + } + + public void resetToAbsolute() { + // Sensor ticks + double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset); + angleMotor.setPosition(absolutePosition*DriveConstants.MODULE_CONSTANTS.angleGearRatio); + } + + private void configCANcoder() { + CANcoder.getConfigurator().apply(new CANcoderConfiguration()); + CANcoder.getConfigurator().apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1). + withSensorDirection(DriveConstants.MODULE_CONSTANTS.canCoderInvert?SensorDirectionValue.Clockwise_Positive:SensorDirectionValue.CounterClockwise_Positive)); + } + + private void configAngleMotor() { + angleMotor.getConfigurator().apply(new TalonFXConfiguration()); + + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; + angleMotor.getConfigurator().apply(config); + angleMotor.getConfigurator().apply(new Slot0Configs() + .withKP(DriveConstants.MODULE_CONSTANTS.angleKP) + .withKI(DriveConstants.MODULE_CONSTANTS.angleKI) + .withKD(DriveConstants.MODULE_CONSTANTS.angleKD)); + angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR)); + angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE); + angleMotor.setPosition(0); + + // optimize bus utilization for angle motor + angleMotor.optimizeBusUtilization(); + + resetToAbsolute(); + } + + /** + * @return Speed in RPM + */ + public double getDriveVelocity() { + return inputs.driveVelocityRadPerSec*60/DriveConstants.MODULE_CONSTANTS.driveGearRatio/2/Math.PI; + } + + public double getDriveVoltage(){ + return inputs.driveAppliedVolts; + } + + public double getDriveStatorCurrent(){ + return inputs.driveCurrentAmps; + } + + // I took the config things straight from this file + public void setNewCurrentLimit(double currentSteer, double currentDrive) { + CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); + // steer + steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; + steerConfig.SupplyCurrentLimit = currentSteer; + steerConfig.SupplyCurrentLowerLimit = currentSteer; + steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; + angleMotor.getConfigurator().apply(steerConfig); // apply + + // drive + CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs(); + driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + driveConfig.SupplyCurrentLimit = currentDrive; + driveConfig.SupplyCurrentLowerLimit = currentDrive; + driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; + driveConfig.StatorCurrentLimit = currentDrive; + driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + driveMotor.getConfigurator().apply(driveConfig); // apply + } + + private void configDriveMotor() { + var talonFXConfigs = new TalonFXConfiguration(); + // set Motion Magic settings + var motionMagicConfigs = talonFXConfigs.MotionMagic; + motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; + motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL/DriveConstants.WHEEL_CIRCUMFERENCE * DriveConstants.DRIVE_GEAR_RATIO; + var slot0Configs = talonFXConfigs.Slot0; + slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction + slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output + slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output + slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output + slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error + slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output + driveMotor.getConfigurator().apply(talonFXConfigs); + CurrentLimitsConfigs config = new CurrentLimitsConfigs(); + config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT; + config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; + config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; + config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; + driveMotor.getConfigurator().apply(config); + driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR)); + driveMotor.getConfigurator().apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); + driveMotor.getConfigurator().apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP)); + driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE); + + // optimize bus utilization for drive motor + driveMotor.optimizeBusUtilization(); + + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + inputs.driveVelocityRadPerSec*DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + inputs.drivePositionRad*DriveConstants.WHEEL_RADIUS, + getAngle()); + } + + public SwerveModuleState getDesiredState() { + return desiredState; + } + + + public double getDriveVelocityError() { + return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond; + } + + public void stop() { + driveMotor.set(0); + angleMotor.set(0); + } + + public ModuleType getModuleType(){ + return type; + } + + public void setStateDeadband(boolean enabled) { + stateDeadband = enabled; + } - double feedforward = velocity * moduleConstants.getDriveV(); - driveMotor.setControl( - velocityRequest - .withVelocity(velocity) - .withFeedForward(feedforward)); - } - } - - private void setAngle() { - if (!DriveConstants.DISABLE_DEADBAND_AND_OPTIMIZATION) { - // Prevent rotating module if desired speed < 1%. Prevents jittering and - // unnecessary movement. - if (stateDeadband && (Math.abs(desiredState.speedMetersPerSecond) <= (DriveConstants.MAX_SPEED * 0.01))) { - stop(); - return; + public double getDesiredVelocity() { + return getDesiredState().speedMetersPerSecond; } + + public Rotation2d getDesiredAngle() { + return getDesiredState().angle; + } + + /** Returns the module positions received this cycle. */ + public SwerveModulePosition[] getOdometryPositions() { + return odometryPositions; + } + + /** Returns the timestamps of the samples received this cycle. */ + public double[] getOdometryTimestamps() { + return inputs.odometryTimestamps; + } + + /** returns the drive position status signal for time-synced odometry. */ + public StatusSignal getDrivePositionSignal() { + return drivePosition; + } + + /** returns the turn position status signal for time-synced odometry. */ + public StatusSignal getTurnPositionSignal() { + return turnPosition; + } + + /** returns the turn absolute position status signal for time-synced odometry. */ + public StatusSignal getTurnAbsolutePositionSignal() { + return turnAbsolutePosition; + } + + public TalonFX[] getMotors() { + return new TalonFX[]{angleMotor, driveMotor}; } - if (desiredState == null) { - return; - } - angleMotor.setControl( - new PositionDutyCycle(desiredState.angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio)); - } - - public void setDriveVoltage(Voltage voltage) { - driveMotor.setControl(new VoltageOut(voltage.baseUnitMagnitude())); - } - - public void setAngle(Rotation2d angle) { - angleMotor.setControl(new PositionDutyCycle(angle.getRotations() * DriveConstants.MODULE_CONSTANTS.angleGearRatio)); - } - - public void setOptimize(boolean enable) { - optimizeStates = enable; - } - - public byte getModuleIndex() { - return type.id; - } - - public Rotation2d getAngle() { - return inputs.turnPosition; - } - - public Rotation2d getCANcoder() { - return inputs.turnAbsolutePosition; - } - - public void resetToAbsolute() { - // Sensor ticks - double absolutePosition = getCANcoder().getRotations() - Units.degreesToRotations(angleOffset); - angleMotor.setPosition(absolutePosition * DriveConstants.MODULE_CONSTANTS.angleGearRatio); - } - - private void configCANcoder() { - CANcoder.getConfigurator().apply(new CANcoderConfiguration()); - CANcoder.getConfigurator() - .apply(new MagnetSensorConfigs().withAbsoluteSensorDiscontinuityPoint(1).withSensorDirection( - DriveConstants.MODULE_CONSTANTS.canCoderInvert ? SensorDirectionValue.Clockwise_Positive - : SensorDirectionValue.CounterClockwise_Positive)); - } - - private void configAngleMotor() { - angleMotor.getConfigurator().apply(new TalonFXConfiguration()); - - CurrentLimitsConfigs config = new CurrentLimitsConfigs(); - config.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; - config.SupplyCurrentLimit = DriveConstants.STEER_CONTINUOUS_CURRENT_LIMIT; - config.SupplyCurrentLowerLimit = DriveConstants.STEER_PEAK_CURRENT_LIMIT; - config.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; - angleMotor.getConfigurator().apply(config); - angleMotor.getConfigurator().apply(new Slot0Configs() - .withKP(DriveConstants.MODULE_CONSTANTS.angleKP) - .withKI(DriveConstants.MODULE_CONSTANTS.angleKI) - .withKD(DriveConstants.MODULE_CONSTANTS.angleKD)); - angleMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_STEER_MOTOR)); - angleMotor.setNeutralMode(DriveConstants.STEER_NEUTRAL_MODE); - angleMotor.setPosition(0); - - // optimize bus utilization for angle motor - angleMotor.optimizeBusUtilization(); - - resetToAbsolute(); - } - - /** - * @return Speed in RPM - */ - public double getDriveVelocity() { - return inputs.driveVelocityRadPerSec * 60 / DriveConstants.MODULE_CONSTANTS.driveGearRatio / 2 / Math.PI; - } - - public double getDriveVoltage() { - return inputs.driveAppliedVolts; - } - - public double getDriveStatorCurrent() { - return inputs.driveCurrentAmps; - } - - // I took the config things straight from this file - public void setNewCurrentLimit(double currentSteer, double currentDrive) { - CurrentLimitsConfigs steerConfig = new CurrentLimitsConfigs(); - // steer - steerConfig.SupplyCurrentLimitEnable = DriveConstants.STEER_ENABLE_CURRENT_LIMIT; - steerConfig.SupplyCurrentLimit = currentSteer; - steerConfig.SupplyCurrentLowerLimit = currentSteer; - steerConfig.SupplyCurrentLowerTime = DriveConstants.STEER_PEAK_CURRENT_DURATION; - angleMotor.getConfigurator().apply(steerConfig); // apply - - // drive - CurrentLimitsConfigs driveConfig = new CurrentLimitsConfigs(); - driveConfig.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - driveConfig.SupplyCurrentLimit = currentDrive; - driveConfig.SupplyCurrentLowerLimit = currentDrive; - driveConfig.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; - driveConfig.StatorCurrentLimit = currentDrive; - driveConfig.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - driveMotor.getConfigurator().apply(driveConfig); // apply - } - - private void configDriveMotor() { - var talonFXConfigs = new TalonFXConfiguration(); - // set Motion Magic settings - var motionMagicConfigs = talonFXConfigs.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = DriveConstants.MAX_SPEED / DriveConstants.WHEEL_CIRCUMFERENCE - * DriveConstants.DRIVE_GEAR_RATIO; - motionMagicConfigs.MotionMagicAcceleration = DriveConstants.MAX_DRIVE_ACCEL / DriveConstants.WHEEL_CIRCUMFERENCE - * DriveConstants.DRIVE_GEAR_RATIO; - var slot0Configs = talonFXConfigs.Slot0; - slot0Configs.kS = 0; // Add 0.25 V output to overcome static friction - slot0Configs.kV = 0.11; // A velocity target of 1 rps results in 0.12 V output - slot0Configs.kA = 0.006; // An acceleration of 1 rps/s requires 0.01 V output - slot0Configs.kP = moduleConstants.getDriveP(); // A position error of 2.5 rotations results in 12 V output - slot0Configs.kI = moduleConstants.getDriveI(); // no output for integrated error - slot0Configs.kD = moduleConstants.getDriveD(); // A velocity error of 1 rps results in 0.1 V output - driveMotor.getConfigurator().apply(talonFXConfigs); - CurrentLimitsConfigs config = new CurrentLimitsConfigs(); - config.SupplyCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - config.SupplyCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; - config.SupplyCurrentLowerLimit = DriveConstants.DRIVE_PEAK_CURRENT_LIMIT; - config.SupplyCurrentLowerTime = DriveConstants.DRIVE_PEAK_CURRENT_DURATION; - config.StatorCurrentLimit = DriveConstants.DRIVE_CONTINUOUS_CURRENT_LIMIT; - config.StatorCurrentLimitEnable = DriveConstants.DRIVE_ENABLE_CURRENT_LIMIT; - driveMotor.getConfigurator().apply(config); - driveMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(DriveConstants.INVERT_DRIVE_MOTOR)); - driveMotor.getConfigurator() - .apply(new OpenLoopRampsConfigs().withDutyCycleOpenLoopRampPeriod(DriveConstants.OPEN_LOOP_RAMP)); - driveMotor.getConfigurator() - .apply(new ClosedLoopRampsConfigs().withDutyCycleClosedLoopRampPeriod(DriveConstants.CLOSE_LOOP_RAMP)); - driveMotor.setNeutralMode(DriveConstants.DRIVE_NEUTRAL_MODE); - - // optimize bus utilization for drive motor - driveMotor.optimizeBusUtilization(); - - } - - public SwerveModuleState getState() { - return new SwerveModuleState( - inputs.driveVelocityRadPerSec * DriveConstants.WHEEL_RADIUS, - getAngle()); - } - - public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - inputs.drivePositionRad * DriveConstants.WHEEL_RADIUS, - getAngle()); - } - - public SwerveModuleState getDesiredState() { - return desiredState; - } - - public double getDriveVelocityError() { - return getDesiredState().speedMetersPerSecond - getState().speedMetersPerSecond; - } - - public void stop() { - driveMotor.set(0); - angleMotor.set(0); - } - - public ModuleType getModuleType() { - return type; - } - - public void setStateDeadband(boolean enabled) { - stateDeadband = enabled; - } - - public double getDesiredVelocity() { - return getDesiredState().speedMetersPerSecond; - } - - public Rotation2d getDesiredAngle() { - return getDesiredState().angle; - } - - /** Returns the module positions received this cycle. */ - public SwerveModulePosition[] getOdometryPositions() { - return odometryPositions; - } - - /** Returns the timestamps of the samples received this cycle. */ - public double[] getOdometryTimestamps() { - return inputs.odometryTimestamps; - } - - /** returns the drive position status signal for time-synced odometry. */ - public StatusSignal getDrivePositionSignal() { - return drivePosition; - } - - /** returns the turn position status signal for time-synced odometry. */ - public StatusSignal getTurnPositionSignal() { - return turnPosition; - } - - /** - * returns the turn absolute position status signal for time-synced odometry. - */ - public StatusSignal getTurnAbsolutePositionSignal() { - return turnAbsolutePosition; - } - - public TalonFX[] getMotors() { - return new TalonFX[] { angleMotor, driveMotor }; - } } -- 2.39.5