From cebae861cd436a462e9c5abe1992a763150e0153 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Sat, 7 Mar 2026 13:06:49 -0500 Subject: [PATCH 1/9] Robot code debugging - somewhat working code --- src/main/java/frc/robot/Constants.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 40 +++++++++++++------ .../java/frc/robot/subsystems/Hopper.java | 16 +++++++- .../java/frc/robot/subsystems/Intake.java | 17 +++++--- .../java/frc/robot/subsystems/Shooter.java | 38 ++++++++++++------ 5 files changed, 80 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c23c8bb..15dc2dd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -302,7 +302,7 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0).withKS(10.0); + new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKS(0.05).withKS(10.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -653,8 +653,8 @@ public static TalonFXConfiguration getFXConfig() { } public class FeederConstants { - public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0); + public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(20.0); + public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(100.0); } public class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70a5332..f2cbbfd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,6 +50,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; + +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -103,17 +107,17 @@ public RobotContainer() { new Shooter( new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(false), + "ShooterLeftFlywheel", + ShooterFlywheelConstants.getFXConfig(true), Ports.LeftFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(true), + "ShooterRightFlywheel", + ShooterFlywheelConstants.getFXConfig(false), Ports.RightFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, + "ShooterTower", ShooterFlywheelConstants.getFXConfig(false), Ports.TowerRoller)), new RotaryMechanismReal( @@ -179,7 +183,7 @@ public RobotContainer() { new MotorIOTalonFXSim( ShooterFlywheelConstants.NAME, ShooterFlywheelConstants.getFXConfig(true), - Ports.LeftFlywheel), + Ports.RightFlywheel), ShooterFlywheelConstants.DCMOTOR, ShooterFlywheelConstants.MOI, ShooterFlywheelConstants.TOLERANCE), @@ -322,9 +326,9 @@ private void configureButtonBindings() { drive) .ignoringDisable(true)); - controller - .x() - .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); + // controller + // .x() + // .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); @@ -337,10 +341,20 @@ private void configureButtonBindings() { Robot.fuelSim.spawnStartingFuel(); intake.simBalls = 0; })); - controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); - controller.leftBumper().onTrue(intake.intake()); - controller.rightBumper().onTrue(intake.stowAndStopRollers()); - controller.a().onTrue(shooter.runFlywheel()); + // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); + // controller.a().onTrue(intake.intake()); + // controller.x().onTrue(intake.stowAndStopRollers()); + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(10))); + controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); + controller.rightBumper().whileTrue(Commands.parallel(intake.runRollers(RotationsPerSecond.of(100)), hopper.runSpindexer(100), shooter.runTower(RotationsPerSecond.of(100)))); + controller.rightBumper().onFalse(Commands.parallel(intake.runRollers(RotationsPerSecond.of(0)), hopper.runSpindexer(0), shooter.runTower(RotationsPerSecond.of(0)))); + + controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(100)))); + controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); + controller.x().whileTrue(hopper.runSpindexer(100)); + controller.x().onFalse(hopper.runSpindexer(0)); + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(100))); + controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); } /** diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 43cc31e..24cea43 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -1,14 +1,15 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; -import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.units.measure.*; -import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.robot.Constants.HopperConstants; +import frc.robot.Constants.IntakeFlywheelConstants; public class Hopper extends SubsystemBase { private FlywheelMechanism _io; @@ -27,6 +28,17 @@ public void setGoal(double position) { PIDSlot.SLOT_0); } + // Velocity of Rollers + public void setVelocity(double velocity) { + AngularVelocity angVelo = RotationsPerSecond.of(velocity); + + _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + } + + public Command runSpindexer(double velocity) { + return Commands.runOnce(() -> setVelocity(velocity)); + } + @Override public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 414cebc..81f76ff 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -36,10 +37,10 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { } // Velocity of Rollers - public void setVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); + public void setVelocity(AngularVelocity velocity) { + // AngularVelocity angVelo = RotationsPerSecond.of(velocity); - _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + _rollerIO.runVelocity(velocity, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } public Command setPivotAngle(Angle pivotAngle) { @@ -63,12 +64,16 @@ public Angle getPosition() { } public void stop() { - setVelocity(0); + setVelocity(RotationsPerSecond.of(0)); + } + + public Command runRollers(AngularVelocity velocity) { + return Commands.runOnce(() -> setVelocity(velocity)); } public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + Commands.run(() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } @@ -85,7 +90,7 @@ public boolean canIntake() { public Command stowAndStopRollers() { return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + Commands.run(() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setStowAngle(IntakePivotConstants.STOW_ANGLE)); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2cc0c7a..5d1db0d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -13,6 +13,7 @@ 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.Velocity; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -23,6 +24,7 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -49,19 +51,22 @@ public Shooter( } // Sets feeder motor speed - public void runFeeder() { + public void runFeeder(AngularVelocity velocity) { _feeder.runVelocity( - FeederConstants.FEED_SPEED, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); } // Sets the flywheel velocity based on an input. - public void setFlywheelVelocity(double velocity) { + public void setFlywheelVelocity(AngularVelocity velocity) { // store the desired velocity then send converted velocity to the mechanism - this.desiredVelo = velocity; - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - AngularVelocity negangVelo = RotationsPerSecond.of(velocity); - _lflywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - _rflywheel.runVelocity(negangVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + // this.desiredVelo = velocity; + // AngularVelocity angVelo = RotationsPerSecond.of(velocity); + // AngularVelocity negangVelo = RotationsPerSecond.of(velocity); + _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); + Logger.recordOutput("LeftFlywheel/TargetSpeed",velocity); + Logger.recordOutput("RightFlywheel/TargetSpeed",velocity); + } public enum State { @@ -125,7 +130,7 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } - public Command shoot(double velocity) { + public Command shoot(AngularVelocity velocity) { // Prepare targets return Commands.sequence( // Set and wait in parallel for both hood and flywheel @@ -133,13 +138,17 @@ public Command shoot(double velocity) { Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity), Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), // feed once ready - Commands.runOnce(() -> runFeeder()), + Commands.runOnce(() -> runFeeder(FeederConstants.FEED_SPEED)), // stop flywheel when finished - Commands.runOnce(() -> setFlywheelVelocity(0))); + Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0)))); + } + + public Command runFlywheel(AngularVelocity velocity) { + return Commands.runOnce(() -> setFlywheelVelocity(velocity)); } - public Command runFlywheel() { - return Commands.runOnce(() -> setFlywheelVelocity(1)); + public Command runTower(AngularVelocity velocity) { + return Commands.runOnce(() -> runFeeder(velocity)); } public void simShoot() { @@ -180,6 +189,9 @@ public void simShoot() { public void periodic() { _hood.periodic(); + _lflywheel.periodic(); + _rflywheel.periodic(); + _feeder.periodic(); // _feeder.periodic(); // _flywheel.periodic(); From c2b9f2860aa29f9f51b720c8d448ecdb48ce7faf Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 7 Mar 2026 17:44:53 -0500 Subject: [PATCH 2/9] Refactor subsystems and update dependencies - Updated feeder motor speed configuration in Shooter from SLOT_2 to SLOT_1. - Enhanced ModuleIOTalonFX to optimize periodic frame configurations and signal refresh priorities. - Updated vendir deos --- .../AbsoluteEncoderIOCANCoder.java | 6 +- .../DistanceSensorIOCANRange.java | 7 + .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 104 ++-- src/main/java/frc/robot/Constants.java | 53 +- src/main/java/frc/robot/RobotContainer.java | 20 +- .../frc/robot/generated/TunerConstants.java | 550 +++++++++--------- .../java/frc/robot/subsystems/Climber.java | 14 +- .../java/frc/robot/subsystems/Intake.java | 4 +- .../java/frc/robot/subsystems/Shooter.java | 19 +- .../subsystems/drive/ModuleIOTalonFX.java | 39 +- vendordeps/AdvantageKit.json | 6 +- ...enix6-26.1.0.json => Phoenix6-26.1.1.json} | 62 +- vendordeps/REVLib.json | 18 +- vendordeps/photonlib.json | 12 +- 14 files changed, 498 insertions(+), 416 deletions(-) rename vendordeps/{Phoenix6-26.1.0.json => Phoenix6-26.1.1.json} (92%) diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index d6a3641..521b665 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -41,7 +41,11 @@ public AbsoluteEncoderIOCANCoder( angle = CANCoder.getAbsolutePosition(); - updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(200)); + // Further reduce update frequency for absolute encoder since it's mainly used for initialization + updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0 + + // Optimize bus utilization + CANCoder.optimizeBusUtilization(1.0); } @Override diff --git a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java index 90c66a2..d53cbb6 100644 --- a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java +++ b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java @@ -51,6 +51,13 @@ public DistanceSensorIOCANRange(Device.CAN id, String name, CANrangeConfiguratio ambientSignal = CANRange.getAmbientSignal(); distance = CANRange.getDistance(); + + // Set very low update frequencies for distance sensors (not critical for control) + updateThread.CTRECheckErrorAndRetry(() -> + BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance)); + + // Optimize bus utilization + CANRange.optimizeBusUtilization(1.0); } @Override diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 83c744e..1623aea 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -105,6 +105,7 @@ public MotorIOTalonFX( this.name = name; motor = new TalonFX(main.id(), main.bus()); + motor.optimizeBusUtilization(); updateThread.CTRECheckErrorAndRetry(() -> motor.getConfigurator().apply(config)); // Initialize lists @@ -140,17 +141,38 @@ public MotorIOTalonFX( closedLoopReference = motor.getClosedLoopReference(); closedLoopReferenceSlope = motor.getClosedLoopReferenceSlope(); + // Set different update frequencies based on signal importance + // Critical signals for control (reduced frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 100, position, velocity, supplyCurrent, supplyCurrent, torqueCurrent, temperature)); + 50.0, position, velocity)); // Critical for control loops + // Important telemetry (lower frequency) updateThread.CTRECheckErrorAndRetry( () -> BaseStatusSignal.setUpdateFrequencyForAll( - 200, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); + 20.0, supplyCurrent, torqueCurrent)); // Important for monitoring - motor.optimizeBusUtilization(0, 1.0); + // Non-critical telemetry (very low frequency) + updateThread.CTRECheckErrorAndRetry( + () -> + BaseStatusSignal.setUpdateFrequencyForAll( + 10.0, temperature, supplyVoltage)); // Less critical + + // Control loop feedback (reduced frequency when in closed-loop) + updateThread.CTRECheckErrorAndRetry( + () -> + BaseStatusSignal.setUpdateFrequencyForAll( + 100.0, closedLoopError, closedLoopReference, closedLoopReferenceSlope)); + + // Optimize bus utilization with longer timeout for better optimization + motor.optimizeBusUtilization(1.0, 1.0); // Increased timeout for better optimization + + // Optimize follower bus utilization + for (TalonFX follower : followers) { + follower.optimizeBusUtilization(1.0, 1.0); + } } /** @@ -241,18 +263,15 @@ protected ControlType getCurrentControlType() { */ @Override public void updateInputs(MotorInputs inputs) { - inputs.connected = - BaseStatusSignal.refreshAll( - position, - velocity, - supplyVoltage, - supplyCurrent, - torqueCurrent, - temperature, - closedLoopError, - closedLoopReference, - closedLoopReferenceSlope) - .isOK(); + // Refresh signals in groups based on priority + // Refresh critical signals first + boolean criticalSignalsOk = BaseStatusSignal.refreshAll(position, velocity).isOK(); + + // Refresh telemetry signals + boolean telemetrySignalsOk = BaseStatusSignal.refreshAll( + supplyCurrent, torqueCurrent, temperature, supplyVoltage).isOK(); + + inputs.connected = criticalSignalsOk && telemetrySignalsOk; inputs.position = position.getValue(); inputs.velocity = velocity.getValue(); @@ -261,34 +280,47 @@ public void updateInputs(MotorInputs inputs) { inputs.torqueCurrent = torqueCurrent.getValue(); inputs.temperature = temperature.getValue(); - // Interpret control-loop status signals conditionally based on current mode - Double closedLoopErrorValue = closedLoopError.getValue(); - Double closedLoopTargetValue = closedLoopReference.getValue(); - + // Check current control modes boolean isRunningPositionControl = isRunningPositionControl(); boolean isRunningMotionMagic = isRunningMotionMagic(); boolean isRunningVelocityControl = isRunningVelocityControl(); - inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; - - inputs.activeTrajectoryPosition = - isRunningPositionControl && isRunningMotionMagic - ? Rotations.of(closedLoopTargetValue) - : null; - - inputs.goalPosition = isRunningPositionControl ? goalPosition : null; - - if (isRunningVelocityControl) { - inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); - } else if (isRunningPositionControl && isRunningMotionMagic) { - var targetVelocity = closedLoopReferenceSlope.getValue(); - inputs.velocityError = - RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); - inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); + // Only update closed-loop signals when in closed-loop modes + if (isRunningPositionControl || isRunningVelocityControl) { + BaseStatusSignal.refreshAll(closedLoopError, closedLoopReference, closedLoopReferenceSlope); + + // Interpret control-loop status signals conditionally based on current mode + Double closedLoopErrorValue = closedLoopError.getValue(); + Double closedLoopTargetValue = closedLoopReference.getValue(); + + inputs.positionError = isRunningPositionControl ? Rotations.of(closedLoopErrorValue) : null; + + inputs.activeTrajectoryPosition = + isRunningPositionControl && isRunningMotionMagic + ? Rotations.of(closedLoopTargetValue) + : null; + + inputs.goalPosition = isRunningPositionControl ? goalPosition : null; + + if (isRunningVelocityControl) { + inputs.velocityError = RotationsPerSecond.of(closedLoopErrorValue); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(closedLoopTargetValue); + } else if (isRunningPositionControl && isRunningMotionMagic) { + var targetVelocity = closedLoopReferenceSlope.getValue(); + inputs.velocityError = + RotationsPerSecond.of(targetVelocity - inputs.velocity.in(RotationsPerSecond)); + inputs.activeTrajectoryVelocity = RotationsPerSecond.of(targetVelocity); + } else { + inputs.velocityError = null; + inputs.activeTrajectoryVelocity = null; + } } else { + // Not in closed-loop mode, set control loop values to null + inputs.positionError = null; inputs.velocityError = null; inputs.activeTrajectoryVelocity = null; + inputs.activeTrajectoryPosition = null; + inputs.goalPosition = null; } inputs.controlType = getCurrentControlType(); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 15dc2dd..81a9de7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -654,7 +654,58 @@ public static TalonFXConfiguration getFXConfig() { public class FeederConstants { public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(20.0); - public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(100.0); + public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(50); + + public static String NAME = "ShooterTower"; + + public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); + public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); + + private static final double GEARING = (15.0 / 7.0); + + public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); + + public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); + public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0); + + // Velocity PID + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKS(0.05).withKS(10.0); + + public static TalonFXConfiguration getFXConfig(boolean invert) { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.1; + + config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal(); + config.CurrentLimits.StatorCurrentLimit = 80.0; + + config.Voltage.PeakForwardVoltage = 12.0; + config.Voltage.PeakReverseVoltage = -12.0; + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + // config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + if (invert == true) { + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + } else { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + } + + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + + config.Feedback.RotorToSensorRatio = 1.0; + + config.Feedback.SensorToMechanismRatio = GEARING; + + config.Slot0 = SLOT0CONFIG; + + return config; + } } public class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2cbbfd..910f8b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,8 @@ package frc.robot; import au.grapplerobotics.LaserCan; + +import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -71,7 +73,7 @@ public class RobotContainer { private final Hopper hopper; private final Shooter shooter; public final Intake intake; - private final BallCounter ballCounter; +// private final BallCounter ballCounter; private final Vision vision; // Controller @@ -83,8 +85,10 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Check if the robot is real before using the ball counter! - if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); - else ballCounter = null; + // if (Robot.isReal()) ballCounter = new BallCounter(new LaserCan(1)); + // else ballCounter = null; + + BaseStatusSignal.setUpdateFrequencyForAll(50.0); switch (Constants.currentMode) { case REAL: @@ -344,17 +348,17 @@ private void configureButtonBindings() { // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); // controller.a().onTrue(intake.intake()); // controller.x().onTrue(intake.stowAndStopRollers()); - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(10))); + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(20))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); controller.rightBumper().whileTrue(Commands.parallel(intake.runRollers(RotationsPerSecond.of(100)), hopper.runSpindexer(100), shooter.runTower(RotationsPerSecond.of(100)))); controller.rightBumper().onFalse(Commands.parallel(intake.runRollers(RotationsPerSecond.of(0)), hopper.runSpindexer(0), shooter.runTower(RotationsPerSecond.of(0)))); - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(100)))); + controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(20)))); controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); - controller.x().whileTrue(hopper.runSpindexer(100)); + controller.x().whileTrue(hopper.runSpindexer(50)); controller.x().onFalse(hopper.runSpindexer(0)); - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(100))); - controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(15))); + controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); } /** diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index fc20735..3a89cdf 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,309 +8,279 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; - +// // import frc.robot.subsystems.CommandSwerveDrivetrain; -// Generated by the Tuner X Swerve Project Generator +// Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(2.66) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./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.73); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; - - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 0; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 35; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.1416015625); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(12.25); - private static final Distance kFrontLeftYPos = Inches.of(10.25); - - // Front Right - private static final int kFrontRightDriveMotorId = 41; - private static final int kFrontRightSteerMotorId = 42; - private static final int kFrontRightEncoderId = 4; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.1748046875); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(12.25); - private static final Distance kFrontRightYPos = Inches.of(-10.25); - - // Back Left - private static final int kBackLeftDriveMotorId = 21; - private static final int kBackLeftSteerMotorId = 25; - private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.369384765625); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-12.25); - private static final Distance kBackLeftYPos = Inches.of(10.25); - - // Back Right - private static final int kBackRightDriveMotorId = 31; - private static final int kBackRightSteerMotorId = 32; - private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.388916015625); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-12.25); - private static final Distance kBackRightYPos = Inches.of(-10.25); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - // public static CommandSwerveDrivetrain createDrivetrain() { - // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - // ); - // } - - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

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

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

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

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

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

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } } - } } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 4aade98..1442246 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -47,15 +47,15 @@ public boolean isAboveCurrentLimit() { public void periodic() { _io.periodic(); - double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position + // double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position - // The z of the Translation3D should be - // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing - // motor configs. - Logger.recordOutput( - "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); + // // The z of the Translation3D should be + // // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing + // // motor configs. + // Logger.recordOutput( + // "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + // _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } public Command runClimber() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 81f76ff..6c42ce8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -107,8 +107,8 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) - _pivotIO.runVoltage(Volts.of(0.25)); + // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) + // _pivotIO.runVoltage(Volts.of(0.25)); _pivotIO.periodic(); Logger.recordOutput( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5d1db0d..2befcda 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -53,7 +53,7 @@ public Shooter( // Sets feeder motor speed public void runFeeder(AngularVelocity velocity) { _feeder.runVelocity( - velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_2); + velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_1); } // Sets the flywheel velocity based on an input. @@ -144,10 +144,13 @@ public Command shoot(AngularVelocity velocity) { } public Command runFlywheel(AngularVelocity velocity) { + System.out.println("Flywheel"); + return Commands.runOnce(() -> setFlywheelVelocity(velocity)); } public Command runTower(AngularVelocity velocity) { + System.out.println("Tower"); return Commands.runOnce(() -> runFeeder(velocity)); } @@ -195,16 +198,16 @@ public void periodic() { // _feeder.periodic(); // _flywheel.periodic(); - double pitch = - Math.toRadians( - Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position + // double pitch = + // Math.toRadians( + // Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing // motor configs. - Logger.recordOutput( - "3DField/3_Hood", - new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); + // Logger.recordOutput( + // "3DField/3_Hood", + // new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); + // _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index bb6a6d6..96e79e2 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -172,28 +172,39 @@ public ModuleIOTalonFX( turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); - // Configure periodic frames + // Configure periodic frames with optimized frequencies + // Odometry signals need high frequency for accurate pose estimation BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); + + // Control signals need reduced frequency BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); + 50.0, driveVelocity, turnVelocity); + + // Telemetry signals can use very low frequency + BaseStatusSignal.setUpdateFrequencyForAll( + 10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent); + + // Absolute encoder only needs very low updates since it's mainly used for initialization + BaseStatusSignal.setUpdateFrequencyForAll(20.0, turnAbsolutePosition); + + // Optimize bus utilization for each motor with longer timeout for better optimization ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); + cancoder.optimizeBusUtilization(1.0); } @Override public void updateInputs(ModuleIOInputs inputs) { - // Refresh all signals - var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); - var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + // Refresh signals in priority groups + // Refresh critical odometry signals first + var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity); + var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity); + + // Refresh telemetry signals + BaseStatusSignal.refreshAll(driveAppliedVolts, driveCurrent); + BaseStatusSignal.refreshAll(turnAppliedVolts, turnCurrent); + + // Refresh absolute encoder with lower priority var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); // Update drive inputs diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 162ad66..868ae9d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6-26.1.0.json b/vendordeps/Phoenix6-26.1.1.json similarity index 92% rename from vendordeps/Phoenix6-26.1.0.json rename to vendordeps/Phoenix6-26.1.1.json index dc5dc62..7a0eca0 100644 --- a/vendordeps/Phoenix6-26.1.0.json +++ b/vendordeps/Phoenix6-26.1.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.1.0.json", + "fileName": "Phoenix6-26.1.1.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.1.1", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.1.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.1.1", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..e8196c3 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.4", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.4", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 0a948bd..62f135c 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", + "version": "v2026.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-2" + "version": "v2026.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-2" + "version": "v2026.3.1" } ] } \ No newline at end of file From 4c3b9d2794a9c4ee226cde37589cdd550105f35b Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 8 Mar 2026 12:29:40 -0400 Subject: [PATCH 3/9] Adding changes from Saturday - robot works now! --- .../java/frc/lib/W8/io/motor/MotorIO.java | 2 +- src/main/java/frc/robot/Constants.java | 39 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 11 +++--- .../java/frc/robot/subsystems/Hopper.java | 3 +- .../java/frc/robot/subsystems/Intake.java | 2 +- .../java/frc/robot/subsystems/Shooter.java | 6 +-- 6 files changed, 35 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIO.java b/src/main/java/frc/lib/W8/io/motor/MotorIO.java index adc1b87..e657ba3 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIO.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIO.java @@ -42,7 +42,7 @@ public interface MotorIO { @Getter @AllArgsConstructor public enum PIDSlot { - SLOT_0(0), + SLOT_0(0), SLOT_1(1), SLOT_2(2); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 81a9de7..dfbe034 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -227,21 +227,21 @@ public static TalonFXConfiguration getFXConfig() { config.Voltage.PeakReverseVoltage = -12.0; config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(5).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -302,7 +302,7 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKS(0.05).withKS(10.0); + new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(10.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -542,6 +542,11 @@ public static VisionSystemSim getSystemSim() { } } + // config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + private static Slot0Configs SLOT0CONFIG = + new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(10.0); + + public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -559,19 +564,19 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = SLOT0CONFIG; config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -670,7 +675,7 @@ public class FeederConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKS(0.05).withKS(10.0); + new Slot0Configs().withKP(10.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(12.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 910f8b2..d5e5cb7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,7 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.FeederConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; @@ -122,7 +123,7 @@ public RobotContainer() { new FlywheelMechanismReal( new MotorIOTalonFX( "ShooterTower", - ShooterFlywheelConstants.getFXConfig(false), + FeederConstants.getFXConfig(false), Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( @@ -348,16 +349,16 @@ private void configureButtonBindings() { // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); // controller.a().onTrue(intake.intake()); // controller.x().onTrue(intake.stowAndStopRollers()); - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(20))); + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(5))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - controller.rightBumper().whileTrue(Commands.parallel(intake.runRollers(RotationsPerSecond.of(100)), hopper.runSpindexer(100), shooter.runTower(RotationsPerSecond.of(100)))); + controller.rightBumper().whileTrue(Commands.parallel(intake.runRollers(RotationsPerSecond.of(20)), hopper.runSpindexer(20), shooter.runTower(RotationsPerSecond.of(50)))); controller.rightBumper().onFalse(Commands.parallel(intake.runRollers(RotationsPerSecond.of(0)), hopper.runSpindexer(0), shooter.runTower(RotationsPerSecond.of(0)))); controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(20)))); controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); - controller.x().whileTrue(hopper.runSpindexer(50)); + controller.x().whileTrue(hopper.runSpindexer(20)); controller.x().onFalse(hopper.runSpindexer(0)); - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(15))); + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(20))); controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); } diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 24cea43..a93d90b 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -36,8 +36,9 @@ public void setVelocity(double velocity) { } public Command runSpindexer(double velocity) { - return Commands.runOnce(() -> setVelocity(velocity)); + return Commands.runOnce(() -> setVelocity(velocity), this); } + @Override public void periodic() {} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6c42ce8..297f257 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -68,7 +68,7 @@ public void stop() { } public Command runRollers(AngularVelocity velocity) { - return Commands.runOnce(() -> setVelocity(velocity)); + return Commands.run(() -> setVelocity(velocity), this); } public Command intake() { diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 2befcda..7c28b8e 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -53,7 +53,7 @@ public Shooter( // Sets feeder motor speed public void runFeeder(AngularVelocity velocity) { _feeder.runVelocity( - velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_1); + velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); } // Sets the flywheel velocity based on an input. @@ -146,12 +146,12 @@ public Command shoot(AngularVelocity velocity) { public Command runFlywheel(AngularVelocity velocity) { System.out.println("Flywheel"); - return Commands.runOnce(() -> setFlywheelVelocity(velocity)); + return Commands.run(() -> setFlywheelVelocity(velocity), this); } public Command runTower(AngularVelocity velocity) { System.out.println("Tower"); - return Commands.runOnce(() -> runFeeder(velocity)); + return Commands.run(() -> runFeeder(velocity), this); } public void simShoot() { From 9019da2786d1af25f3f9993db695083becf120a1 Mon Sep 17 00:00:00 2001 From: chris park Date: Sun, 8 Mar 2026 13:34:50 -0400 Subject: [PATCH 4/9] Added calibration for hood and climber + spotless apply --- .../AbsoluteEncoderIOCANCoder.java | 8 +- .../DistanceSensorIOCANRange.java | 8 +- .../java/frc/lib/W8/io/motor/MotorIO.java | 2 +- .../frc/lib/W8/io/motor/MotorIOTalonFX.java | 11 +- src/main/java/frc/robot/Constants.java | 14 +- src/main/java/frc/robot/RobotContainer.java | 45 +- .../frc/robot/generated/TunerConstants.java | 547 +++++++++--------- .../java/frc/robot/subsystems/Climber.java | 16 +- .../java/frc/robot/subsystems/Hopper.java | 4 +- .../java/frc/robot/subsystems/Intake.java | 8 +- .../java/frc/robot/subsystems/Shooter.java | 14 + .../subsystems/drive/ModuleIOTalonFX.java | 13 +- 12 files changed, 381 insertions(+), 309 deletions(-) diff --git a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java index 521b665..e915ac2 100644 --- a/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java +++ b/src/main/java/frc/lib/W8/io/absoluteencoder/AbsoluteEncoderIOCANCoder.java @@ -41,9 +41,11 @@ public AbsoluteEncoderIOCANCoder( angle = CANCoder.getAbsolutePosition(); - // Further reduce update frequency for absolute encoder since it's mainly used for initialization - updateThread.CTRECheckErrorAndRetry(() -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0 - + // Further reduce update frequency for absolute encoder since it's mainly used for + // initialization + updateThread.CTRECheckErrorAndRetry( + () -> angle.setUpdateFrequency(10.0)); // Further reduced from 25.0 + // Optimize bus utilization CANCoder.optimizeBusUtilization(1.0); } diff --git a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java index d53cbb6..02a2d54 100644 --- a/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java +++ b/src/main/java/frc/lib/W8/io/distancesensor/DistanceSensorIOCANRange.java @@ -51,11 +51,11 @@ public DistanceSensorIOCANRange(Device.CAN id, String name, CANrangeConfiguratio ambientSignal = CANRange.getAmbientSignal(); distance = CANRange.getDistance(); - + // Set very low update frequencies for distance sensors (not critical for control) - updateThread.CTRECheckErrorAndRetry(() -> - BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance)); - + updateThread.CTRECheckErrorAndRetry( + () -> BaseStatusSignal.setUpdateFrequencyForAll(10.0, ambientSignal, distance)); + // Optimize bus utilization CANRange.optimizeBusUtilization(1.0); } diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIO.java b/src/main/java/frc/lib/W8/io/motor/MotorIO.java index e657ba3..adc1b87 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIO.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIO.java @@ -42,7 +42,7 @@ public interface MotorIO { @Getter @AllArgsConstructor public enum PIDSlot { - SLOT_0(0), + SLOT_0(0), SLOT_1(1), SLOT_2(2); diff --git a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java index 1623aea..729f730 100644 --- a/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java +++ b/src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java @@ -168,7 +168,7 @@ public MotorIOTalonFX( // Optimize bus utilization with longer timeout for better optimization motor.optimizeBusUtilization(1.0, 1.0); // Increased timeout for better optimization - + // Optimize follower bus utilization for (TalonFX follower : followers) { follower.optimizeBusUtilization(1.0, 1.0); @@ -266,10 +266,11 @@ public void updateInputs(MotorInputs inputs) { // Refresh signals in groups based on priority // Refresh critical signals first boolean criticalSignalsOk = BaseStatusSignal.refreshAll(position, velocity).isOK(); - + // Refresh telemetry signals - boolean telemetrySignalsOk = BaseStatusSignal.refreshAll( - supplyCurrent, torqueCurrent, temperature, supplyVoltage).isOK(); + boolean telemetrySignalsOk = + BaseStatusSignal.refreshAll(supplyCurrent, torqueCurrent, temperature, supplyVoltage) + .isOK(); inputs.connected = criticalSignalsOk && telemetrySignalsOk; @@ -288,7 +289,7 @@ public void updateInputs(MotorInputs inputs) { // Only update closed-loop signals when in closed-loop modes if (isRunningPositionControl || isRunningVelocityControl) { BaseStatusSignal.refreshAll(closedLoopError, closedLoopReference, closedLoopReferenceSlope); - + // Interpret control-loop status signals conditionally based on current mode Double closedLoopErrorValue = closedLoopError.getValue(); Double closedLoopTargetValue = closedLoopReference.getValue(); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dfbe034..ed30f87 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,12 +13,15 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Foot; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.KilogramSquareMeters; import static edu.wpi.first.units.Units.Kilograms; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; @@ -31,7 +34,7 @@ import com.ctre.phoenix6.configs.LEDConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.*; +import com.ctre.phoenix6.controls.RainbowAnimation; import com.ctre.phoenix6.signals.Enable5VRailValue; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; @@ -284,6 +287,8 @@ public final class ShooterConstants { public static final double IDLE_HOOD_ANGLE = 25.0; // degrees public static final RotaryMechCharacteristics CONSTANTS = new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE); + + public static final double HARD_STOP_CURRENT_LIMIT = 50.0; } public class ShooterFlywheelConstants { @@ -546,7 +551,6 @@ public static VisionSystemSim getSystemSim() { private static Slot0Configs SLOT0CONFIG = new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(10.0); - public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -724,8 +728,12 @@ public class ClimberConstants { public static final Distance STARTING_DISTANCE = Inches.of(0.0); public static final Distance DRUM_RADIUS = Inches.of(2.0); public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); + public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(2 * Math.PI).times(10.0); + public static final AngularVelocity CALIBRATE_VELOCITY = + RadiansPerSecond.of(-2 * Math.PI).times(1.0); + public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); public static final Distance BOTTOM = Inches.of(0.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d5e5cb7..0c08dba 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,7 +13,7 @@ package frc.robot; -import au.grapplerobotics.LaserCan; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; @@ -42,7 +42,6 @@ import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; -import frc.robot.subsystems.BallCounter; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -53,10 +52,6 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; - -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -74,7 +69,7 @@ public class RobotContainer { private final Hopper hopper; private final Shooter shooter; public final Intake intake; -// private final BallCounter ballCounter; + // private final BallCounter ballCounter; private final Vision vision; // Controller @@ -122,9 +117,7 @@ public RobotContainer() { Ports.RightFlywheel)), new FlywheelMechanismReal( new MotorIOTalonFX( - "ShooterTower", - FeederConstants.getFXConfig(false), - Ports.TowerRoller)), + "ShooterTower", FeederConstants.getFXConfig(false), Ports.TowerRoller)), new RotaryMechanismReal( new MotorIOTalonFX( ShooterRotaryConstants.NAME, @@ -349,16 +342,38 @@ private void configureButtonBindings() { // controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(RotationsPerSecond.of(10)))); // controller.a().onTrue(intake.intake()); // controller.x().onTrue(intake.stowAndStopRollers()); - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(5))); + + // Flywheel + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(50))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - controller.rightBumper().whileTrue(Commands.parallel(intake.runRollers(RotationsPerSecond.of(20)), hopper.runSpindexer(20), shooter.runTower(RotationsPerSecond.of(50)))); - controller.rightBumper().onFalse(Commands.parallel(intake.runRollers(RotationsPerSecond.of(0)), hopper.runSpindexer(0), shooter.runTower(RotationsPerSecond.of(0)))); + // Intake + Spindexer + Tower + controller + .rightBumper() + .whileTrue( + Commands.parallel( + intake.runRollers(RotationsPerSecond.of(30)), + hopper.runSpindexer(21), + shooter.runTower(RotationsPerSecond.of(45)))); + controller + .rightBumper() + .onFalse( + Commands.parallel( + intake.runRollers(RotationsPerSecond.of(0)), + hopper.runSpindexer(0), + shooter.runTower(RotationsPerSecond.of(0)))); + + // Intake Rollers 11 Motor: 9 Intake controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(20)))); controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); - controller.x().whileTrue(hopper.runSpindexer(20)); + + // Spindexer 1:1 + controller.x().whileTrue(hopper.runSpindexer(21)); controller.x().onFalse(hopper.runSpindexer(0)); - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(20))); + + // Tower - 15 Motor:7 Tower + + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(50))); controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 3a89cdf..c891c3c 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -8,279 +8,310 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; + // // import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; - - private static final double kDriveGearRatio = 6.122448979591837; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 0; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 35; - private static final int kFrontLeftSteerMotorId = 12; - private static final int kFrontLeftEncoderId = 1; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.06396484375); - private static final boolean kFrontLeftSteerMotorInverted = true; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10.75); - private static final Distance kFrontLeftYPos = Inches.of(10.75); - - // Front Right - private static final int kFrontRightDriveMotorId = 41; - private static final int kFrontRightSteerMotorId = 42; - private static final int kFrontRightEncoderId = 4; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.27783203125); - private static final boolean kFrontRightSteerMotorInverted = true; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10.75); - private static final Distance kFrontRightYPos = Inches.of(-10.75); - - // Back Left - private static final int kBackLeftDriveMotorId = 21; - private static final int kBackLeftSteerMotorId = 25; - private static final int kBackLeftEncoderId = 2; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.380859375); - private static final boolean kBackLeftSteerMotorInverted = true; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10.75); - private static final Distance kBackLeftYPos = Inches.of(10.75); - - // Back Right - private static final int kBackRightDriveMotorId = 31; - private static final int kBackRightSteerMotorId = 32; - private static final int kBackRightEncoderId = 3; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.11279296875); - private static final boolean kBackRightSteerMotorInverted = true; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10.75); - private static final Distance kBackRightYPos = Inches.of(-10.75); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.66) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.04); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 3.5714285714285716; + + private static final double kDriveGearRatio = 6.122448979591837; + private static final double kSteerGearRatio = 21.428571428571427; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 35; + private static final int kFrontLeftSteerMotorId = 12; + private static final int kFrontLeftEncoderId = 1; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.06396484375); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.75); + private static final Distance kFrontLeftYPos = Inches.of(10.75); + + // Front Right + private static final int kFrontRightDriveMotorId = 41; + private static final int kFrontRightSteerMotorId = 42; + private static final int kFrontRightEncoderId = 4; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.27783203125); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.75); + private static final Distance kFrontRightYPos = Inches.of(-10.75); + + // Back Left + private static final int kBackLeftDriveMotorId = 21; + private static final int kBackLeftSteerMotorId = 25; + private static final int kBackLeftEncoderId = 2; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.380859375); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.75); + private static final Distance kBackLeftYPos = Inches.of(10.75); + + // Back Right + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 32; + private static final int kBackRightEncoderId = 3; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.11279296875); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.75); + private static final Distance kBackRightYPos = Inches.of(-10.75); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ + // public static CommandSwerveDrivetrain createDrivetrain() { + // return new CommandSwerveDrivetrain( + // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + // ); + // } + + /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

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

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

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

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

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

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); } + } } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 1442246..ee7bc6c 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,19 +2,13 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; import frc.robot.Constants.ClimberConstants; -import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -69,6 +63,16 @@ public Command runClimber() { PIDSlot.SLOT_0)); } + public Command calibrateClimber() { + return this.run( + () -> + _io.runVelocity( + ClimberConstants.CALIBRATE_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index a93d90b..c021895 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -9,7 +9,6 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.robot.Constants.HopperConstants; -import frc.robot.Constants.IntakeFlywheelConstants; public class Hopper extends SubsystemBase { private FlywheelMechanism _io; @@ -36,9 +35,8 @@ public void setVelocity(double velocity) { } public Command runSpindexer(double velocity) { - return Commands.runOnce(() -> setVelocity(velocity), this); + return Commands.runOnce(() -> setVelocity(velocity), this); } - @Override public void periodic() {} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 297f257..398bb24 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,9 +2,7 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -73,7 +71,8 @@ public Command runRollers(AngularVelocity velocity) { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), + Commands.run( + () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } @@ -90,7 +89,8 @@ public boolean canIntake() { public Command stowAndStopRollers() { return Commands.sequence( - Commands.run(() -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), + Commands.run( + () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), setStowAngle(IntakePivotConstants.STOW_ANGLE)); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7c28b8e..4301e4d 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; @@ -130,6 +131,19 @@ public boolean hoodAtAngle() { return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE; } + public boolean isAboveCurrentLimit() { + if (_hood.getSupplyCurrent().in(Amps) > ShooterConstants.HARD_STOP_CURRENT_LIMIT) { + return true; + } else { + return false; + } + } + + public Command calibrateHood() { + return this.run(() -> _hood.runVelocity(ShooterConstants.HOOD_VELOCITY, ShooterConstants.HOOD_ACCELERATION, PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + public Command shoot(AngularVelocity velocity) { // Prepare targets return Commands.sequence( diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 96e79e2..39878e4 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -176,15 +176,14 @@ public ModuleIOTalonFX( // Odometry signals need high frequency for accurate pose estimation BaseStatusSignal.setUpdateFrequencyForAll( Drive.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - + // Control signals need reduced frequency - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, driveVelocity, turnVelocity); - + BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, turnVelocity); + // Telemetry signals can use very low frequency BaseStatusSignal.setUpdateFrequencyForAll( 10.0, driveAppliedVolts, driveCurrent, turnAppliedVolts, turnCurrent); - + // Absolute encoder only needs very low updates since it's mainly used for initialization BaseStatusSignal.setUpdateFrequencyForAll(20.0, turnAbsolutePosition); @@ -199,11 +198,11 @@ public void updateInputs(ModuleIOInputs inputs) { // Refresh critical odometry signals first var driveStatus = BaseStatusSignal.refreshAll(drivePosition, driveVelocity); var turnStatus = BaseStatusSignal.refreshAll(turnPosition, turnVelocity); - + // Refresh telemetry signals BaseStatusSignal.refreshAll(driveAppliedVolts, driveCurrent); BaseStatusSignal.refreshAll(turnAppliedVolts, turnCurrent); - + // Refresh absolute encoder with lower priority var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); From 6c6af581d28bfcb894db543edd6d748a6f02ad12 Mon Sep 17 00:00:00 2001 From: chris park Date: Sun, 8 Mar 2026 13:41:12 -0400 Subject: [PATCH 5/9] Added logger for Spindexer Supply Current --- src/main/java/frc/robot/subsystems/Hopper.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index c021895..ea23804 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -39,5 +41,7 @@ public Command runSpindexer(double velocity) { } @Override - public void periodic() {} + public void periodic() { + Logger.recordOutput("Spindexer/SupplyCurrent", _io.getSupplyCurrent()); + } } From c0796678b6dc232f06c9a147de4e1aaabe6e5959 Mon Sep 17 00:00:00 2001 From: chris park Date: Sun, 8 Mar 2026 15:54:03 -0400 Subject: [PATCH 6/9] Tuning stuff --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- src/main/java/frc/robot/subsystems/Hopper.java | 8 ++++++-- src/main/java/frc/robot/subsystems/Intake.java | 5 +++++ src/main/java/frc/robot/subsystems/Shooter.java | 6 ++++-- 5 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ed30f87..3403051 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -244,7 +244,7 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(5).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(18).withKI(0.0).withKD(0.01).withKS(2.5).withKV(0.05); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -307,7 +307,7 @@ public class ShooterFlywheelConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(10.0); + new Slot0Configs().withKP(18.0).withKI(0.0).withKD(0.01).withKV(0.05).withKS(8.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -549,7 +549,7 @@ public static VisionSystemSim getSystemSim() { // config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(25.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(10.0); + new Slot0Configs().withKP(30).withKI(0.0).withKD(0.0).withKV(0.05).withKS(8.0); public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0c08dba..42b6648 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -344,7 +344,7 @@ private void configureButtonBindings() { // controller.x().onTrue(intake.stowAndStopRollers()); // Flywheel - controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(50))); + controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(25))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); // Intake + Spindexer + Tower @@ -354,7 +354,7 @@ private void configureButtonBindings() { Commands.parallel( intake.runRollers(RotationsPerSecond.of(30)), hopper.runSpindexer(21), - shooter.runTower(RotationsPerSecond.of(45)))); + shooter.runTower(RotationsPerSecond.of(50)))); controller .rightBumper() .onFalse( @@ -364,11 +364,11 @@ private void configureButtonBindings() { shooter.runTower(RotationsPerSecond.of(0)))); // Intake Rollers 11 Motor: 9 Intake - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(20)))); +controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); // Spindexer 1:1 - controller.x().whileTrue(hopper.runSpindexer(21)); + controller.x().whileTrue(hopper.runSpindexer(18)); controller.x().onFalse(hopper.runSpindexer(0)); // Tower - 15 Motor:7 Tower diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index ea23804..8c2191a 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -15,6 +15,8 @@ public class Hopper extends SubsystemBase { private FlywheelMechanism _io; + public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); + public Hopper(FlywheelMechanism io) { _io = io; } @@ -34,6 +36,7 @@ public void setVelocity(double velocity) { AngularVelocity angVelo = RotationsPerSecond.of(velocity); _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = angVelo; } public Command runSpindexer(double velocity) { @@ -42,6 +45,7 @@ public Command runSpindexer(double velocity) { @Override public void periodic() { - Logger.recordOutput("Spindexer/SupplyCurrent", _io.getSupplyCurrent()); - } + _io.periodic(); + Logger.recordOutput("Hopper/TargetVelocity", targetVelocity); + } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 398bb24..fe0bf70 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -26,6 +26,7 @@ public class Intake extends SubsystemBase { double pivotAngle; public double desiredAngle; public int simBalls; + public AngularVelocity targetSpeed = RotationsPerSecond.of(0); public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; @@ -39,6 +40,7 @@ public void setVelocity(AngularVelocity velocity) { // AngularVelocity angVelo = RotationsPerSecond.of(velocity); _rollerIO.runVelocity(velocity, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + targetSpeed = velocity; } public Command setPivotAngle(Angle pivotAngle) { @@ -109,8 +111,11 @@ private Command setStowAngle(Angle stowAngle) { public void periodic() { // if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) // _pivotIO.runVoltage(Volts.of(0.25)); + _rollerIO.periodic(); + Logger.recordOutput("Intake/TargetSpeed", targetSpeed); _pivotIO.periodic(); + //Logger.recordOutput("Intake/TargetPivot", null); Logger.recordOutput( "3DField/1_Intake", new Pose3d( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 4301e4d..69f4e81 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -40,6 +40,8 @@ public class Shooter extends SubsystemBase { private double desiredVelo; private double hoodAngle; + public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); + public Shooter( FlywheelMechanism lflywheel, FlywheelMechanism rflywheel, @@ -65,8 +67,7 @@ public void setFlywheelVelocity(AngularVelocity velocity) { // AngularVelocity negangVelo = RotationsPerSecond.of(velocity); _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); - Logger.recordOutput("LeftFlywheel/TargetSpeed",velocity); - Logger.recordOutput("RightFlywheel/TargetSpeed",velocity); + targetVelocity = velocity; } @@ -209,6 +210,7 @@ public void periodic() { _lflywheel.periodic(); _rflywheel.periodic(); _feeder.periodic(); + Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); // _feeder.periodic(); // _flywheel.periodic(); From 06b1ea3eb1435131c9ad5e0382422dc49952f432 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 8 Mar 2026 19:15:52 -0400 Subject: [PATCH 7/9] tuning + initial climber testing --- src/main/java/frc/robot/Constants.java | 36 +++++++++------- src/main/java/frc/robot/RobotContainer.java | 43 +++++++++++++++++-- .../java/frc/robot/subsystems/Climber.java | 37 ++++++++++++++++ .../java/frc/robot/subsystems/Hopper.java | 2 +- .../java/frc/robot/subsystems/Shooter.java | 3 ++ 5 files changed, 100 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3403051..c0a54d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,7 +197,7 @@ public class HopperConstants { public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); - public static final double GEARING = (5.0 / 1.0); + public static final double GEARING = (1.0 / 1.0); public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -244,7 +244,7 @@ public static TalonFXConfiguration getFXConfig() { config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(18).withKI(0.0).withKD(0.01).withKS(2.5).withKV(0.05); + config.Slot0 = new Slot0Configs().withKP(10).withKI(0.0).withKD(0.0).withKS(2.5).withKV(0.05); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -670,7 +670,7 @@ public class FeederConstants { public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI); public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second); - private static final double GEARING = (15.0 / 7.0); + private static final double GEARING = (15.0 / 30.0); public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1); @@ -679,7 +679,7 @@ public class FeederConstants { // Velocity PID private static Slot0Configs SLOT0CONFIG = - new Slot0Configs().withKP(10.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(12.0); + new Slot0Configs().withKP(5.0).withKI(0.0).withKD(0.0).withKV(0.05).withKS(12.0); public static TalonFXConfiguration getFXConfig(boolean invert) { TalonFXConfiguration config = new TalonFXConfiguration(); @@ -721,8 +721,9 @@ public class ClimberConstants { public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1); public static final Mass CARRIAGE_MASS = Kilograms.of(2.5); public static final String MOTOR_NAME = "Climber motor"; + public static final String NAME = "Climber"; public static final Distance TOLERANCE = Inches.of(0.1); - public static final double GEARING = (5.0 / 1.0); + public static final double GEARING = 1; public static final Distance MIN_DISTANCE = Inches.of(0.0); public static final Distance MAX_DISTANCE = Inches.of(10.0); public static final Distance STARTING_DISTANCE = Inches.of(0.0); @@ -730,9 +731,11 @@ public class ClimberConstants { public static final DistanceAngleConverter CONVERTER = new DistanceAngleConverter(DRUM_RADIUS); public static final AngularVelocity CRUISE_VELOCITY = - RadiansPerSecond.of(2 * Math.PI).times(10.0); + RadiansPerSecond.of(2 * Math.PI).times(5000.0); + public static final AngularVelocity LOWER_VELOCITY = + RadiansPerSecond.of(-2 * Math.PI).times(5000.0); public static final AngularVelocity CALIBRATE_VELOCITY = - RadiansPerSecond.of(-2 * Math.PI).times(1.0); + RadiansPerSecond.of(-2 * Math.PI).times(5000.0); public static final AngularAcceleration ACCELERATION = CRUISE_VELOCITY.div(0.1).per(Second); public static final Velocity JERK = ACCELERATION.per(Second); @@ -765,19 +768,20 @@ public static TalonFXConfiguration getFXConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = - CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); - - config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = - CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + // config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = + // CONVERTER.toAngle(MAX_DISTANCE).in(Rotations); + config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false; + // config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = + // CONVERTER.toAngle(MIN_DISTANCE).in(Rotations); config.Feedback.RotorToSensorRatio = 1.0; config.Feedback.SensorToMechanismRatio = GEARING; - config.Slot0 = new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0); + config.Slot0 = new Slot0Configs().withKP(50).withKI(0.0).withKD(0.0); config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY.in(RotationsPerSecond); config.MotionMagic.MotionMagicAcceleration = ACCELERATION.in(RotationsPerSecondPerSecond); @@ -791,6 +795,6 @@ public static TalonFXConfiguration getFXConfig() { public static final AngularAcceleration ANGULAR_ACCELERATION = RotationsPerSecondPerSecond.of(1); public static final double CLIMB_SPEED = 1.0; - public static final double HARD_STOP_CURRENT_LIMIT = 50.0; + public static final double HARD_STOP_CURRENT_LIMIT = 20.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42b6648..b29e6e5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,9 +29,13 @@ import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; import frc.lib.W8.mechanisms.flywheel.*; +import frc.lib.W8.mechanisms.linear.LinearMechanism; +import frc.lib.W8.mechanisms.linear.LinearMechanismReal; +import frc.lib.W8.mechanisms.linear.LinearMechanismSim; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; +import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.FeederConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; @@ -42,6 +46,7 @@ import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -71,6 +76,7 @@ public class RobotContainer { public final Intake intake; // private final BallCounter ballCounter; private final Vision vision; + private final Climber climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -140,6 +146,14 @@ public RobotContainer() { Ports.IntakePivot), IntakePivotConstants.CONSTANTS, Optional.empty())); + climber = + new Climber( + new LinearMechanismReal( + new MotorIOTalonFX( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.CHARACTERISTICS)); vision = new Vision( new VisionIOPhotonVision( @@ -233,6 +247,17 @@ public RobotContainer() { VisionConstants.aprilTagLayout, PoseStrategy.CONSTRAINED_SOLVEPNP, VisionConstants.getSystemSim())); + climber = + new Climber( + new LinearMechanismSim( + new MotorIOTalonFXSim( + ClimberConstants.MOTOR_NAME, + ClimberConstants.getFXConfig(), + Ports.ClimberMotor), + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -259,6 +284,10 @@ public RobotContainer() { new FlywheelMechanism() {}, new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); vision = new Vision(null); + + climber = + new Climber( + new LinearMechanism(ClimberConstants.NAME, ClimberConstants.CHARACTERISTICS) {}); break; } @@ -352,9 +381,11 @@ private void configureButtonBindings() { .rightBumper() .whileTrue( Commands.parallel( - intake.runRollers(RotationsPerSecond.of(30)), - hopper.runSpindexer(21), - shooter.runTower(RotationsPerSecond.of(50)))); + // intake.runRollers(RotationsPerSecond.of(30)), + hopper.runSpindexer(15), + shooter.runTower(RotationsPerSecond.of(70)))); + + controller .rightBumper() .onFalse( @@ -373,8 +404,12 @@ private void configureButtonBindings() { // Tower - 15 Motor:7 Tower - controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(50))); + controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); + + controller.povLeft().whileTrue(climber.calibrateClimber()); + controller.povUp().whileTrue(climber.raiseClimber()); + controller.povDown().whileTrue(climber.lowerClimber()); } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index ee7bc6c..74a474b 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,9 +2,12 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; @@ -37,6 +40,13 @@ public boolean isAboveCurrentLimit() { } } + // public Command homeCommand() + // { + // return Commands.sequence( + // runOnce(() -> _io.runVoltage(Volts.of(-2))), + // Commands.waitUntil(() -> isAboveCurrentLimit())); + // } + @Override public void periodic() { _io.periodic(); @@ -64,6 +74,8 @@ public Command runClimber() { } public Command calibrateClimber() { + System.out.println(ClimberConstants.CALIBRATE_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); return this.run( () -> _io.runVelocity( @@ -73,6 +85,31 @@ public Command calibrateClimber() { .until(() -> isAboveCurrentLimit()); } + public Command raiseClimber() { + System.out.println(ClimberConstants.CRUISE_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.CRUISE_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public Command lowerClimber() { + System.out.println(ClimberConstants.LOWER_VELOCITY); + System.out.println(ClimberConstants.ACCELERATION); + return this.run( + () -> + _io.runVelocity( + ClimberConstants.LOWER_VELOCITY, + ClimberConstants.ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); + } + + public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 8c2191a..686ef08 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -47,5 +47,5 @@ public Command runSpindexer(double velocity) { public void periodic() { _io.periodic(); Logger.recordOutput("Hopper/TargetVelocity", targetVelocity); - } + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 69f4e81..339ea9b 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -41,6 +41,7 @@ public class Shooter extends SubsystemBase { private double hoodAngle; public AngularVelocity targetVelocity = RotationsPerSecond.of(0.0); + public AngularVelocity feederTargetVelocity = RotationsPerSecond.of(0.0); public Shooter( FlywheelMechanism lflywheel, @@ -57,6 +58,7 @@ public Shooter( public void runFeeder(AngularVelocity velocity) { _feeder.runVelocity( velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); + feederTargetVelocity = velocity; } // Sets the flywheel velocity based on an input. @@ -211,6 +213,7 @@ public void periodic() { _rflywheel.periodic(); _feeder.periodic(); Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); + Logger.recordOutput("Feeder/TargetVelocity", feederTargetVelocity); // _feeder.periodic(); // _flywheel.periodic(); From 7723570ee4573269ff064b45db08b18b0bf89500 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 8 Mar 2026 19:36:23 -0400 Subject: [PATCH 8/9] comment out climber button bindings --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b29e6e5..ec425df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -395,7 +395,7 @@ private void configureButtonBindings() { shooter.runTower(RotationsPerSecond.of(0)))); // Intake Rollers 11 Motor: 9 Intake -controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); + controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(15)))); controller.a().onFalse((intake.runRollers(RotationsPerSecond.of(0)))); // Spindexer 1:1 @@ -407,9 +407,9 @@ private void configureButtonBindings() { controller.y().whileTrue(shooter.runTower(RotationsPerSecond.of(70))); controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); - controller.povLeft().whileTrue(climber.calibrateClimber()); - controller.povUp().whileTrue(climber.raiseClimber()); - controller.povDown().whileTrue(climber.lowerClimber()); + // controller.povLeft().whileTrue(climber.calibrateClimber()); + // controller.povUp().whileTrue(climber.raiseClimber()); + // controller.povDown().whileTrue(climber.lowerClimber()); } /** From 5e991611ca14f2153f7c4473f1ba01e5bdd7f4a6 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 8 Mar 2026 19:43:07 -0400 Subject: [PATCH 9/9] spotless --- src/main/java/frc/robot/RobotContainer.java | 14 +++++------ .../java/frc/robot/subsystems/Climber.java | 8 +------ .../java/frc/robot/subsystems/Hopper.java | 3 +-- .../java/frc/robot/subsystems/Intake.java | 2 +- .../java/frc/robot/subsystems/Shooter.java | 23 +++++++++---------- 5 files changed, 20 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec425df..62ca824 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -153,7 +153,7 @@ public RobotContainer() { ClimberConstants.MOTOR_NAME, ClimberConstants.getFXConfig(), Ports.ClimberMotor), - ClimberConstants.CHARACTERISTICS)); + ClimberConstants.CHARACTERISTICS)); vision = new Vision( new VisionIOPhotonVision( @@ -254,10 +254,10 @@ public RobotContainer() { ClimberConstants.MOTOR_NAME, ClimberConstants.getFXConfig(), Ports.ClimberMotor), - ClimberConstants.DCMOTOR, - ClimberConstants.CARRIAGE_MASS, - ClimberConstants.CHARACTERISTICS, - false)); + ClimberConstants.DCMOTOR, + ClimberConstants.CARRIAGE_MASS, + ClimberConstants.CHARACTERISTICS, + false)); break; default: @@ -382,9 +382,7 @@ private void configureButtonBindings() { .whileTrue( Commands.parallel( // intake.runRollers(RotationsPerSecond.of(30)), - hopper.runSpindexer(15), - shooter.runTower(RotationsPerSecond.of(70)))); - + hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(70)))); controller .rightBumper() diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 74a474b..90f7371 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,12 +2,9 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; @@ -103,13 +100,10 @@ public Command lowerClimber() { return this.run( () -> _io.runVelocity( - ClimberConstants.LOWER_VELOCITY, - ClimberConstants.ACCELERATION, - PIDSlot.SLOT_0)) + ClimberConstants.LOWER_VELOCITY, ClimberConstants.ACCELERATION, PIDSlot.SLOT_0)) .until(() -> isAboveCurrentLimit()); } - public boolean nearGoalposition() { if (Math.abs( goalDistance.in(Meters) diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 686ef08..46c4b82 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -2,8 +2,6 @@ import static edu.wpi.first.units.Units.*; -import org.littletonrobotics.junction.Logger; - import edu.wpi.first.units.measure.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -11,6 +9,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.robot.Constants.HopperConstants; +import org.littletonrobotics.junction.Logger; public class Hopper extends SubsystemBase { private FlywheelMechanism _io; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index fe0bf70..224ea29 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -115,7 +115,7 @@ public void periodic() { Logger.recordOutput("Intake/TargetSpeed", targetSpeed); _pivotIO.periodic(); - //Logger.recordOutput("Intake/TargetPivot", null); + // Logger.recordOutput("Intake/TargetPivot", null); Logger.recordOutput( "3DField/1_Intake", new Pose3d( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 339ea9b..60b5ddf 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,7 +4,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -14,8 +13,6 @@ 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.Velocity; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,7 +22,6 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Robot; import org.littletonrobotics.junction.Logger; @@ -56,8 +52,7 @@ public Shooter( // Sets feeder motor speed public void runFeeder(AngularVelocity velocity) { - _feeder.runVelocity( - velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); + _feeder.runVelocity(velocity, FeederConstants.FEED_ACCELERATION, PIDSlot.SLOT_0); feederTargetVelocity = velocity; } @@ -70,7 +65,6 @@ public void setFlywheelVelocity(AngularVelocity velocity) { _lflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); _rflywheel.runVelocity(velocity, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0); targetVelocity = velocity; - } public enum State { @@ -143,8 +137,13 @@ public boolean isAboveCurrentLimit() { } public Command calibrateHood() { - return this.run(() -> _hood.runVelocity(ShooterConstants.HOOD_VELOCITY, ShooterConstants.HOOD_ACCELERATION, PIDSlot.SLOT_0)) - .until(() -> isAboveCurrentLimit()); + return this.run( + () -> + _hood.runVelocity( + ShooterConstants.HOOD_VELOCITY, + ShooterConstants.HOOD_ACCELERATION, + PIDSlot.SLOT_0)) + .until(() -> isAboveCurrentLimit()); } public Command shoot(AngularVelocity velocity) { @@ -161,7 +160,7 @@ public Command shoot(AngularVelocity velocity) { } public Command runFlywheel(AngularVelocity velocity) { - System.out.println("Flywheel"); + System.out.println("Flywheel"); return Commands.run(() -> setFlywheelVelocity(velocity), this); } @@ -212,8 +211,8 @@ public void periodic() { _lflywheel.periodic(); _rflywheel.periodic(); _feeder.periodic(); - Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); - Logger.recordOutput("Feeder/TargetVelocity", feederTargetVelocity); + Logger.recordOutput("Flywheel/TargetVelocity", targetVelocity); + Logger.recordOutput("Feeder/TargetVelocity", feederTargetVelocity); // _feeder.periodic(); // _flywheel.periodic();