From ca9b0f75acd621f578b1611db2c4f5b5cd6478f4 Mon Sep 17 00:00:00 2001 From: iefomit Date: Thu, 16 Apr 2026 19:22:06 -0700 Subject: [PATCH] bug fixes --- src/main/java/frc/robot/Robot.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 5 +---- .../java/frc/robot/commands/gpm/AutoShootCommand.java | 2 +- src/main/java/frc/robot/commands/gpm/Superstructure.java | 2 +- src/main/java/frc/robot/constants/Constants.java | 3 +++ .../frc/robot/controls/PS5ControllerDriverConfig.java | 9 ++++++--- src/main/java/frc/robot/subsystems/Intake/Intake.java | 4 ++-- src/main/java/frc/robot/subsystems/shooter/Shooter.java | 4 ++-- .../java/frc/robot/subsystems/spindexer/Spindexer.java | 6 ++++++ 9 files changed, 24 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 93fb78b..5559db2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -37,8 +37,8 @@ public class Robot extends LoggedRobot { public Robot(){ CanBridge.runTCP(); - PortForwarder.add(5800,"10.9.72.12",5800); - PortForwarder.add(1182,"10.9.72.12",1182); + PortForwarder.add(5800, Constants.VISION_CAMERA_HOST, 5800); + PortForwarder.add(1182, Constants.VISION_CAMERA_HOST, 1182); // Set up data receivers & replay source switch (Constants.CURRENT_MODE) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9bf6fa..2318df8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -155,9 +155,6 @@ public class RobotContainer { initializeAutoBuilder(); autoChooserInit(); - // put the Chooser on the SmartDashboard - SmartDashboard.putData("Auto chooser", autoChooser); - if (turret != null && drive != null && hood != null && shooter != null) { SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter)); } @@ -424,7 +421,7 @@ public class RobotContainer { } } if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); + SmartDashboard.putString("Alliance", DriverStation.getAlliance().map(a -> a.name()).orElse("Unknown")); } } } diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 02abe4c..6703146 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -169,7 +169,7 @@ public class AutoShootCommand extends Command { ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); // Add a phase delay extrapolation component for latency delay - drivepose.exp( + drivepose = drivepose.exp( new Twist2d( robotRelVel.vxMetersPerSecond * phaseDelay, robotRelVel.vyMetersPerSecond * phaseDelay, diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 0928289..f47ca18 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -196,7 +196,7 @@ public class Superstructure extends Command { ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); // Add a phase delay extrapolation component for latency delay - drivepose.exp( + drivepose = drivepose.exp( new Twist2d( robotRelVel.vxMetersPerSecond * phaseDelay, robotRelVel.vyMetersPerSecond * phaseDelay, diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 3df7e6f..f18925a 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -83,4 +83,7 @@ public class Constants { // Enables 3D logs of mechanisms public static final boolean LOG_MECHANISMS = true; + + // Network setting for vision + public static final String VISION_CAMERA_HOST = "10.9.72.12"; } diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 6449efe..3b67382 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -25,6 +25,7 @@ import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.DPad; import lib.controllers.PS5Controller.PS5Axis; import lib.controllers.PS5Controller.PS5Button; +import org.littletonrobotics.junction.Logger; /** * Driver controls for the PS5 controller @@ -39,6 +40,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private Hood hood; private Intake intake; private Spindexer spindexer; + private double originalSpindexerCurrentLimit; public PS5ControllerDriverConfig( Drivetrain drive, @@ -156,13 +158,14 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { private void shootFocus(boolean turnOn) { if (turnOn) { - System.out.println("Shooting is now Focused"); + Logger.recordOutput("ShootFocus", "Focused"); + originalSpindexerCurrentLimit = spindexer.getCurrentLimit(); spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT * 0.25); } else { - System.out.println("Shooting back to normal (From focus)"); - spindexer.setNewCurrentLimit(SpindexerConstants.currentLimit); + Logger.recordOutput("ShootFocus", "Normal"); + spindexer.setNewCurrentLimit(originalSpindexerCurrentLimit); drive.applyNewModuleCurrents(DriveConstants.STEER_PEAK_CURRENT_LIMIT, DriveConstants.DRIVE_PEAK_CURRENT_LIMIT); } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index 56ef814..7dff223 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -269,7 +269,7 @@ public class Intake extends SubsystemBase implements IntakeIO{ */ public double rotationsToInches(double motorRotations) { // circumference of the rack pinion - double circ = 2 * Math.PI * 0.5; + double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; double pinionRotations = motorRotations / IntakeConstants.GEAR_RATIO; double inches = pinionRotations * circ; return inches; @@ -281,7 +281,7 @@ public class Intake extends SubsystemBase implements IntakeIO{ * @return motor rotations */ public double inchesToRotations(double inches){ - double circ = 2 * Math.PI * 0.5; + double circ = 2 * Math.PI * IntakeConstants.RADIUS_RACK_PINION; double pinionRotations = inches / circ; double motorRotations = pinionRotations * IntakeConstants.GEAR_RATIO; return motorRotations; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b34a4e5..46f3980 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -99,11 +99,11 @@ public class Shooter extends SubsystemBase implements ShooterIO { shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true)); if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER); + Logger.recordOutput("Shooter/realVelocity", Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble()); Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed); } - double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER; + double actualWheelVelocity = Math.PI * ShooterConstants.SHOOTER_LAUNCH_DIAMETER * shooterMotorLeft.getVelocity().getValueAsDouble(); if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity); diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index b4b2ae6..625efb8 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -21,6 +21,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { private boolean wasSpindexerSlow = false; private SpindexerState state = SpindexerState.STOPPED; private SpindexerIOInputsAutoLogged inputs = new SpindexerIOInputsAutoLogged(); + private double currentLimit = SpindexerConstants.currentLimit; public boolean noIndexing = false; @@ -127,6 +128,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { } public void setNewCurrentLimit(double newCurrentLimit) { + currentLimit = newCurrentLimit; CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs(); limitConfig.StatorCurrentLimit = newCurrentLimit; limitConfig.StatorCurrentLimitEnable = true; @@ -135,6 +137,10 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { motor.getConfigurator().apply(limitConfig); } + public double getCurrentLimit() { + return currentLimit; + } + @Override public void updateInputs() { inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble(); //SpindexerConstants.gearRatio; -- 2.39.5