From d7af5c7b0ef34642f771170ee5cad3528a4821cb Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Sat, 11 Apr 2026 21:11:23 -0500 Subject: [PATCH 01/36] tuned shooter --- src/main/java/frc/robot/RobotContainer.java | 24 +++++++------ .../subsystems/intake/IntakeConstants.java | 6 ++-- .../intake/extend/IntakeExtend.java | 4 +++ .../intake/extend/IntakeExtendIO.java | 3 ++ .../intake/extend/IntakeExtendIOSparkMax.java | 35 ++++++++++++++----- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../subsystems/shooter/ShooterConstants.java | 4 +-- 7 files changed, 54 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 151a6d629..00939a83a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -256,7 +256,8 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); autoChooser.addDefaultOption("Do Nothing", Commands.none()); - + autoChooser.addOption( + "RunIntakeOut", intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); autoChooser.addOption("Manual A-CC", autos.ACCManualAuto()); autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt()); autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManualAutoOverBump()); @@ -330,8 +331,12 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.aimToHub()); - driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); + // driverController.x().toggleOnTrue(orchestrator.aimToHub()); + // + // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); + driverController.x().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(20))); + driverController.y().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(60))); + driverController.a().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(110))); driverController .leftBumper() .and(operatorController.pov(180)) @@ -350,13 +355,12 @@ private void configureButtonBindings() { // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); - // driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController.rightTrigger(0.1).toggleOnTrue(indexer.run()); + driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController - .a() - .and(operatorController.pov(180)) - .onTrue(intakeExtend.resetExtendPosition()); + // driverController + // .a() + // .and(operatorController.pov(180)) + // .onTrue(intakeExtend.resetExtendPosition()); driverController .rightBumper() .and(() -> !operatorController.pov(180).getAsBoolean()) @@ -378,7 +382,7 @@ private void configureButtonBindings() { .toggleOnTrue(orchestrator.spinUpShooterHub()); operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4)); operatorController.y().whileTrue(indexer.reverse()); - // operatorController.x().whileTrue(intake.outtake()); + operatorController.x().whileTrue(intakeRollers.outtake()); } /** diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index ee3abab23..9400c56cc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -10,13 +10,13 @@ public class IntakeConstants { public static final double STALL_TIME = 0.1; public static final double STALL_VELOCITY = 0.1; - public static final double INTAKE_VOLTS = 12; - public static final double OUTTAKE_VOLTS = -10; + public static final double INTAKE_VOLTS = 8; + public static final double OUTTAKE_VOLTS = -12; public static final double EXTEND_VOLTS = 0.01; public static final double COLLAPSE_VOLTS = -0.01; public static final double INTAKE_MAX_VELOCITY = 200; - public static final int EXTEND_LIMIT_ID = 1; + public static final int EXTEND_LIMIT_ID = 0; public static final double PID_P = 0.023; // arbritrary values public static final double PID_I = 0.000004; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 2b0621a3b..4b1044e1d 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -70,6 +70,10 @@ public DoubleSupplier getAngle() { return () -> inputs.getExtendPos; } + public void setIdleMode(boolean idleMode) { + io.setIdleMode(idleMode); + } + public Command setPose(double pose) { return Commands.runOnce(() -> io.resetExtendEncoder(pose), this); } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java index 082317d7c..706131aab 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -35,6 +35,9 @@ default void goToPos(double pos) {} default void setPIDEnabled(boolean enabled) {} + default void setIdleMode(boolean cost) {} + ; + default boolean getPIDEnabled() { return false; } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index ed688a961..c21bc77d9 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -33,6 +33,16 @@ public class IntakeExtendIOSparkMax implements IntakeExtendIO { public IntakeExtendIOSparkMax() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); + + pidController = extendMotor.getClosedLoopController(); + extendMotorEncoder = extendMotor.getEncoder(); + + extendMotor.configure( + getSparkMaxConfig(), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); + } + + public SparkMaxConfig getSparkMaxConfig() { EncoderConfig extendEnc = new EncoderConfig(); extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION); @@ -46,13 +56,7 @@ public IntakeExtendIOSparkMax() { .pid(PID_P, PID_I, PID_D) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); extendConfig.encoder.apply(extendEnc); - - pidController = extendMotor.getClosedLoopController(); - extendMotorEncoder = extendMotor.getEncoder(); - - extendMotor.configure( - extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); + return extendConfig; } @Override @@ -61,7 +65,7 @@ public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendVelocity = extendMotorEncoder.getVelocity(); inputs.extendVolts = extendMotor.getAppliedOutput(); inputs.extendAmps = extendMotor.getOutputCurrent(); - inputs.isCollapsed = false; + inputs.isCollapsed = isCollapsed(); inputs.getExtendPos = extendMotorEncoder.getPosition(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID; inputs.hasSetpoint = usingPID; @@ -100,6 +104,21 @@ public void setPIDEnabled(boolean enabled) { this.usingPID = enabled; } + @Override + public void setIdleMode(boolean coast) { + if (coast) { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } else { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } + } + @Override public void resetExtendEncoder(double position) { extendMotorEncoder.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3d18d1c9a..0bbe8abce 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,7 +40,7 @@ public class Shooter extends SubsystemBase { // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit - private final SlewRateLimiter targetRamper = new SlewRateLimiter(800); + private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600); /** * Initializes the shooter with a Shooter IO diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 7db7ddb98..e6873cc40 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -13,9 +13,9 @@ public class ShooterConstants { public static final int CURRENT_LIMIT = 40; public static final IdleMode IDLE_MODE = IdleMode.kCoast; - public static final double KV = 0.0465; + public static final double KV = 0.074; public static final double KA = 0.031259; - public static final double KS = 0.25; + public static final double KS = 0.615; public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5; public static final double CLOSE_HUB_SHOOTER_RPM = 1085; From adb15a11ef142b95e54ec74916c58833cb87f39c Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 10:03:56 -0400 Subject: [PATCH 02/36] home intake logic --- .../subsystems/intake/IntakeConstants.java | 1 + .../intake/extend/IntakeExtend.java | 54 ++++++++++++++----- .../intake/extend/IntakeExtendIO.java | 1 + 3 files changed, 43 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 9400c56cc..fedf93f7f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -27,6 +27,7 @@ public class IntakeConstants { // shake around the funnel pos by this much public static final double SHAKE_POS_OFFSET = 1; + public static final double HOME_VOLTAGE = 6; public static final double COLLAPSE_POS = 4.0; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 4b1044e1d..77f4bb8c7 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -2,6 +2,7 @@ import static frc.robot.subsystems.intake.IntakeConstants.COLLAPSE_POS; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POS; +import static frc.robot.subsystems.intake.IntakeConstants.HOME_VOLTAGE; import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE; import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY; @@ -25,6 +26,7 @@ public class IntakeExtend extends SubsystemBase { private boolean stalledCollapse = false; private boolean isStowed = false; private boolean isExtended = false; + private boolean hasPose = false; private double targetExtendPosition = 0; @@ -54,7 +56,9 @@ public void periodic() { } if (inputs.isCollapsed) { io.resetExtendEncoder(0.0); + hasPose = true; } + inputs.hasPose = hasPose; } public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { @@ -128,26 +132,50 @@ public Command stopExtendingCommand() { return Commands.run(this::stopExtend, this); } - public Command extendToAngle(double angle) { + public Command homeExtend() { return Commands.run( () -> { - io.setPIDEnabled(true); - io.goToPos(angle); + io.setPIDEnabled(false); + io.setVoltageExtend(-HOME_VOLTAGE); }, this) - .until(() -> inputs.atSetpoint) + .until(() -> inputs.isCollapsed) .finallyDo(this::stopExtend) - .withName("extendToAngle"); + .withName("homeExtend"); + } + + public Command extendToAngle(double angle) { + Command extendCommand = + Commands.run( + () -> { + io.setPIDEnabled(true); + io.goToPos(angle); + }, + this) + .until(() -> inputs.atSetpoint) + .finallyDo(this::stopExtend) + .withName("extendToAngle"); + + if (!hasPose) { + return homeExtend().andThen(extendCommand); + } + return extendCommand; } public Command holdAngle(double angle) { - return Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - }, - this) - .finallyDo(this::stopExtend) - .withName("holdAngle"); + Command holdCommand = + Commands.run( + () -> { + io.setPIDEnabled(true); + io.goToPos(angle); + }, + this) + .finallyDo(this::stopExtend) + .withName("holdAngle"); + + if (!hasPose) { + return homeExtend().andThen(holdCommand); + } + return holdCommand; } } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java index 706131aab..6a8695012 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -21,6 +21,7 @@ class IntakeExtendIOInputs { public double stallExtendTimer = 0.0; public double stallCollapseTimer = 0.0; public boolean stowed = false; + public boolean hasPose = false; } default void updateInputs(IntakeExtendIOInputs inputs) {} From f0320fd5f8064a609ce6cfe040085bdcff0e4cae Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 10:20:33 -0400 Subject: [PATCH 03/36] update shooter gear ratio --- .../java/frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e6873cc40..4425746f7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -17,7 +17,7 @@ public class ShooterConstants { public static final double KA = 0.031259; public static final double KS = 0.615; - public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5; + public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; public static final double CLOSE_HUB_SHOOTER_RPM = 1085; public static final double MAX_VOLTAGE = 12.0; From 19fe7ed5ef446ae5b6ee7e1f2f6131eeca936348 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Sun, 12 Apr 2026 09:37:25 -0500 Subject: [PATCH 04/36] saturday changes --- src/main/java/frc/robot/Orchestrator.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++----------- .../frc/robot/subsystems/intake/Intake.java | 4 +--- .../frc/robot/subsystems/shooter/Shooter.java | 2 +- .../subsystems/shooter/ShooterConstants.java | 6 ++--- 5 files changed, 15 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 5c9ed0a49..2fb2be0c7 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -51,7 +51,7 @@ public enum ZoneId { ZONE_4 } - private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0); + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0); private final Drive drive; private final Shooter shooter; private final MagicCarpet magicCarpet; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 00939a83a..993886d4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,7 +104,7 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2)); + new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); } case ROBOT_2026_COMP -> { @@ -331,12 +331,8 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - // driverController.x().toggleOnTrue(orchestrator.aimToHub()); - // - // driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); - driverController.x().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(20))); - driverController.y().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(60))); - driverController.a().toggleOnTrue(shooter.setTargetVelocity(RadiansPerSecond.of(110))); + driverController.x().toggleOnTrue(orchestrator.aimToHub()); + driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() .and(operatorController.pov(180)) @@ -351,16 +347,16 @@ private void configureButtonBindings() { driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED) .and(() -> !operatorController.pov(180).getAsBoolean()) - .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); + .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS)); // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - // driverController - // .a() - // .and(operatorController.pov(180)) - // .onTrue(intakeExtend.resetExtendPosition()); + driverController + .a() + .and(operatorController.pov(180)) + .onTrue(intakeExtend.resetExtendPosition()); driverController .rightBumper() .and(() -> !operatorController.pov(180).getAsBoolean()) @@ -370,7 +366,7 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - // operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 468aaec25..02af35363 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -32,9 +32,7 @@ public Command holdAngleAndIntake(double angle) { public Command runIntakeMotor() { return rollers - .intake() - .onlyWhile(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE) - .onlyIf(() -> extend.getAngle().getAsDouble() < COLLAPSE_POS - SAFETY_TOLERANCE); + .intake(); } // Jack's Chugga Chugga mode diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0bbe8abce..96fb9fe67 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -40,7 +40,7 @@ public class Shooter extends SubsystemBase { // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit - private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600); + private final SlewRateLimiter targetRamper = new SlewRateLimiter(2400); /** * Initializes the shooter with a Shooter IO diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 4425746f7..7cad67ea2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,7 +10,7 @@ public class ShooterConstants { public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; public static final double VOLTAGE_COMPENSATION = 9.0; - public static final int CURRENT_LIMIT = 40; + public static final int CURRENT_LIMIT = 20; public static final IdleMode IDLE_MODE = IdleMode.kCoast; public static final double KV = 0.074; @@ -18,11 +18,11 @@ public class ShooterConstants { public static final double KS = 0.615; public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; - public static final double CLOSE_HUB_SHOOTER_RPM = 1085; + public static final double CLOSE_HUB_SHOOTER_RPM = 1337; public static final double MAX_VOLTAGE = 12.0; - public static final double TOLERANCE = 50; // measured in radians + public static final double TOLERANCE = 5; // measured in radians public static Transform2d kShooterOffsetFromRobotCenter = new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0)); From c44f31ae29ab6658f8cca97985ad65bd24866f62 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 11:44:54 -0400 Subject: [PATCH 05/36] fix build --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/intake/Intake.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 993886d4a..647244cd7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -104,7 +104,7 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2)); + new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); } case ROBOT_2026_COMP -> { diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 02af35363..750ea055b 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -31,8 +31,7 @@ public Command holdAngleAndIntake(double angle) { } public Command runIntakeMotor() { - return rollers - .intake(); + return rollers.intake(); } // Jack's Chugga Chugga mode From 06c5d83cf068b2681166d0510fb4cdf69ddf11cd Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 11:54:23 -0400 Subject: [PATCH 06/36] logging for intake --- .../frc/robot/subsystems/intake/extend/IntakeExtend.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 77f4bb8c7..32d919477 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -55,10 +55,14 @@ public void periodic() { RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; } if (inputs.isCollapsed) { + if (!hasPose) { + Logger.recordOutput("Intake/IntakeExtend/HomeCompleted", true); + } io.resetExtendEncoder(0.0); hasPose = true; } inputs.hasPose = hasPose; + Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose); } public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { @@ -139,6 +143,7 @@ public Command homeExtend() { io.setVoltageExtend(-HOME_VOLTAGE); }, this) + .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true)) .until(() -> inputs.isCollapsed) .finallyDo(this::stopExtend) .withName("homeExtend"); @@ -157,6 +162,7 @@ public Command extendToAngle(double angle) { .withName("extendToAngle"); if (!hasPose) { + Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true); return homeExtend().andThen(extendCommand); } return extendCommand; @@ -174,6 +180,7 @@ public Command holdAngle(double angle) { .withName("holdAngle"); if (!hasPose) { + Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true); return homeExtend().andThen(holdCommand); } return holdCommand; From 99cba051f841e0c576ea4a95dcf68a32c0d02f7e Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 12:03:09 -0400 Subject: [PATCH 07/36] conditional command for homing intake --- .../subsystems/intake/extend/IntakeExtend.java | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 32d919477..66c762559 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -9,6 +9,7 @@ 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.ConditionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotState; import frc.robot.RobotState.IntakePosition; @@ -150,8 +151,8 @@ public Command homeExtend() { } public Command extendToAngle(double angle) { - Command extendCommand = - Commands.run( + return + new ConditionalCommand(homeExtend(), Commands.none(), () -> hasPose).andThen(Commands.run( () -> { io.setPIDEnabled(true); io.goToPos(angle); @@ -159,13 +160,7 @@ public Command extendToAngle(double angle) { this) .until(() -> inputs.atSetpoint) .finallyDo(this::stopExtend) - .withName("extendToAngle"); - - if (!hasPose) { - Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true); - return homeExtend().andThen(extendCommand); - } - return extendCommand; + .withName("extendToAngle")); } public Command holdAngle(double angle) { From 2c1d7f9a0bfedd11e445d2fc721f9da93a94ca9e Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 12:08:36 -0400 Subject: [PATCH 08/36] fix logic --- .../intake/extend/IntakeExtend.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 66c762559..018e4a142 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -151,16 +151,17 @@ public Command homeExtend() { } public Command extendToAngle(double angle) { - return - new ConditionalCommand(homeExtend(), Commands.none(), () -> hasPose).andThen(Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - }, - this) - .until(() -> inputs.atSetpoint) - .finallyDo(this::stopExtend) - .withName("extendToAngle")); + return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose) + .andThen( + Commands.run( + () -> { + io.setPIDEnabled(true); + io.goToPos(angle); + }, + this) + .until(() -> inputs.atSetpoint) + .finallyDo(this::stopExtend) + .withName("extendToAngle")); } public Command holdAngle(double angle) { From 2ac467a92ef73b59db3f9af8481bb9a1524519b7 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 12:11:39 -0400 Subject: [PATCH 09/36] nuke hold angle --- src/main/java/frc/robot/RobotContainer.java | 2 -- .../frc/robot/subsystems/intake/Intake.java | 5 ----- .../subsystems/intake/extend/IntakeExtend.java | 18 ------------------ 3 files changed, 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 647244cd7..c396ff0f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,8 +183,6 @@ public RobotContainer() { new Orchestrator( drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController); Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter); - NamedCommands.registerCommand( - "startIntake", intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(10.0)); NamedCommands.registerCommand( "endIntake", Commands.parallel( diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 750ea055b..e1db70243 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -25,11 +25,6 @@ public Command extendToAngle(double angle) { return extend.extendToAngle(angle); } - public Command holdAngleAndIntake(double angle) { - return Commands.parallel(extend.holdAngle(angle), runIntakeMotor()) - .withName("holdAngleAndIntake"); - } - public Command runIntakeMotor() { return rollers.intake(); } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 018e4a142..0651b7ee7 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -163,22 +163,4 @@ public Command extendToAngle(double angle) { .finallyDo(this::stopExtend) .withName("extendToAngle")); } - - public Command holdAngle(double angle) { - Command holdCommand = - Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - }, - this) - .finallyDo(this::stopExtend) - .withName("holdAngle"); - - if (!hasPose) { - Logger.recordOutput("Intake/IntakeExtend/AutoHoming", true); - return homeExtend().andThen(holdCommand); - } - return holdCommand; - } } From 1811fb774a23e263d4997ed57a3ad4cf99a39b32 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 12:12:37 -0400 Subject: [PATCH 10/36] nuke named commands --- src/main/java/frc/robot/RobotContainer.java | 30 --------------------- 1 file changed, 30 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c396ff0f2..99a042203 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,36 +183,6 @@ public RobotContainer() { new Orchestrator( drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController); Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter); - NamedCommands.registerCommand( - "endIntake", - Commands.parallel( - intakeRollers.stopIntakeCommand().withTimeout(0.05), - indexer.stop().withTimeout(0.05)) - .withTimeout(0.05)); - NamedCommands.registerCommand( - "spinUp", - shooter.setTargetVelocityRepeatedly(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM))); - NamedCommands.registerCommand("feedShooter", orchestrator.feedUp()); - NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0)); - NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub()); - NamedCommands.registerCommand("driveToHubAuto", orchestrator.driveToHub().withTimeout(3.0)); - NamedCommands.registerCommand( - "shootAuto", - Commands.sequence( - Commands.parallel( - shooter - .setTargetVelocityRepeatedly( - Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)) - .withTimeout(0.8), - intakeRollers.stopIntakeCommand(), - indexer.stop()) - .withTimeout(0.8), - Commands.deadline( - Commands.waitSeconds(3.2), - shooter.setTargetVelocityRepeatedly( - Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)), - orchestrator.feedUp()), - Commands.parallel(indexer.stop().withTimeout(0.05)).withTimeout(0.05))); AutoBuilder.configure( drive::getPose, drive::setPose, From 3be0b0343e123c655156da825c42a2d7753b6c2d Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Sun, 12 Apr 2026 12:15:16 -0400 Subject: [PATCH 11/36] fix build --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99a042203..45fac5f3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; -import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; import static frc.robot.subsystems.vision.VisionConstants.*; import com.pathplanner.lib.auto.AutoBuilder; From b1e5c8714424e9eb7a42ba196a6ec7ce07cb99e3 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Sun, 12 Apr 2026 15:33:21 -0500 Subject: [PATCH 12/36] sunday changes --- src/main/java/frc/robot/Orchestrator.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 22 ++++----- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/indexer/IndexerConstants.java | 2 +- .../subsystems/indexer/IndexerIOSparkMax.java | 2 +- .../subsystems/intake/IntakeConstants.java | 2 +- .../intake/extend/IntakeExtend.java | 2 +- .../magicCarpet/MagicCarpetConstants.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 48 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 23 +++++---- .../subsystems/shooter/ShooterIOSparkMax.java | 22 +++++---- 11 files changed, 90 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 2fb2be0c7..acf8c4136 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -234,8 +234,7 @@ public Command driveToHub() { public Command feedUp() { return Commands.repeatingSequence( - Commands.parallel(magicCarpet.run(), indexer.run()) - .until(() -> !RobotState.getInstance().shooterAtSpeed)) + Commands.parallel(indexer.run()).until(() -> !RobotState.getInstance().shooterAtSpeed)) .onlyIf(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) .onlyIf(() -> RobotState.getInstance().shooterAtSpeed) .onlyWhile(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 45fac5f3b..5035fd99c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,6 @@ import static frc.robot.subsystems.vision.VisionConstants.*; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; @@ -28,7 +27,6 @@ import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; -import frc.robot.commands.auto.DriveToPose; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.drive.DriveWithDpad; import frc.robot.subsystems.drive.*; @@ -120,7 +118,7 @@ public RobotContainer() { new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); - leds = new Leds(); + // leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); @@ -202,12 +200,12 @@ public RobotContainer() { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); - NamedCommands.registerCommand( - "Drive Back Left", - new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); - NamedCommands.registerCommand( - "Drive Over Left", - new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Back Left", + // new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Over Left", + // new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); // // NamedCommands.registerCommand("startShooter",Commands.parallel(orchestrator.preloadBalls(),orchestrator.prepShooter())); @@ -276,7 +274,6 @@ public RobotContainer() { private void configureButtonBindings() { indexer.setDefaultCommand(indexer.stop()); shooter.setDefaultCommand(shooter.stop()); - // shooter.setDefaultCommand(shooter.stop()); drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -316,7 +313,7 @@ private void configureButtonBindings() { .and(() -> !operatorController.pov(180).getAsBoolean()) .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS)); - // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE + // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); @@ -333,7 +330,7 @@ private void configureButtonBindings() { .and(operatorController.pov(180)) .whileTrue(intakeExtend.runIntakeExtendVolts(4)) .onFalse(intakeExtend.stopExtendingCommand()); - operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) @@ -343,7 +340,6 @@ private void configureButtonBindings() { .rightTrigger(0.1) .and(operatorController.pov(0)) .toggleOnTrue(orchestrator.spinUpShooterHub()); - operatorController.leftTrigger(0.1).toggleOnTrue(shooter.setVoltage(4)); operatorController.y().whileTrue(indexer.reverse()); operatorController.x().whileTrue(intakeRollers.outtake()); } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 09d8613f1..3add398c2 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { - // logCameraPositions(); // uncomment to show camera positions in advantage scope +// logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index 2125a44b1..f32361c72 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -3,5 +3,5 @@ public class IndexerConstants { public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1; public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1; - public static final double FEEDUP_VOLT = 12; + public static final double FEEDUP_VOLT = 10; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java index 4536f6fdb..c5fb5513d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java @@ -24,7 +24,7 @@ public IndexerIOSparkMax() { .inverted(false) .idleMode(IdleMode.kBrake) .voltageCompensation(12) - .smartCurrentLimit(30); + .smartCurrentLimit(20); EncoderConfig feederUpEnc = new EncoderConfig(); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index fedf93f7f..fd09d8c8c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -27,7 +27,7 @@ public class IntakeConstants { // shake around the funnel pos by this much public static final double SHAKE_POS_OFFSET = 1; - public static final double HOME_VOLTAGE = 6; + public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java index 0651b7ee7..bbf147252 100644 --- a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -141,7 +141,7 @@ public Command homeExtend() { return Commands.run( () -> { io.setPIDEnabled(false); - io.setVoltageExtend(-HOME_VOLTAGE); + io.setVoltageExtend(HOME_VOLTAGE); }, this) .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true)) diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java index dff198558..abf58ff8b 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java @@ -4,8 +4,8 @@ public final class MagicCarpetConstants { public static final boolean MOTOR_INVERTED = false; - public static final int CURRENT_LIMIT = 30; - public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed + public static final int CURRENT_LIMIT = 15; + public static final double BELT_SPEED = 0.7; // TODO: bump this value up if needed private MagicCarpetConstants() {} ; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 96fb9fe67..3d6c099c8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,10 @@ import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR; +import static frc.robot.subsystems.shooter.ShooterConstants.KA; +import static frc.robot.subsystems.shooter.ShooterConstants.KS; +import static frc.robot.subsystems.shooter.ShooterConstants.KV; import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE; import edu.wpi.first.math.controller.PIDController; @@ -12,6 +16,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -32,15 +37,15 @@ public class Shooter extends SubsystemBase { private double rampedTarget = 0.0; // Feedforward: handles steady-state voltage (V = KS + KV * velocity) - private final SimpleMotorFeedforward feedforward = - new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV, KA); // PID: handles error correction on top of feedforward // P only — no I term (causes integral windup oscillation) private final PIDController pid = new PIDController(0.001, 0.1, 0.1, 0.02); // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit - private final SlewRateLimiter targetRamper = new SlewRateLimiter(2400); + private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600); + private double feedForwardScalar; /** * Initializes the shooter with a Shooter IO @@ -48,6 +53,11 @@ public class Shooter extends SubsystemBase { * @param io A ShooterIO implementing instance */ public Shooter(ShooterIO io) { + SmartDashboard.putNumber("Shooter/KS", KS * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); + SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); + feedForwardScalar = FF_SCALAR; this.io = io; sysId = new SysIdRoutine( @@ -61,6 +71,37 @@ public Shooter(ShooterIO io) { @Override public void periodic() { + + if (SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedForwardScalar + || SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKs() + || SmartDashboard.getNumber("Shooter/KA", feedforward.getKa()) != feedforward.getKa() + || SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKv()) { + feedForwardScalar = SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar); + feedforward.setKa(SmartDashboard.getNumber("Shooter/KA", feedforward.getKa())); + feedforward.setKv( + SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + feedforward.setKa( + SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + } io.updateInputs(inputs); RobotState.getInstance().shooterAtSpeed = isAtSetpoint() && targetSpeed.gt(RadiansPerSecond.of(0)); @@ -165,6 +206,7 @@ public Command setTargetVelocity(Supplier velocity) { controllerEnabled = true; }, this) + .finallyDo(io::stop) .withName("setTargetVelocity"); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 7cad67ea2..dfdd8b6d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,25 +4,32 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; - public static final double VOLTAGE_COMPENSATION = 9.0; - public static final int CURRENT_LIMIT = 20; + public static final double VOLTAGE_COMPENSATION = 12.0; + public static final int CURRENT_LIMIT = 55; public static final IdleMode IDLE_MODE = IdleMode.kCoast; - public static final double KV = 0.074; - public static final double KA = 0.031259; - public static final double KS = 0.615; + public static final double KV = 0.0662; + public static final double KA = 0.028853; + public static final double KS = 0.2723; - public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; - public static final double CLOSE_HUB_SHOOTER_RPM = 1337; + // for 3.7647 gear ratio + // public static final double KV = 0.0662; + // public static final double KA = 0.028853; + // public static final double KS = 0.2723; + public static final double FF_SCALAR = 1; + public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; + public static final double CLOSE_HUB_SHOOTER_RPM = 850; public static final double MAX_VOLTAGE = 12.0; - public static final double TOLERANCE = 5; // measured in radians + public static final double TOLERANCE = + Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians public static Transform2d kShooterOffsetFromRobotCenter = new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index b019e9229..17cb653a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -44,26 +44,26 @@ public ShooterIOSparkMax(boolean independentMode) { .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - bottomLeftMotorConfig.follow(shooterTopRightMotorCanId, false); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); bottomLeftMotorConfig.apply(enc); var topLeftMotorConfig = new SparkMaxConfig(); topLeftMotorConfig - .inverted(false) + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - topLeftMotorConfig.follow(shooterTopRightMotorCanId, true); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); topLeftMotorConfig.apply(enc); var bottomRightMotorConfig = new SparkMaxConfig(); bottomRightMotorConfig - .inverted(false) + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - bottomRightMotorConfig.follow(shooterTopRightMotorCanId, true); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); bottomRightMotorConfig.apply(enc); var topRightMotorConfig = new SparkMaxConfig(); @@ -71,7 +71,8 @@ public ShooterIOSparkMax(boolean independentMode) { .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); topRightMotorConfig.apply(enc); bottomLeftMotor.configure( @@ -125,6 +126,9 @@ public void updateInputs(ShooterIOInputs inputs) { @Override public void setVoltage(double volts) { topRightMotor.setVoltage(volts); + topLeftMotor.setVoltage(volts); + bottomRightMotor.setVoltage(volts); + bottomLeftMotor.setVoltage(volts); } @Override From 869708902fc237f90f2bc7725dcb7af04ad14843 Mon Sep 17 00:00:00 2001 From: Ronit Sharma Date: Sun, 12 Apr 2026 16:54:12 -0400 Subject: [PATCH 13/36] Adding controller rumble feature when intaking --- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5035fd99c..d6b8b2de5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,8 +17,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -314,8 +316,17 @@ private void configureButtonBindings() { .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS)); // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE - driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); + // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); + driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); + driverController.leftTrigger(0.2).toggleOnTrue( + Commands.parallel( + rumblePulse(0.5, 0.2), + intake.runIntakeMotor() + ).finallyDo(() -> + CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)) + ) +); driverController .a() @@ -357,4 +368,12 @@ public void robotPeriodic() { RobotState.getInstance().updateLEDState(); orchestrator.orchestratorPeriodic(); } + + private Command rumblePulse(double intensity, double seconds) { + return Commands.sequence( + Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), + Commands.waitSeconds(seconds), + Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0)) + ); +} } From b842701218bbca8b6f16a1633866b5e5d47af0f7 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 17:11:33 -0400 Subject: [PATCH 14/36] autos to go under tench + removes named cmmands --- .../deploy/pathplanner/paths/A-Cycle1.path | 107 ++++++-------- .../deploy/pathplanner/paths/A-Cycle2.path | 60 ++++---- .../deploy/pathplanner/paths/B-Cycle1.path | 133 ++++++++---------- .../deploy/pathplanner/paths/B-Cycle2.path | 96 ++++++------- 4 files changed, 175 insertions(+), 221 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 9d22515a8..680284a2a 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.220865926213596, - "y": 7.587085598717599 + "x": 5.365636024664715, + "y": 7.580018615477625 }, "isLocked": false, "linkedName": null @@ -20,56 +20,72 @@ "y": 6.832881932021468 }, "prevControl": { - "x": 7.335202354903501, - "y": 7.21155347245828 + "x": 7.352205583793333, + "y": 7.527391495153063 }, "nextControl": { - "x": 7.834164231584512, - "y": 6.465719538012889 + "x": 7.96399098844635, + "y": 5.729501726483708 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.7021109123434695, - "y": 4.074563506261182 + "x": 7.896815742397138, + "y": 4.0593381037567084 }, "prevControl": { - "x": 8.22271615942871, - "y": 4.362336125245133 + "x": 7.990620981847936, + "y": 4.332081885610835 }, "nextControl": { - "x": 7.200064179283715, - "y": 3.7970493935359855 + "x": 7.809668414958673, + "y": 3.8059525593755392 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.965992844364937, - "y": 5.664652951699464 + "x": 5.463005366726297, + "y": 7.369320214669052 }, "prevControl": { - "x": 6.215916040758477, - "y": 5.658456508813674 + "x": 7.167489045136595, + "y": 6.930742523290983 }, "nextControl": { - "x": 4.002719141323792, - "y": 5.713329159212882 + "x": 4.218132652871465, + "y": 7.689636218757288 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.256, - "y": 5.665 + "x": 3.402379248658318, + "y": 7.369320214669052 }, "prevControl": { - "x": 4.375552524919555, - "y": 5.729905885962339 + "x": 3.755531829449267, + "y": 7.579788697952738 + }, + "nextControl": { + "x": 2.276559141756934, + "y": 6.698364760176492 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.256350626118068, + "y": 4.553276315789475 + }, + "prevControl": { + "x": 2.412531719157826, + "y": 5.065468189438994 }, "nextControl": null, "isLocked": false, @@ -87,7 +103,7 @@ }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": -136.0 + "rotationDegrees": -106.0 }, { "waypointRelativePos": 3.0160427807486627, @@ -101,7 +117,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.1254237288135598, + "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { "maxVelocity": 1.2, @@ -111,49 +127,10 @@ "nominalVoltage": 12, "unlimited": false } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 3.118644067796608, - "maxWaypointRelativePos": 3.5254237288135593, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, - "constraints": { - "maxVelocity": 4, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, - "unlimited": false - } } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 0.5288135593220339, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "preloadBalls", - "waypointRelativePos": 2.535593220338976, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 6.0, @@ -164,7 +141,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -23.4357656437963 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 976a35738..4532e53ee 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -4,12 +4,12 @@ { "anchor": { "x": 3.256350626118068, - "y": 5.664652951699464 + "y": 4.553276315789475 }, "prevControl": null, "nextControl": { - "x": 3.6674363972882924, - "y": 5.6284350881968495 + "x": 3.073045025903002, + "y": 5.010824871550651 }, "isLocked": false, "linkedName": "A-Cycle1-End" @@ -20,12 +20,12 @@ "y": 7.254742397137747 }, "prevControl": { - "x": 2.1979715994142452, - "y": 6.347572517197855 + "x": 1.7727566660512677, + "y": 6.028530039315472 }, "nextControl": { - "x": 3.5736065913462336, - "y": 7.526672441691426 + "x": 3.449050576638838, + "y": 7.414011757246001 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 7.254742397137747 }, "prevControl": { - "x": 5.8071316550071765, - "y": 7.778212679002352 + "x": 5.771047211855309, + "y": 7.83231450077766 }, "nextControl": { - "x": 7.57757028119644, - "y": 6.342250134115599 + "x": 7.749507558733082, + "y": 6.1555316523359425 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 4.409032894736844 }, "prevControl": { - "x": 6.863907746462161, - "y": 5.216361806741775 + "x": 6.559973370874717, + "y": 4.568129553133817 }, "nextControl": { - "x": 5.937085933334687, - "y": 3.8213770349498226 + "x": 5.871274488423377, + "y": 4.096763618189595 }, "isLocked": false, "linkedName": null @@ -68,12 +68,12 @@ "y": 4.553276315789475 }, "prevControl": { - "x": 5.799882977639703, - "y": 4.304117085888362 + "x": 5.751215757710489, + "y": 4.286409263322725 }, "nextControl": { - "x": 5.6990813534677445, - "y": 5.5301053915928176 + "x": 5.805650962114751, + "y": 4.801893945471092 }, "isLocked": false, "linkedName": null @@ -84,12 +84,12 @@ "y": 7.447760964912282 }, "prevControl": { - "x": 6.523025544615784, - "y": 6.993212709981212 + "x": 6.769357739292765, + "y": 6.851722923583836 }, "nextControl": { - "x": 5.527831315774364, - "y": 7.601532227331069 + "x": 5.565220720039658, + "y": 7.576712704575512 }, "isLocked": false, "linkedName": null @@ -100,12 +100,12 @@ "y": 7.447760964912282 }, "prevControl": { - "x": 3.619467665950238, - "y": 7.613424526324748 + "x": 3.7599789090650377, + "y": 7.631925045537918 }, "nextControl": { - "x": 2.9084995834559355, - "y": 7.3654202472362 + "x": 2.9050441758844623, + "y": 7.3760898401707475 }, "isLocked": false, "linkedName": null @@ -116,8 +116,8 @@ "y": 6.284197368421054 }, "prevControl": { - "x": 1.9265215510296607, - "y": 7.001825662162521 + "x": 1.867329032576315, + "y": 6.919804106628433 }, "nextControl": null, "isLocked": false, @@ -187,7 +187,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -23.4357656437963 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 14317bd18..036b0753e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -4,12 +4,12 @@ { "anchor": { "x": 4.295, - "y": 0.538 + "y": 0.5380000000000011 }, "prevControl": null, "nextControl": { - "x": 5.220865926213596, - "y": 0.4819144012824006 + "x": 5.260984348859008, + "y": 0.548709162061195 }, "isLocked": false, "linkedName": null @@ -17,63 +17,79 @@ { "anchor": { "x": 7.588533094812165, - "y": 1.236118067978532 + "y": 1.2361180679785324 }, "prevControl": { - "x": 7.335202354903501, - "y": 0.8574465275417207 + "x": 7.333281138772382, + "y": 0.5372671705796467 }, "nextControl": { - "x": 7.834164231584512, - "y": 1.603280461987111 + "x": 8.01198853278679, + "y": 2.3954910162934935 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.7021109123434695, - "y": 3.994436493738819 + "x": 7.896815742397138, + "y": 4.009661896243292 }, "prevControl": { - "x": 8.22271615942871, - "y": 3.7066638747548675 + "x": 7.956228852277296, + "y": 3.7035208327957765 }, "nextControl": { - "x": 7.200064179283715, - "y": 4.271950606464015 + "x": 7.8491866370218535, + "y": 4.255082898443899 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.965992844364937, - "y": 2.4043470483005365 + "x": 5.463005366726297, + "y": 0.6996797853309484 }, "prevControl": { - "x": 6.215916040758477, - "y": 2.410543491186327 + "x": 7.033869255673328, + "y": 1.1479768709205327 }, "nextControl": { - "x": 4.002719141323792, - "y": 2.355670840787119 + "x": 4.179018869195575, + "y": 0.33325249087882547 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.175, - "y": 2.404 + "x": 3.402379248658318, + "y": 0.6996797853309484 }, "prevControl": { - "x": 4.294552524919553, - "y": 2.3390941140376613 + "x": 3.6989618406989995, + "y": 0.5442676850187853 + }, + "nextControl": { + "x": 2.3915233528638398, + "y": 1.2293778933601753 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4510554561717353, + "y": 3.2642933810375667 + }, + "prevControl": { + "x": 2.5378407958797427, + "y": 2.6730525818495314 }, "nextControl": null, "isLocked": false, - "linkedName": "B-Cycle1-End" + "linkedName": "B-Cycle-End" } ], "rotationTargets": [ @@ -83,11 +99,11 @@ }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": 86.315500893566 + "rotationDegrees": 86.315 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": 115.5063543100694 + "rotationDegrees": 94.0 }, { "waypointRelativePos": 3.0160427807486627, @@ -101,70 +117,31 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 1.1254237288135598, + "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { "maxVelocity": 1.2, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 3.118644067796608, - "maxWaypointRelativePos": 3.5254237288135593, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0, - "constraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 0.5288135593220339, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "preloadBalls", - "waypointRelativePos": 2.535593220338976, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 4, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 51.95295746817396 + "rotation": 37.71655912222805 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 5ddf30eec..ee6cc4b5c 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.1752236135957066, - "y": 2.4043470483005365 + "x": 3.4510554561717353, + "y": 3.2642933810375667 }, "prevControl": null, "nextControl": { - "x": 3.586309384765931, - "y": 2.4405649118031514 + "x": 3.2421167389384307, + "y": 2.967186063127149 }, "isLocked": false, - "linkedName": "B-Cycle1-End" + "linkedName": "B-Cycle-End" }, { "anchor": { "x": 3.256350626118068, - "y": 0.8142576028622535 + "y": 0.8142576028622539 }, "prevControl": { - "x": 2.1979715994142452, - "y": 1.721427482802146 + "x": 1.5889035900296817, + "y": 2.0483193166651414 }, "nextControl": { - "x": 3.5736065913462336, - "y": 0.5423275583085745 + "x": 3.457302404516393, + "y": 0.6655351003330456 }, "isLocked": false, "linkedName": null @@ -33,15 +33,15 @@ { "anchor": { "x": 6.4525328947368426, - "y": 0.8142576028622535 + "y": 0.8142576028622539 }, "prevControl": { - "x": 5.8071316550071765, - "y": 0.2907873209976489 + "x": 5.752347562091977, + "y": 0.24039328263318294 }, "nextControl": { - "x": 7.57757028119644, - "y": 1.7267498658844018 + "x": 7.910712691624192, + "y": 2.0093659818847356 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 3.6599671052631573 }, "prevControl": { - "x": 6.863907746462161, - "y": 2.852638193258226 + "x": 6.59100273960581, + "y": 3.4940005006714894 }, "nextControl": { - "x": 5.937085933334687, - "y": 4.247622965050178 + "x": 5.920916329346852, + "y": 3.9160880386065315 }, "isLocked": false, "linkedName": null @@ -68,12 +68,12 @@ "y": 3.515723684210526 }, "prevControl": { - "x": 5.799882977639703, - "y": 3.764882914111639 + "x": 5.837373947319441, + "y": 3.7589081110194775 }, "nextControl": { - "x": 5.6990813534677445, - "y": 2.538894608407183 + "x": 5.6945270228567875, + "y": 3.159737112788334 }, "isLocked": false, "linkedName": null @@ -81,15 +81,15 @@ { "anchor": { "x": 5.779396929824561, - "y": 0.6212390350877184 + "y": 0.6212390350877186 }, "prevControl": { - "x": 6.523025544615784, - "y": 1.0757872900187888 + "x": 6.736601761736383, + "y": 1.2079600435401154 }, "nextControl": { - "x": 5.527831315774364, - "y": 0.46746777266893197 + "x": 5.5662512875867165, + "y": 0.49059090002633776 }, "isLocked": false, "linkedName": null @@ -97,15 +97,15 @@ { "anchor": { "x": 3.1445504385964913, - "y": 0.6212390350877184 + "y": 0.6212390350877186 }, "prevControl": { - "x": 3.619467665950238, - "y": 0.4555754736752522 + "x": 3.5589239547141958, + "y": 0.44627660534504465 }, "nextControl": { - "x": 2.9084995834559355, - "y": 0.7035797527638007 + "x": 2.8278583396709256, + "y": 0.7549570864425861 }, "isLocked": false, "linkedName": null @@ -113,11 +113,11 @@ { "anchor": { "x": 2.548344298245614, - "y": 1.784802631578947 + "y": 1.7848026315789465 }, "prevControl": { - "x": 1.9265215510296607, - "y": 1.0671743378374803 + "x": 1.9675297251591926, + "y": 1.3628954494520977 }, "nextControl": null, "isLocked": false, @@ -142,7 +142,7 @@ "rotationDegrees": 119.99999999999999 }, { - "waypointRelativePos": 3.9622641509434033, + "waypointRelativePos": 3.962264150943403, "rotationDegrees": -100.0 }, { @@ -161,10 +161,10 @@ "maxWaypointRelativePos": 4.235188509874326, "constraints": { "maxVelocity": 1.5, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false } } @@ -172,22 +172,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 4, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 55.84030545433046 + "rotation": 38.90134455575158 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 51.95295746817396 + "rotation": 37.71655912222805 }, "useDefaultConstraints": true } \ No newline at end of file From a7fc35387d1a1939becd1db8a2e35c3cbbf08bc9 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 17:15:03 -0400 Subject: [PATCH 15/36] Fixed autos to go iunder trench + get rid of named commands bla bla bla --- .../pathplanner/autos/A-Side 2 Cycle.auto | 25 +++++++++++++++++++ .../pathplanner/autos/B-Side 2 Cycle.auto | 25 +++++++++++++++++++ 2 files changed, 50 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto create mode 100644 src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto diff --git a/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto new file mode 100644 index 000000000..ce72f9f46 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A-Cycle1" + } + }, + { + "type": "path", + "data": { + "pathName": "A-Cycle2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto new file mode 100644 index 000000000..6961e215d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "B-Cycle1" + } + }, + { + "type": "path", + "data": { + "pathName": "B-Cycle2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From a82c8e943056c38504c69f0ba72c853f1cdf4e2e Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 17:19:43 -0400 Subject: [PATCH 16/36] bla bla bla --- src/main/java/frc/robot/RobotContainer.java | 23 ++++++++----------- .../frc/robot/subsystems/drive/Drive.java | 2 +- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6b8b2de5..1bf04af26 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -317,16 +317,13 @@ private void configureButtonBindings() { // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); - + driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController.leftTrigger(0.2).toggleOnTrue( - Commands.parallel( - rumblePulse(0.5, 0.2), - intake.runIntakeMotor() - ).finallyDo(() -> - CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)) - ) -); + driverController + .leftTrigger(0.2) + .toggleOnTrue( + Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor()) + .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)))); driverController .a() @@ -371,9 +368,9 @@ public void robotPeriodic() { private Command rumblePulse(double intensity, double seconds) { return Commands.sequence( - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), + Commands.runOnce( + () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), Commands.waitSeconds(seconds), - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0)) - ); -} + Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3add398c2..09d8613f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { -// logCameraPositions(); // uncomment to show camera positions in advantage scope + // logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); From 64566f5d6ceaedc39a2fb85ad6d69ddb4210426a Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 18:02:02 -0400 Subject: [PATCH 17/36] fix autos and auto logic --- .../deploy/pathplanner/paths/A-Cycle1.path | 10 ++-- .../deploy/pathplanner/paths/B-Cycle1.path | 10 ++-- .../java/frc/robot/commands/auto/Autos.java | 51 ++++++++++++++----- 3 files changed, 48 insertions(+), 23 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 680284a2a..c105af3f0 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.256350626118068, - "y": 4.553276315789475 + "x": 3.256, + "y": 4.553 }, "prevControl": { - "x": 2.412531719157826, - "y": 5.065468189438994 + "x": 2.412181093039758, + "y": 5.065191873649519 }, "nextControl": null, "isLocked": false, @@ -141,7 +141,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -23.4357656437963 + "rotation": -23.436 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 036b0753e..23d68c24c 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.4510554561717353, - "y": 3.2642933810375667 + "x": 3.451, + "y": 3.264 }, "prevControl": { - "x": 2.5378407958797427, - "y": 2.6730525818495314 + "x": 2.5377853397080075, + "y": 2.6727592008119645 }, "nextControl": null, "isLocked": false, @@ -141,7 +141,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 37.71655912222805 + "rotation": 37.717000000000006 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index c145ea0e5..b727ac44a 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -98,14 +98,6 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); - private static final Supplier connectBside = - () -> - AllianceFlipUtil.apply( - new Pose2d(3.175, 2.404, new Rotation2d(Units.degreesToRadians(-118.29 + 180)))); - private static final Supplier connectAside = - () -> - AllianceFlipUtil.apply( - new Pose2d(3.256, 5.665, new Rotation2d(Units.degreesToRadians(115.83 + 180)))); /** * Helper function for a single cycle auto with a constant shooting speed @@ -130,6 +122,21 @@ private Command ppCycle(String path, Supplier startPose) { orchestrator.feedUp())); } + private Command ppCycleConnect(String path) { + return Commands.sequence( + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + orchestrator.spinUpShooterHub()) + .withTimeout(14.5), + orchestrator.aimToHub().withTimeout(2.5), + Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); + } + /** * Helper function for a single cycle auto that uses shooter regression to shoot into the hub * @@ -157,6 +164,25 @@ private Command ppCycleRegression(String path, Supplier startPose) { orchestrator.feedUp())); } + private Command ppCycleRegressionConnect(String path) { + return Commands.sequence( + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) + .withTimeout(14.5), + Commands.deadline( + orchestrator.aimToHub().withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), + Commands.parallel( + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + orchestrator.feedUp()) + .withTimeout(2.5), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + orchestrator.feedUp())); + } + /** * Helper function for a double cycle auto that uses shooter regression to shoot into the hub * twice (once after each cycle) @@ -167,11 +193,10 @@ private Command ppCycleRegression(String path, Supplier startPose) { * up in after the path * @return A command that follows the paths and shoots */ - private Command pp2CycleRegression( - String path1, String path2, Supplier startPose, Supplier startPose2) { + private Command pp2CycleRegression(String path1, String path2, Supplier startPose) { return ppCycleRegression(path1, startPose) .withTimeout(10.5) - .andThen(ppCycleRegression(path2, startPose2)); + .andThen(ppCycleRegressionConnect(path2)); } /** @@ -198,7 +223,7 @@ public Command ppBCycleRight() { * @return A command that executes the auto */ public Command ppB2CycleRightRegression() { - return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside, connectBside); + return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside); } /** @@ -207,7 +232,7 @@ public Command ppB2CycleRightRegression() { * @return A comand that executes the auto */ public Command ppA2CycleRightRegression() { - return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside, connectAside); + return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside); } /** From 47818c3f4e4f35afc0bf2589cccd39390c5355e8 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Sun, 12 Apr 2026 18:21:32 -0500 Subject: [PATCH 18/36] v2 working with regression and auto aim --- src/main/java/frc/robot/Orchestrator.java | 15 ++++++++++-- src/main/java/frc/robot/RobotContainer.java | 23 ++++++++----------- .../frc/robot/commands/auto/DriveToPose.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/indexer/IndexerConstants.java | 2 +- .../subsystems/indexer/IndexerIOSparkMax.java | 2 +- .../magicCarpet/MagicCarpetConstants.java | 4 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 8 +++---- .../subsystems/shooter/ShooterConstants.java | 2 +- 9 files changed, 34 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index acf8c4136..93d7bb2c2 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -10,6 +10,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -29,6 +30,7 @@ import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.ShooterLeadCompensator; import frc.robot.util.Zone; import frc.robot.util.Zone.Tuple2d; @@ -186,7 +188,11 @@ public Supplier getHubDistance() { return () -> Meters.of( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) - .getDistance(drive.getPose().getTranslation())); + .getDistance( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation())); } private Rotation2d filteredHubAngle(Rotation2d raw) { @@ -291,7 +297,12 @@ public Command aimToHub() { drive.getPose().getX(), drive.getPose().getY(), AllianceFlipUtil.apply(Hub.blueCenter) - .minus(drive.getPose().getTranslation()) + .plus(new Translation2d(-0.6, 0)) + .minus( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation()) .getAngle())); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6b8b2de5..1bf04af26 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -317,16 +317,13 @@ private void configureButtonBindings() { // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); - + driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController.leftTrigger(0.2).toggleOnTrue( - Commands.parallel( - rumblePulse(0.5, 0.2), - intake.runIntakeMotor() - ).finallyDo(() -> - CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)) - ) -); + driverController + .leftTrigger(0.2) + .toggleOnTrue( + Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor()) + .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)))); driverController .a() @@ -371,9 +368,9 @@ public void robotPeriodic() { private Command rumblePulse(double intensity, double seconds) { return Commands.sequence( - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), + Commands.runOnce( + () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), Commands.waitSeconds(seconds), - Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0)) - ); -} + Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))); + } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 9f51e5912..4b9eafb20 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -64,7 +64,7 @@ public class DriveToPose extends Command { thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); thetaMaxAcceleration.initDefault(Units.degreesToRadians(720.0)); driveTolerance.initDefault(0.01); - thetaTolerance.initDefault(Units.degreesToRadians(1.0)); + thetaTolerance.initDefault(Units.degreesToRadians(0.2)); case ROBOT_2025_COMP: driveKp.initDefault(2.5); driveKd.initDefault(0.0); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3add398c2..09d8613f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { -// logCameraPositions(); // uncomment to show camera positions in advantage scope + // logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index f32361c72..2125a44b1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -3,5 +3,5 @@ public class IndexerConstants { public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1; public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1; - public static final double FEEDUP_VOLT = 10; + public static final double FEEDUP_VOLT = 12; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java index c5fb5513d..4536f6fdb 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java @@ -24,7 +24,7 @@ public IndexerIOSparkMax() { .inverted(false) .idleMode(IdleMode.kBrake) .voltageCompensation(12) - .smartCurrentLimit(20); + .smartCurrentLimit(30); EncoderConfig feederUpEnc = new EncoderConfig(); diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java index abf58ff8b..8d715dfe1 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java @@ -4,8 +4,8 @@ public final class MagicCarpetConstants { public static final boolean MOTOR_INVERTED = false; - public static final int CURRENT_LIMIT = 15; - public static final double BELT_SPEED = 0.7; // TODO: bump this value up if needed + public static final int CURRENT_LIMIT = 20; + public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed private MagicCarpetConstants() {} ; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3d6c099c8..569fd178f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -261,11 +261,11 @@ public Time getAirTimeSeconds(Distance distance) { * @return Setpoint in radians per second */ public Supplier calculateSetpoint(Supplier distance) { - // calculate rad/s depending on distance + // calculate rpm depending on distance return () -> { - AngularVelocity velocity = RadiansPerSecond.of(183.35 * distance.get().in(Meters) + 750.93); - if (velocity.gt(RadiansPerSecond.of(5000))) { - return RadiansPerSecond.of(5000); + AngularVelocity velocity = RPM.of(102.4367 * distance.get().in(Meters) + 799.60799); + if (velocity.gt(RPM.of(1550))) { + return RPM.of(1550); } else return velocity; }; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index dfdd8b6d1..e2e0cd5f3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -32,5 +32,5 @@ public class ShooterConstants { Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians public static Transform2d kShooterOffsetFromRobotCenter = - new Transform2d(new Translation2d(-0.1839, 0.0), new Rotation2d(0.0)); + new Transform2d(new Translation2d(-0.1585, 0.0), new Rotation2d(0.0)); } From 10f0dac586a49bb1cbe50885a98d2a4e10821009 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 19:28:43 -0400 Subject: [PATCH 19/36] A-Cycle fixes --- .../deploy/pathplanner/paths/A-Cycle1.path | 50 ++++++------ .../deploy/pathplanner/paths/A-Cycle2.path | 78 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle1.path | 10 +-- .../deploy/pathplanner/paths/B-Cycle2.path | 14 ++-- src/main/deploy/pathplanner/settings.json | 14 ++-- 5 files changed, 83 insertions(+), 83 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index c105af3f0..11b272093 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.295, - "y": 7.531 + "x": 4.327227191413238, + "y": 7.369320214669052 }, "prevControl": null, "nextControl": { - "x": 5.365636024664715, - "y": 7.580018615477625 + "x": 5.340843841537115, + "y": 7.412915747707522 }, "isLocked": false, "linkedName": null @@ -20,28 +20,28 @@ "y": 6.832881932021468 }, "prevControl": { - "x": 7.352205583793333, - "y": 7.527391495153063 + "x": 7.370803269541251, + "y": 7.412129900536413 }, "nextControl": { - "x": 7.96399098844635, - "y": 5.729501726483708 + "x": 7.961914258614824, + "y": 5.839539470347579 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.896815742397138, - "y": 4.0593381037567084 + "x": 7.783237924865832, + "y": 4.984186046511627 }, "prevControl": { - "x": 7.990620981847936, - "y": 4.332081885610835 + "x": 7.869597464645584, + "y": 5.341020494676783 }, "nextControl": { - "x": 7.809668414958673, - "y": 3.8059525593755392 + "x": 7.721527918569696, + "y": 4.729202565831794 }, "isLocked": false, "linkedName": null @@ -52,12 +52,12 @@ "y": 7.369320214669052 }, "prevControl": { - "x": 7.167489045136595, - "y": 6.930742523290983 + "x": 7.21326055178334, + "y": 7.274241428662003 }, "nextControl": { - "x": 4.218132652871465, - "y": 7.689636218757288 + "x": 4.101641571321129, + "y": 7.443273326241669 }, "isLocked": false, "linkedName": null @@ -68,12 +68,12 @@ "y": 7.369320214669052 }, "prevControl": { - "x": 3.755531829449267, - "y": 7.579788697952738 + "x": 3.8027459218125412, + "y": 7.537383389137978 }, "nextControl": { - "x": 2.276559141756934, - "y": 6.698364760176492 + "x": 2.207362527433732, + "y": 6.867684296448937 }, "isLocked": false, "linkedName": null @@ -84,8 +84,8 @@ "y": 4.553 }, "prevControl": { - "x": 2.412181093039758, - "y": 5.065191873649519 + "x": 2.4792984729357204, + "y": 5.052513451565847 }, "nextControl": null, "isLocked": false, @@ -120,7 +120,7 @@ "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { - "maxVelocity": 1.2, + "maxVelocity": 1.7, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 4532e53ee..76099a153 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 3.073045025903002, - "y": 5.010824871550651 + "x": 2.9906316326262106, + "y": 5.102714219733314 }, "isLocked": false, "linkedName": "A-Cycle1-End" @@ -20,76 +20,76 @@ "y": 7.254742397137747 }, "prevControl": { - "x": 1.7727566660512677, - "y": 6.028530039315472 + "x": 1.3305248035949697, + "y": 6.4381945652407 }, "nextControl": { - "x": 3.449050576638838, - "y": 7.414011757246001 + "x": 3.5229482940373034, + "y": 7.367779487606841 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.4525328947368426, - "y": 7.254742397137747 + "x": 6.6312343470483, + "y": 7.14216457960644 }, "prevControl": { - "x": 5.771047211855309, - "y": 7.83231450077766 + "x": 5.0062651135040195, + "y": 7.6666279904228025 }, "nextControl": { - "x": 7.749507558733082, - "y": 6.1555316523359425 + "x": 8.088080921525812, + "y": 6.671963231966102 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.327521929824561, - "y": 4.409032894736844 + "x": 6.6312343470483, + "y": 4.091788908765653 }, "prevControl": { - "x": 6.559973370874717, - "y": 4.568129553133817 + "x": 6.88316360390668, + "y": 4.291079662851518 }, "nextControl": { - "x": 5.871274488423377, - "y": 4.096763618189595 + "x": 6.118695040000839, + "y": 3.686340386137116 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 4.553276315789475 + "x": 5.933542039355992, + "y": 4.28649373881932 }, "prevControl": { - "x": 5.751215757710489, - "y": 4.286409263322725 + "x": 6.073073117998111, + "y": 4.07905440288477 }, "nextControl": { - "x": 5.805650962114751, - "y": 4.801893945471092 + "x": 5.716749683104094, + "y": 4.608796575249948 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 7.447760964912282 + "x": 5.738837209302326, + "y": 7.254742397137747 }, "prevControl": { - "x": 6.769357739292765, - "y": 6.851722923583836 + "x": 6.853018104342613, + "y": 6.708689690967459 }, "nextControl": { - "x": 5.565220720039658, - "y": 7.576712704575512 + "x": 5.193957247675758, + "y": 7.521784471641628 }, "isLocked": false, "linkedName": null @@ -100,24 +100,24 @@ "y": 7.447760964912282 }, "prevControl": { - "x": 3.7599789090650377, - "y": 7.631925045537918 + "x": 3.970192282405868, + "y": 7.43239065104425 }, "nextControl": { - "x": 2.9050441758844623, - "y": 7.3760898401707475 + "x": 2.728196522051797, + "y": 7.455511892781865 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.548344298245614, - "y": 6.284197368421054 + "x": 2.8831663685152056, + "y": 5.909033989266548 }, "prevControl": { - "x": 1.867329032576315, - "y": 6.919804106628433 + "x": 1.8461433351002032, + "y": 6.781400700455524 }, "nextControl": null, "isLocked": false, @@ -160,7 +160,7 @@ "minWaypointRelativePos": 2.526032315978456, "maxWaypointRelativePos": 4.235188509874326, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.7, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 23d68c24c..74a18e9d7 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -132,11 +132,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index ee6cc4b5c..130a709c9 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -37,7 +37,7 @@ }, "prevControl": { "x": 5.752347562091977, - "y": 0.24039328263318294 + "y": 0.24039328263318283 }, "nextControl": { "x": 7.910712691624192, @@ -89,7 +89,7 @@ }, "nextControl": { "x": 5.5662512875867165, - "y": 0.49059090002633776 + "y": 0.49059090002633765 }, "isLocked": false, "linkedName": null @@ -172,11 +172,11 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4, - "maxAcceleration": 6, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12, + "maxVelocity": 4.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index d5a531bdd..65f9a1368 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.702, - "robotLength": 0.702, + "robotWidth": 0.9017, + "robotLength": 0.8128, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -19,14 +19,14 @@ "driveCurrentLimit": 46.0, "wheelCOF": 1.2, "flModuleX": 0.273, - "flModuleY": 0.244, + "flModuleY": 0.299, "frModuleX": 0.273, - "frModuleY": -0.244, + "frModuleY": -0.299, "blModuleX": -0.273, - "blModuleY": 0.244, + "blModuleY": 0.299, "brModuleX": -0.273, - "brModuleY": -0.244, + "brModuleY": -0.299, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} +} \ No newline at end of file From 858cfea13d43e74342481f15ef89a9d9a8eb718d Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Sun, 12 Apr 2026 18:29:05 -0500 Subject: [PATCH 20/36] rumble when intaking --- src/main/java/frc/robot/RobotContainer.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1bf04af26..c37b7dee3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,7 +20,6 @@ import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -285,7 +284,11 @@ private void configureButtonBindings() { () -> -driverController.getRightX())); // Lock to 0 degrees when A button is held - + Trigger intaking = new Trigger(() -> RobotState.getInstance().intaking); + intaking.onTrue( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.2))); + intaking.onFalse( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.0))); driverController .start() .onTrue( @@ -319,11 +322,7 @@ private void configureButtonBindings() { // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController - .leftTrigger(0.2) - .toggleOnTrue( - Commands.parallel(rumblePulse(0.5, 0.2), intake.runIntakeMotor()) - .finallyDo(() -> CommandScheduler.getInstance().schedule(rumblePulse(0.3, 0.15)))); + driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); driverController .a() From 295ac022c0066cf16bbbed6359a2db031109b316 Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 19:30:30 -0400 Subject: [PATCH 21/36] Update settings.json --- src/main/deploy/pathplanner/settings.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 65f9a1368..7c4dbdbec 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -29,4 +29,4 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} \ No newline at end of file +} From 7abe7057ea396c74099e30b78533cb5c6554260d Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 19:45:02 -0400 Subject: [PATCH 22/36] A-Cycle fixes --- .../deploy/pathplanner/paths/A-Cycle1.path | 48 ++++++------ .../deploy/pathplanner/paths/A-Cycle2.path | 74 +++++++++---------- 2 files changed, 61 insertions(+), 61 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 11b272093..1c2c9ac6d 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 7.588533094812165, - "y": 6.832881932021468 + "x": 7.783237924865832, + "y": 7.028586762075134 }, "prevControl": { - "x": 7.370803269541251, - "y": 7.412129900536413 + "x": 7.767012522361359, + "y": 7.531574239713775 }, "nextControl": { - "x": 7.961914258614824, - "y": 5.839539470347579 + "x": 7.817452345049281, + "y": 5.967939736388225 }, "isLocked": false, "linkedName": null @@ -36,43 +36,43 @@ "y": 4.984186046511627 }, "prevControl": { - "x": 7.869597464645584, - "y": 5.341020494676783 + "x": 7.831914132379248, + "y": 5.324919499105545 }, "nextControl": { - "x": 7.721527918569696, - "y": 4.729202565831794 + "x": 7.746136792727668, + "y": 4.724478121544485 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.463005366726297, + "x": 6.7285867620751345, "y": 7.369320214669052 }, "prevControl": { - "x": 7.21326055178334, - "y": 7.274241428662003 + "x": 8.286225402504472, + "y": 7.35309481216458 }, "nextControl": { - "x": 4.101641571321129, - "y": 7.443273326241669 + "x": 5.365289737415033, + "y": 7.383521225342595 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.402379248658318, + "x": 2.8669409660107332, "y": 7.369320214669052 }, "prevControl": { - "x": 3.8027459218125412, + "x": 3.2673076391649563, "y": 7.537383389137978 }, "nextControl": { - "x": 2.207362527433732, + "x": 1.671924244786147, "y": 6.867684296448937 }, "isLocked": false, @@ -80,12 +80,12 @@ }, { "anchor": { - "x": 3.256, - "y": 4.553 + "x": 2.753363148479427, + "y": 4.805706618962433 }, "prevControl": { - "x": 2.4792984729357204, - "y": 5.052513451565847 + "x": 1.9766616214151478, + "y": 5.30522007052828 }, "nextControl": null, "isLocked": false, @@ -99,11 +99,11 @@ }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": -93.685 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": -106.0 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 3.0160427807486627, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 76099a153..6a19f9a9b 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,108 +3,108 @@ "waypoints": [ { "anchor": { - "x": 3.256350626118068, - "y": 4.553276315789475 + "x": 2.753363148479427, + "y": 4.805706618962433 }, "prevControl": null, "nextControl": { - "x": 2.9906316326262106, - "y": 5.102714219733314 + "x": 2.48764415498757, + "y": 5.355144522906272 }, "isLocked": false, "linkedName": "A-Cycle1-End" }, { "anchor": { - "x": 3.256350626118068, - "y": 7.254742397137747 + "x": 2.477531305903399, + "y": 7.447760964912282 }, "prevControl": { - "x": 1.3305248035949697, - "y": 6.4381945652407 + "x": 2.039445438282648, + "y": 7.351408549885447 }, "nextControl": { - "x": 3.5229482940373034, - "y": 7.367779487606841 + "x": 2.760343374071401, + "y": 7.509962528077773 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.6312343470483, - "y": 7.14216457960644 + "x": 6.809713774597495, + "y": 7.447760964912282 }, "prevControl": { - "x": 5.0062651135040195, - "y": 7.6666279904228025 + "x": 5.320357204173185, + "y": 7.530332248967907 }, "nextControl": { - "x": 8.088080921525812, - "y": 6.671963231966102 + "x": 7.669660107334526, + "y": 7.400084757398864 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.6312343470483, - "y": 4.091788908765653 + "x": 6.809713774597495, + "y": 3.9619856887298743 }, "prevControl": { - "x": 6.88316360390668, - "y": 4.291079662851518 + "x": 7.0616430314558745, + "y": 4.16127644281574 }, "nextControl": { - "x": 6.118695040000839, - "y": 3.686340386137116 + "x": 6.297174467550033, + "y": 3.556537166101337 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.933542039355992, - "y": 4.28649373881932 + "x": 6.06334525939177, + "y": 4.513649373881932 }, "prevControl": { - "x": 6.073073117998111, - "y": 4.07905440288477 + "x": 6.202876338033889, + "y": 4.306210037947382 }, "nextControl": { - "x": 5.716749683104094, - "y": 4.608796575249948 + "x": 5.846552903139872, + "y": 4.83595221031256 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.738837209302326, - "y": 7.254742397137747 + "x": 5.771288014311271, + "y": 7.447760964912282 }, "prevControl": { - "x": 6.853018104342613, - "y": 6.708689690967459 + "x": 6.858389982110913, + "y": 7.448760964912282 }, "nextControl": { - "x": 5.193957247675758, - "y": 7.521784471641628 + "x": 5.164488763675691, + "y": 7.447202784292056 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.1445504385964913, + "x": 3.142772808586762, "y": 7.447760964912282 }, "prevControl": { - "x": 3.970192282405868, + "x": 3.9684146523961386, "y": 7.43239065104425 }, "nextControl": { - "x": 2.728196522051797, + "x": 2.7264188920420676, "y": 7.455511892781865 }, "isLocked": false, From 15e272d1934d333c469d02fb91bfc7cd25fba04c Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Sun, 12 Apr 2026 20:06:04 -0400 Subject: [PATCH 23/36] A-Cycle fixes --- .../deploy/pathplanner/paths/A-Cycle1.path | 22 +++++++++---------- .../deploy/pathplanner/paths/A-Cycle2.path | 6 ++--- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 1c2c9ac6d..3291c8ee5 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 4.327227191413238, + "x": 4.424579606440071, "y": 7.369320214669052 }, "prevControl": null, "nextControl": { - "x": 5.340843841537115, + "x": 5.438196256563948, "y": 7.412915747707522 }, "isLocked": false, @@ -33,15 +33,15 @@ { "anchor": { "x": 7.783237924865832, - "y": 4.984186046511627 + "y": 4.254042933810375 }, "prevControl": { "x": 7.831914132379248, - "y": 5.324919499105545 + "y": 4.594776386404293 }, "nextControl": { "x": 7.746136792727668, - "y": 4.724478121544485 + "y": 3.9943350088432332 }, "isLocked": false, "linkedName": null @@ -49,30 +49,30 @@ { "anchor": { "x": 6.7285867620751345, - "y": 7.369320214669052 + "y": 7.4991234347048294 }, "prevControl": { "x": 8.286225402504472, - "y": 7.35309481216458 + "y": 7.482898032200357 }, "nextControl": { "x": 5.365289737415033, - "y": 7.383521225342595 + "y": 7.513324445378372 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8669409660107332, + "x": 2.753363148479427, "y": 7.369320214669052 }, "prevControl": { - "x": 3.2673076391649563, + "x": 3.1537298216336502, "y": 7.537383389137978 }, "nextControl": { - "x": 1.671924244786147, + "x": 1.558346427254841, "y": 6.867684296448937 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 6a19f9a9b..337e5cb27 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -157,10 +157,10 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.526032315978456, - "maxWaypointRelativePos": 4.235188509874326, + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, "constraints": { - "maxVelocity": 1.7, + "maxVelocity": 1.5, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, From 98d5085debe03f9f40f76ba91973e7abfa924884 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Mon, 13 Apr 2026 13:22:21 +0000 Subject: [PATCH 24/36] retract while shooting --- src/main/java/frc/robot/Orchestrator.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 93d7bb2c2..9a5773b48 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -27,6 +27,7 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.shooter.Shooter; @@ -267,6 +268,22 @@ public Command spinUpShooter(double velocityRPM) { return shooter.setTargetVelocity(Rotations.per(Minute).of(velocityRPM)); } + public Command shootWhileRetractingIntake(Command shooterCommand) { + return Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + shooterCommand, + feedUp()) + .withName("shootWhileRetractingIntake"); + } + + public Command shootWhileRetractingIntakeHub() { + return shootWhileRetractingIntake(spinUpShooterHub()); + } + + public Command shootWhileRetractingIntakeDistance(Supplier targetDistance) { + return shootWhileRetractingIntake(spinUpShooterDistance(targetDistance)); + } + public Command driveShootAtAngle() { return Commands.parallel( Commands.run( From ad17575d1619c22643e5b9d3d4a88b9f92987fc8 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Mon, 13 Apr 2026 16:24:49 -0500 Subject: [PATCH 25/36] new leds and autos --- src/main/deploy/pathplanner/paths/A-Cycle1.path | 15 ++++++++++++++- src/main/deploy/pathplanner/paths/A-Cycle2.path | 2 +- src/main/deploy/pathplanner/paths/B-Cycle1.path | 2 +- src/main/deploy/pathplanner/paths/B-Cycle2.path | 4 ++-- src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc/robot/Orchestrator.java | 4 ++++ src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/auto/Autos.java | 3 ++- .../frc/robot/subsystems/leds/LedConstants.java | 6 +++++- 9 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 3291c8ee5..bc79df9f0 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -127,12 +127,25 @@ "nominalVoltage": 12, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.024307900067523, + "maxWaypointRelativePos": 4.956110735989194, + "constraints": { + "maxVelocity": 1.7, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.5, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 337e5cb27..75bf9b57c 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -172,7 +172,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.5, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index 74a18e9d7..db06b8000 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -132,7 +132,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.5, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 130a709c9..c2d81162a 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -89,7 +89,7 @@ }, "nextControl": { "x": 5.5662512875867165, - "y": 0.49059090002633765 + "y": 0.4905909000263373 }, "isLocked": false, "linkedName": null @@ -172,7 +172,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.5, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 7c4dbdbec..9f4a5d875 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,7 +4,7 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 4.0, + "defaultMaxVel": 2.5, "defaultMaxAccel": 6.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, @@ -29,4 +29,4 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 93d7bb2c2..80da04768 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -305,4 +305,8 @@ public Command aimToHub() { .getTranslation()) .getAngle())); } + + public Command stopShootingAuto() { + return Commands.parallel(indexer.stop(), shooter.stop()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c37b7dee3..c37c543a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,7 +119,7 @@ public RobotContainer() { new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2)); - // leds = new Leds(); + leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); @@ -186,7 +186,7 @@ public RobotContainer() { drive::setPose, drive::getChassisSpeeds, drive::runVelocity, - new PPHolonomicDriveController(new PIDConstants(12, 0, 0), new PIDConstants(9, 0, 0)), + new PPHolonomicDriveController(new PIDConstants(9.0, 0, 0), new PIDConstants(6, 0, 0)), ppConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, drive); diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index b727ac44a..0ee125de8 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -195,7 +195,8 @@ private Command ppCycleRegressionConnect(String path) { */ private Command pp2CycleRegression(String path1, String path2, Supplier startPose) { return ppCycleRegression(path1, startPose) - .withTimeout(10.5) + .withTimeout(14.5) + .andThen(orchestrator.stopShootingAuto().withTimeout(0.1)) .andThen(ppCycleRegressionConnect(path2)); } diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java index 66590c588..0f08a0e3e 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java @@ -7,7 +7,7 @@ public class LedConstants { public static final int LED_COUNT; /* num LEDs - change the first two numbers based on actual led strip */ - public static final int FULL_LENGTH = 42; // 2026 robot LED count + public static final int FULL_LENGTH = 11; // 2026 robot LED count public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH); public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2; @@ -46,6 +46,10 @@ public class LedConstants { LED_CHANNEL = 0; LED_COUNT = 14; } + case ROBOT_2026_COMP -> { + LED_CHANNEL = 0; + LED_COUNT = FULL_LENGTH; + } default -> { LED_CHANNEL = 0; LED_COUNT = 0; From 58d32945dac4be37728204336c34cc2e450a087a Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Mon, 13 Apr 2026 19:07:28 -0400 Subject: [PATCH 26/36] A-Cycle fixes --- src/main/deploy/pathplanner/settings.json | 12 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/auto/Autos.java | 72 ++++++++++++++++++- 3 files changed, 76 insertions(+), 10 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9f4a5d875..04d5ebb58 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,8 +9,8 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 51.7095, - "robotMOI": 0.7, + "robotMass": 50.8, + "robotMOI": 0.1814, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 8.16, @@ -19,13 +19,13 @@ "driveCurrentLimit": 46.0, "wheelCOF": 1.2, "flModuleX": 0.273, - "flModuleY": 0.299, + "flModuleY": 0.244, "frModuleX": 0.273, - "frModuleY": -0.299, + "frModuleY": -0.244, "blModuleX": -0.273, - "blModuleY": 0.299, + "blModuleY": 0.244, "brModuleX": -0.273, - "brModuleY": -0.299, + "brModuleY": -0.244, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c37c543a6..d3dc79dc5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -260,7 +260,7 @@ public RobotContainer() { // pp - autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); + autoChooser.addOption("A-side", autos.ppACycleAlternate()); autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 0ee125de8..b40133d2c 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -98,6 +98,45 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); + private static final Supplier FrontBumpASide = + () -> + AllianceFlipUtil.apply( + new Pose2d(5.775780200958252, 5.496349811553955, new Rotation2d())); + private static final Supplier BackBumpASide = + () -> AllianceFlipUtil.apply(new Pose2d(3.3, 5.496349811553955, new Rotation2d())); + private static final Supplier BackTrenchASide = + () -> + AllianceFlipUtil.apply( + new Pose2d( + 3.3395299911499023, + 7.367389678955078, + new Rotation2d(Units.degreesToRadians(-90)))); + private static final Supplier FrontTrenchASide = + () -> + AllianceFlipUtil.apply( + new Pose2d(5.9, 7.367389678955078, new Rotation2d(Units.degreesToRadians(-90)))); + private static final Supplier BackHubASide = + () -> + AllianceFlipUtil.apply( + new Pose2d(5.9, 4.367389678955078, new Rotation2d(Units.degreesToRadians(-90)))); + + private Command ppCycleAlternate(String path, Supplier startPose) { + return Commands.sequence( + Commands.runOnce(() -> drive.setPose(startAside.get())), + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + orchestrator.spinUpShooterHub()) + .withTimeout(14.5), + new StraightDriveToPose(drive, FrontBumpASide).withTimeout(2.0), + new StraightDriveToPose(drive, BackBumpASide).withTimeout(2.1), + orchestrator.aimToHub().withTimeout(2.5), + Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); + } /** * Helper function for a single cycle auto with a constant shooting speed @@ -132,9 +171,10 @@ private Command ppCycleConnect(String path) { orchestrator.aimToHub().withTimeout(2.5), Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooter(1214), - orchestrator.feedUp())); + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp()) + .withTimeout(2.5)); } /** @@ -200,6 +240,32 @@ private Command pp2CycleRegression(String path1, String path2, Supplier .andThen(ppCycleRegressionConnect(path2)); } + public Command ppACycleAlternate() { + return Commands.sequence( + ppCycleAlternate("A-Cycle1", startAside).withTimeout(10.5), + shooter.stop(), + new StraightDriveToPose(drive, BackTrenchASide).withTimeout(1.8), + new StraightDriveToPose(drive, FrontTrenchASide).withTimeout(1.8), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS), + new StraightDriveToPose(drive, BackHubASide)) + .withTimeout(2.0), + new StraightDriveToPose(drive, FrontBumpASide).withTimeout(1.5), + Commands.parallel( + new StraightDriveToPose(drive, BackBumpASide), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .withTimeout(3.0), + orchestrator.aimToHub().withTimeout(3), + Commands.parallel( + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + orchestrator.feedUp()) + .withTimeout(2.0), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + orchestrator.feedUp())); + } + /** * Left side pathplanner auto for a single cycle with fixed shooting distance * From ae2951040ad2e7deff599c45b53d3c312b7b37d2 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Mon, 13 Apr 2026 18:56:48 -0500 Subject: [PATCH 27/36] new autos --- .../deploy/pathplanner/paths/A-Cycle1.path | 26 ++++---- .../deploy/pathplanner/paths/A-Cycle2.path | 66 +++++++++---------- .../deploy/pathplanner/paths/B-Cycle1.path | 4 +- .../deploy/pathplanner/paths/B-Cycle2.path | 4 +- src/main/deploy/pathplanner/settings.json | 6 +- src/main/java/frc/robot/Orchestrator.java | 6 +- .../subsystems/drive/DriveConstants.java | 2 +- .../subsystems/intake/IntakeConstants.java | 2 +- .../robot/subsystems/leds/LedConstants.java | 2 +- 9 files changed, 58 insertions(+), 60 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index bc79df9f0..87ba06731 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 6.7285867620751345, - "y": 7.4991234347048294 + "x": 6.797094017094016, + "y": 5.611267806267806 }, "prevControl": { - "x": 8.286225402504472, - "y": 7.482898032200357 + "x": 8.354732657523353, + "y": 5.595042403763333 }, "nextControl": { - "x": 5.365289737415033, - "y": 7.513324445378372 + "x": 5.433796992433915, + "y": 5.625468816941349 }, "isLocked": false, "linkedName": null @@ -65,15 +65,15 @@ { "anchor": { "x": 2.753363148479427, - "y": 7.369320214669052 + "y": 5.611267806267806 }, "prevControl": { - "x": 3.1537298216336502, - "y": 7.537383389137978 + "x": 3.1304660226829424, + "y": 5.826516791985741 }, "nextControl": { - "x": 1.558346427254841, - "y": 6.867684296448937 + "x": 2.4817378917378914, + "y": 5.456225071225072 }, "isLocked": false, "linkedName": null @@ -145,8 +145,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 6.0, + "maxVelocity": 1.8, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 75bf9b57c..b7fcf9e69 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -48,76 +48,76 @@ }, { "anchor": { - "x": 6.809713774597495, - "y": 3.9619856887298743 + "x": 7.171780626780627, + "y": 3.9962393162393166 }, "prevControl": { - "x": 7.0616430314558745, - "y": 4.16127644281574 + "x": 7.270845035097081, + "y": 3.554259648365908 }, "nextControl": { - "x": 6.297174467550033, - "y": 3.556537166101337 + "x": 7.003817663817664, + "y": 4.745612535612536 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.06334525939177, - "y": 4.513649373881932 + "x": 6.616210826210827, + "y": 5.120299145299145 }, "prevControl": { - "x": 6.202876338033889, - "y": 4.306210037947382 + "x": 7.094259259259258, + "y": 4.810213675213675 }, "nextControl": { - "x": 5.846552903139872, - "y": 4.83595221031256 + "x": 6.183034659057372, + "y": 5.401278280750036 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.771288014311271, - "y": 7.447760964912282 + "x": 5.28542735042735, + "y": 5.288262108262108 }, "prevControl": { - "x": 6.858389982110913, - "y": 7.448760964912282 + "x": 6.435327635327635, + "y": 5.27534188034188 }, "nextControl": { - "x": 5.164488763675691, - "y": 7.447202784292056 + "x": 4.6786661426501785, + "y": 5.295079649922526 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.142772808586762, - "y": 7.447760964912282 + "x": 3.928803418803419, + "y": 5.288262108262108 }, "prevControl": { - "x": 3.9684146523961386, - "y": 7.43239065104425 + "x": 4.754445262612792, + "y": 5.272891794394076 }, "nextControl": { - "x": 2.7264188920420676, - "y": 7.455511892781865 + "x": 3.5124495022587245, + "y": 5.296013036131691 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8831663685152056, - "y": 5.909033989266548 + "x": 2.753363148479427, + "y": 4.805706618962433 }, "prevControl": { - "x": 1.8461433351002032, - "y": 6.781400700455524 + "x": 3.42491452991453, + "y": 5.301182336182337 }, "nextControl": null, "isLocked": false, @@ -139,19 +139,19 @@ }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": -150.0 + "rotationDegrees": -89.41916770857625 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": 80.0 + "rotationDegrees": -1.3430615380779554 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": 90.0 + "rotationDegrees": -1.3730703273820928 }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": 90.0 + "rotationDegrees": -0.749725876000854 } ], "constraintZones": [ @@ -181,7 +181,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -51.09865544424842 + "rotation": -20.612591739110343 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index db06b8000..dc478d301 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -132,8 +132,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 6.0, + "maxVelocity": 1.8, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index c2d81162a..3a3373947 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -172,8 +172,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 6.0, + "maxVelocity": 1.8, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9f4a5d875..8f4804d02 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 2.5, - "defaultMaxAccel": 6.0, + "defaultMaxVel": 1.8, + "defaultMaxAccel": 3.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, @@ -29,4 +29,4 @@ "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 9e7e8f96b..1e38e5d1b 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -270,9 +270,7 @@ public Command spinUpShooter(double velocityRPM) { public Command shootWhileRetractingIntake(Command shooterCommand) { return Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - shooterCommand, - feedUp()) + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), shooterCommand, feedUp()) .withName("shootWhileRetractingIntake"); } @@ -314,7 +312,7 @@ public Command aimToHub() { drive.getPose().getX(), drive.getPose().getY(), AllianceFlipUtil.apply(Hub.blueCenter) - .plus(new Translation2d(-0.6, 0)) + .plus(new Translation2d(-0.55, 0)) .minus( drive .getPose() diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index ce75e28ba..993178924 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -96,7 +96,7 @@ public class DriveConstants { (2 * PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec // Turn PID configuration - public static final double turnKp = 3.5; + public static final double turnKp = 12.0; public static final double turnKd = 0.0; public static final double turnSimP = 9.0; public static final double turnSimD = 0.0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index fd09d8c8c..6991842e1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -33,6 +33,6 @@ public class IntakeConstants { public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; - public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 20; + public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 30; public static final double INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT = 45; } diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java index 0f08a0e3e..b37a506a3 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java @@ -7,7 +7,7 @@ public class LedConstants { public static final int LED_COUNT; /* num LEDs - change the first two numbers based on actual led strip */ - public static final int FULL_LENGTH = 11; // 2026 robot LED count + public static final int FULL_LENGTH = 43; // 2026 robot LED count public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH); public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2; From ff7b3eb4b1904e9687bdc855f9556bd824e7cd7e Mon Sep 17 00:00:00 2001 From: EJainDev Date: Mon, 13 Apr 2026 20:32:24 -0400 Subject: [PATCH 28/36] Revert "A-Cycle fixes" This reverts commit 58d32945dac4be37728204336c34cc2e450a087a. --- src/main/deploy/pathplanner/settings.json | 12 ++-- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/auto/Autos.java | 72 +------------------ 3 files changed, 10 insertions(+), 76 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 04d5ebb58..9f4a5d875 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,8 +9,8 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 50.8, - "robotMOI": 0.1814, + "robotMass": 51.7095, + "robotMOI": 0.7, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 8.16, @@ -19,13 +19,13 @@ "driveCurrentLimit": 46.0, "wheelCOF": 1.2, "flModuleX": 0.273, - "flModuleY": 0.244, + "flModuleY": 0.299, "frModuleX": 0.273, - "frModuleY": -0.244, + "frModuleY": -0.299, "blModuleX": -0.273, - "blModuleY": 0.244, + "blModuleY": 0.299, "brModuleX": -0.273, - "brModuleY": -0.244, + "brModuleY": -0.299, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d3dc79dc5..c37c543a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -260,7 +260,7 @@ public RobotContainer() { // pp - autoChooser.addOption("A-side", autos.ppACycleAlternate()); + autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index b40133d2c..0ee125de8 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -98,45 +98,6 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); - private static final Supplier FrontBumpASide = - () -> - AllianceFlipUtil.apply( - new Pose2d(5.775780200958252, 5.496349811553955, new Rotation2d())); - private static final Supplier BackBumpASide = - () -> AllianceFlipUtil.apply(new Pose2d(3.3, 5.496349811553955, new Rotation2d())); - private static final Supplier BackTrenchASide = - () -> - AllianceFlipUtil.apply( - new Pose2d( - 3.3395299911499023, - 7.367389678955078, - new Rotation2d(Units.degreesToRadians(-90)))); - private static final Supplier FrontTrenchASide = - () -> - AllianceFlipUtil.apply( - new Pose2d(5.9, 7.367389678955078, new Rotation2d(Units.degreesToRadians(-90)))); - private static final Supplier BackHubASide = - () -> - AllianceFlipUtil.apply( - new Pose2d(5.9, 4.367389678955078, new Rotation2d(Units.degreesToRadians(-90)))); - - private Command ppCycleAlternate(String path, Supplier startPose) { - return Commands.sequence( - Commands.runOnce(() -> drive.setPose(startAside.get())), - Commands.deadline( - drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - orchestrator.spinUpShooterHub()) - .withTimeout(14.5), - new StraightDriveToPose(drive, FrontBumpASide).withTimeout(2.0), - new StraightDriveToPose(drive, BackBumpASide).withTimeout(2.1), - orchestrator.aimToHub().withTimeout(2.5), - Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooter(1214), - orchestrator.feedUp())); - } /** * Helper function for a single cycle auto with a constant shooting speed @@ -171,10 +132,9 @@ private Command ppCycleConnect(String path) { orchestrator.aimToHub().withTimeout(2.5), Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooter(1214), - orchestrator.feedUp()) - .withTimeout(2.5)); + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); } /** @@ -240,32 +200,6 @@ private Command pp2CycleRegression(String path1, String path2, Supplier .andThen(ppCycleRegressionConnect(path2)); } - public Command ppACycleAlternate() { - return Commands.sequence( - ppCycleAlternate("A-Cycle1", startAside).withTimeout(10.5), - shooter.stop(), - new StraightDriveToPose(drive, BackTrenchASide).withTimeout(1.8), - new StraightDriveToPose(drive, FrontTrenchASide).withTimeout(1.8), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS), - new StraightDriveToPose(drive, BackHubASide)) - .withTimeout(2.0), - new StraightDriveToPose(drive, FrontBumpASide).withTimeout(1.5), - Commands.parallel( - new StraightDriveToPose(drive, BackBumpASide), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) - .withTimeout(3.0), - orchestrator.aimToHub().withTimeout(3), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.0), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); - } - /** * Left side pathplanner auto for a single cycle with fixed shooting distance * From a0e7e17792f52eb4d798f057520e7a117172efba Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Tue, 14 Apr 2026 17:09:27 -0400 Subject: [PATCH 29/36] spin up shooter on the way back --- src/main/java/frc/robot/Orchestrator.java | 1 + src/main/java/frc/robot/commands/auto/Autos.java | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 9e7e8f96b..20b4f9da8 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -260,6 +260,7 @@ public Command spinUpShooterDistance(Supplier targetDistance) { return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance)); } + public Command spinUpShooterHub() { return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)); } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 0ee125de8..5b9e7d56d 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -149,7 +149,9 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(12.5) + .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(2.5), @@ -168,7 +170,9 @@ private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(12.5) + .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(2.5), From 85eeeb7c99641004e7f7490d94c82a5a75f52fde Mon Sep 17 00:00:00 2001 From: EJainDev Date: Tue, 14 Apr 2026 17:19:20 -0400 Subject: [PATCH 30/36] Revert "spin up shooter on the way back" This reverts commit a0e7e17792f52eb4d798f057520e7a117172efba. --- src/main/java/frc/robot/Orchestrator.java | 1 - src/main/java/frc/robot/commands/auto/Autos.java | 8 ++------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 20b4f9da8..9e7e8f96b 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -260,7 +260,6 @@ public Command spinUpShooterDistance(Supplier targetDistance) { return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance)); } - public Command spinUpShooterHub() { return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)); } diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 5b9e7d56d..0ee125de8 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -149,9 +149,7 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(12.5) - .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(2.5), @@ -170,9 +168,7 @@ private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), - Commands.waitSeconds(12.5) - .andThen(orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()))) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) .withTimeout(14.5), Commands.deadline( orchestrator.aimToHub().withTimeout(2.5), From 70e6f85ba16e26ff214027ef08049cba253b4800 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Tue, 14 Apr 2026 19:04:56 -0500 Subject: [PATCH 31/36] dance room regression and other cahnges --- .../deploy/pathplanner/paths/A-Cycle1.path | 60 ++++------ .../deploy/pathplanner/paths/A-Cycle2.path | 110 +++++++++++------- .../deploy/pathplanner/paths/B-Cycle1.path | 4 +- .../deploy/pathplanner/paths/B-Cycle2.path | 4 +- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Orchestrator.java | 43 ++++++- src/main/java/frc/robot/RobotContainer.java | 7 +- .../java/frc/robot/commands/auto/Autos.java | 48 ++++---- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/drive/DriveConstants.java | 2 +- .../frc/robot/subsystems/intake/Intake.java | 5 + .../subsystems/intake/IntakeConstants.java | 3 +- .../frc/robot/subsystems/shooter/Shooter.java | 8 +- .../subsystems/shooter/ShooterConstants.java | 4 +- .../subsystems/vision/VisionConstants.java | 6 +- 15 files changed, 188 insertions(+), 122 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path index 87ba06731..6c49fd076 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -33,15 +33,15 @@ { "anchor": { "x": 7.783237924865832, - "y": 4.254042933810375 + "y": 4.805706618962433 }, "prevControl": { - "x": 7.831914132379248, - "y": 4.594776386404293 + "x": 7.77903133903134, + "y": 5.171980056980058 }, "nextControl": { - "x": 7.746136792727668, - "y": 3.9943350088432332 + "x": 7.786250707881794, + "y": 4.543379297786823 }, "isLocked": false, "linkedName": null @@ -52,40 +52,24 @@ "y": 5.611267806267806 }, "prevControl": { - "x": 8.354732657523353, - "y": 5.595042403763333 + "x": 7.802016105528991, + "y": 5.5605141654377555 }, "nextControl": { - "x": 5.433796992433915, - "y": 5.625468816941349 + "x": 5.517991452991453, + "y": 5.675868945868947 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.753363148479427, + "x": 2.533418803418803, "y": 5.611267806267806 }, "prevControl": { - "x": 3.1304660226829424, - "y": 5.826516791985741 - }, - "nextControl": { - "x": 2.4817378917378914, - "y": 5.456225071225072 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.753363148479427, - "y": 4.805706618962433 - }, - "prevControl": { - "x": 1.9766616214151478, - "y": 5.30522007052828 + "x": 3.334472934472934, + "y": 5.675868945868947 }, "nextControl": null, "isLocked": false, @@ -106,11 +90,15 @@ "rotationDegrees": -90.0 }, { - "waypointRelativePos": 3.0160427807486627, + "waypointRelativePos": 3.0, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.8502673796791456, + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, "rotationDegrees": 0.0 } ], @@ -130,10 +118,10 @@ }, { "name": "Constraints Zone", - "minWaypointRelativePos": 4.024307900067523, - "maxWaypointRelativePos": 4.956110735989194, + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, "constraints": { - "maxVelocity": 1.7, + "maxVelocity": 2.3, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -145,8 +133,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.8, - "maxAcceleration": 3.5, + "maxVelocity": 8.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -154,7 +142,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -23.436 + "rotation": -37.18470645323313 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index b7fcf9e69..04a650ce2 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -3,28 +3,28 @@ "waypoints": [ { "anchor": { - "x": 2.753363148479427, - "y": 4.805706618962433 + "x": 2.533418803418803, + "y": 5.611267806267806 }, "prevControl": null, "nextControl": { - "x": 2.48764415498757, - "y": 5.355144522906272 + "x": 2.267699809926946, + "y": 6.160705710211645 }, "isLocked": false, "linkedName": "A-Cycle1-End" }, { "anchor": { - "x": 2.477531305903399, + "x": 2.5075783475783475, "y": 7.447760964912282 }, "prevControl": { - "x": 2.039445438282648, + "x": 2.0694924799575967, "y": 7.351408549885447 }, "nextControl": { - "x": 2.760343374071401, + "x": 2.7903904157463497, "y": 7.509962528077773 }, "isLocked": false, @@ -32,15 +32,15 @@ }, { "anchor": { - "x": 6.809713774597495, + "x": 5.634273504273503, "y": 7.447760964912282 }, "prevControl": { - "x": 5.320357204173185, + "x": 4.144916933849193, "y": 7.530332248967907 }, "nextControl": { - "x": 7.669660107334526, + "x": 6.494219837010534, "y": 7.400084757398864 }, "isLocked": false, @@ -48,76 +48,76 @@ }, { "anchor": { - "x": 7.171780626780627, - "y": 3.9962393162393166 + "x": 6.202763532763532, + "y": 4.060840455840457 }, "prevControl": { - "x": 7.270845035097081, - "y": 3.554259648365908 + "x": 6.215683760683759, + "y": 3.569871794871795 }, "nextControl": { - "x": 7.003817663817664, - "y": 4.745612535612536 + "x": 6.1825608953181, + "y": 4.828540678766911 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.616210826210827, - "y": 5.120299145299145 + "x": 5.866837606837606, + "y": 5.611 }, "prevControl": { - "x": 7.094259259259258, - "y": 4.810213675213675 + "x": 6.590370370370371, + "y": 5.624188034188035 }, "nextControl": { - "x": 6.183034659057372, - "y": 5.401278280750036 + "x": 5.351084085527113, + "y": 5.601599216214575 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.28542735042735, - "y": 5.288262108262108 + "x": 4.755698005698005, + "y": 5.611 }, "prevControl": { - "x": 6.435327635327635, - "y": 5.27534188034188 + "x": 5.905598290598291, + "y": 5.598079772079772 }, "nextControl": { - "x": 4.6786661426501785, - "y": 5.295079649922526 + "x": 4.148936797920834, + "y": 5.6178175416604175 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.928803418803419, - "y": 5.288262108262108 + "x": 3.760840455840455, + "y": 5.611 }, "prevControl": { - "x": 4.754445262612792, - "y": 5.272891794394076 + "x": 4.6006552706552695, + "y": 5.598347578347578 }, "nextControl": { - "x": 3.5124495022587245, - "y": 5.296013036131691 + "x": 3.34446165048371, + "y": 5.61727304986715 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.753363148479427, - "y": 4.805706618962433 + "x": 2.533418803418803, + "y": 5.611267806267806 }, "prevControl": { - "x": 3.42491452991453, - "y": 5.301182336182337 + "x": 3.476099715099715, + "y": 5.624455840455841 }, "nextControl": null, "isLocked": false, @@ -151,7 +151,7 @@ }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": -0.749725876000854 + "rotationDegrees": -0.7497258760008542 } ], "constraintZones": [ @@ -167,12 +167,38 @@ "nominalVoltage": 12, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, + "maxVelocity": 8.0, "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -181,13 +207,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -20.612591739110343 + "rotation": -37.195226690262515 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -23.4357656437963 + "rotation": -37.18470645323313 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index dc478d301..e8711eefc 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -132,8 +132,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.8, - "maxAcceleration": 3.5, + "maxVelocity": 8.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index 3a3373947..dccdb4e65 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -172,8 +172,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.8, - "maxAcceleration": 3.5, + "maxVelocity": 8.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8f4804d02..2e5cfd4e0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 1.8, - "defaultMaxAccel": 3.5, + "defaultMaxVel": 8.0, + "defaultMaxAccel": 6.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 1e38e5d1b..37135b86b 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -142,6 +142,47 @@ public ZoneId getCurrentZone() { return ZoneId.NONE; } + public Command zoneBasedShooter() { + DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); + + return new SelectCommand<>( + Map.ofEntries( + Map.entry(ZoneId.ZONE_1, spinUpShooterDistance(getHubDistance())), + Map.entry( + ZoneId.ZONE_2, + new ConditionalCommand( + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpClosePose) + .getTranslation() + .getDistance((drive.getPose().getTranslation())))), + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))), + () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)), + Map.entry( + ZoneId.ZONE_3, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpFarPose) + .getTranslation() + .getDistance(drive.getPose().getTranslation())))), + Map.entry( + ZoneId.ZONE_4, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))))), + this::getCurrentZone); + } + public Command zoneBasedAim() { DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); @@ -312,7 +353,7 @@ public Command aimToHub() { drive.getPose().getX(), drive.getPose().getY(), AllianceFlipUtil.apply(Hub.blueCenter) - .plus(new Translation2d(-0.55, 0)) + .plus(new Translation2d(-0.4, 0)) .minus( drive .getPose() diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c37c543a6..a205ec02e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -186,7 +186,7 @@ public RobotContainer() { drive::setPose, drive::getChassisSpeeds, drive::runVelocity, - new PPHolonomicDriveController(new PIDConstants(9.0, 0, 0), new PIDConstants(6, 0, 0)), + new PPHolonomicDriveController(new PIDConstants(14.0, 0, 0), new PIDConstants(7, 0, 0)), ppConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, drive); @@ -300,7 +300,7 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.aimToHub()); + driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() @@ -341,8 +341,7 @@ private void configureButtonBindings() { operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) - .toggleOnTrue( - orchestrator.spinUpShooterDistance(orchestrator.getShootWhileDrivingResultDistance())); + .toggleOnTrue(orchestrator.zoneBasedShooter()); operatorController .rightTrigger(0.1) .and(operatorController.pov(0)) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 0ee125de8..a2cb262c9 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.intake.IntakeConstants; import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; /** Contains all autos */ @@ -149,38 +150,39 @@ private Command ppCycleRegression(String path, Supplier startPose) { Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) - .withTimeout(14.5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .withTimeout(15.5), Commands.deadline( - orchestrator.aimToHub().withTimeout(2.5), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); } private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( drive.getAutonomousCommand(path), - intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5)) + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) .withTimeout(14.5), Commands.deadline( - orchestrator.aimToHub().withTimeout(2.5), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())), - Commands.parallel( - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp()) - .withTimeout(2.5), - Commands.parallel( - intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), - orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), - orchestrator.feedUp())); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())))); } /** diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 09d8613f1..4d405d38d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { - // logCameraPositions(); // uncomment to show camera positions in advantage scope + logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 993178924..a469e5c73 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -61,7 +61,7 @@ public class DriveConstants { public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.905); + public static final double wheelRadiusMeters = Units.inchesToMeters(2); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e1db70243..4d41fba74 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -29,6 +29,11 @@ public Command runIntakeMotor() { return rollers.intake(); } + public Command slowlyBringInIntake() { + return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS)) + .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); + } + // Jack's Chugga Chugga mode public Command shakeAndIntake() { return Commands.repeatingSequence( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 6991842e1..426486a16 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -18,7 +18,7 @@ public class IntakeConstants { public static final int EXTEND_LIMIT_ID = 0; - public static final double PID_P = 0.023; // arbritrary values + public static final double PID_P = 0.046; // arbritrary values public static final double PID_I = 0.000004; public static final double PID_D = 0.00001; @@ -29,6 +29,7 @@ public class IntakeConstants { public static final double SHAKE_POS_OFFSET = 1; public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; + public static final double SLOW_VOLTS = 2; // How close we need to be to collapse position to stop the intake rollers public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 569fd178f..80c857bb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -44,7 +44,7 @@ public class Shooter extends SubsystemBase { // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit - private final SlewRateLimiter targetRamper = new SlewRateLimiter(1600); + private final SlewRateLimiter targetRamper = new SlewRateLimiter(800); private double feedForwardScalar; /** @@ -263,7 +263,11 @@ public Time getAirTimeSeconds(Distance distance) { public Supplier calculateSetpoint(Supplier distance) { // calculate rpm depending on distance return () -> { - AngularVelocity velocity = RPM.of(102.4367 * distance.get().in(Meters) + 799.60799); + AngularVelocity velocity = + RPM.of( + 25.30184 * Math.pow(distance.get().in(Meters), 2) + - 47.12642 * distance.get().in(Meters) + + 1001.70713); if (velocity.gt(RPM.of(1550))) { return RPM.of(1550); } else return velocity; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index e2e0cd5f3..f784e5bea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -11,7 +11,7 @@ public class ShooterConstants { public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; public static final double VOLTAGE_COMPENSATION = 12.0; - public static final int CURRENT_LIMIT = 55; + public static final int CURRENT_LIMIT = 35; public static final IdleMode IDLE_MODE = IdleMode.kCoast; public static final double KV = 0.0662; @@ -25,7 +25,7 @@ public class ShooterConstants { public static final double FF_SCALAR = 1; public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; - public static final double CLOSE_HUB_SHOOTER_RPM = 850; + public static final double CLOSE_HUB_SHOOTER_RPM = 1100; public static final double MAX_VOLTAGE = 12.0; public static final double TOLERANCE = diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 109cc6887..463e6ce91 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,7 +24,7 @@ public class VisionConstants { private static Rotation3d RotationCorrection = new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis public static Transform3d robotToCamera0 = - new Transform3d( // front camera + new Transform3d( new Translation3d( Units.inchesToMeters(-8.931), Units.inchesToMeters(12.349), @@ -35,7 +35,7 @@ public class VisionConstants { Units.degreesToRadians(0.0), Units.degreesToRadians(-158.771))); public static Transform3d robotToCamera1 = - new Transform3d( // rear left camera + new Transform3d( new Translation3d( Units.inchesToMeters(8.931), // x Units.inchesToMeters(12.349), // y @@ -44,7 +44,7 @@ public class VisionConstants { new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(0.0), - Units.degreesToRadians(-201.229))); + Units.degreesToRadians(158.771))); public static Transform3d robotToCamera2 = new Transform3d( // rear right camera From b3767b5192dc790f3e1f81ac4d89a1bd7afa3455 Mon Sep 17 00:00:00 2001 From: Pranav Gundu Date: Tue, 14 Apr 2026 22:05:47 -0400 Subject: [PATCH 32/36] b side autos --- .../deploy/pathplanner/paths/A-Cycle2.path | 8 +- .../deploy/pathplanner/paths/B-Cycle1.path | 109 ++++++------- .../deploy/pathplanner/paths/B-Cycle2.path | 144 +++++++++++------- 3 files changed, 144 insertions(+), 117 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path index 04a650ce2..811891d20 100644 --- a/src/main/deploy/pathplanner/paths/A-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -139,19 +139,19 @@ }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": -89.41916770857625 + "rotationDegrees": -90.0 }, { "waypointRelativePos": 3.9622641509434033, - "rotationDegrees": -1.3430615380779554 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": -1.3730703273820928 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": -0.7497258760008542 + "rotationDegrees": 0.0 } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path index e8711eefc..632dc384e 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle1.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -3,93 +3,77 @@ "waypoints": [ { "anchor": { - "x": 4.295, - "y": 0.5380000000000011 + "x": 4.424579606440071, + "y": 0.6996797853309484 }, "prevControl": null, "nextControl": { - "x": 5.260984348859008, - "y": 0.548709162061195 + "x": 5.438196256563948, + "y": 0.6560842522924792 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.588533094812165, - "y": 1.2361180679785324 + "x": 7.783237924865832, + "y": 1.0404132379248665 }, "prevControl": { - "x": 7.333281138772382, - "y": 0.5372671705796467 + "x": 7.767012522361359, + "y": 0.5374257602862258 }, "nextControl": { - "x": 8.01198853278679, - "y": 2.3954910162934935 + "x": 7.817452345049281, + "y": 2.1010602636117754 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.896815742397138, - "y": 4.009661896243292 + "x": 7.783237924865832, + "y": 3.2632933810375677 }, "prevControl": { - "x": 7.956228852277296, - "y": 3.7035208327957765 + "x": 7.77903133903134, + "y": 2.8970199430199433 }, "nextControl": { - "x": 7.8491866370218535, - "y": 4.255082898443899 + "x": 7.786250707881794, + "y": 3.5256207022131782 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.463005366726297, - "y": 0.6996797853309484 - }, - "prevControl": { - "x": 7.033869255673328, - "y": 1.1479768709205327 - }, - "nextControl": { - "x": 4.179018869195575, - "y": 0.33325249087882547 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.402379248658318, - "y": 0.6996797853309484 + "x": 6.797094017094016, + "y": 2.4577321937321948 }, "prevControl": { - "x": 3.6989618406989995, - "y": 0.5442676850187853 + "x": 7.802016105528991, + "y": 2.5084858345622454 }, "nextControl": { - "x": 2.3915233528638398, - "y": 1.2293778933601753 + "x": 5.517991452991453, + "y": 2.3931310541310538 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.451, - "y": 3.264 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": { - "x": 2.5377853397080075, - "y": 2.6727592008119645 + "x": 3.334472934472934, + "y": 2.3931310541310538 }, "nextControl": null, "isLocked": false, - "linkedName": "B-Cycle-End" + "linkedName": "A-Cycle1-End" } ], "rotationTargets": [ @@ -99,18 +83,22 @@ }, { "waypointRelativePos": 1.171122994652407, - "rotationDegrees": 86.315 + "rotationDegrees": 90.0 }, { "waypointRelativePos": 1.9411764705882377, - "rotationDegrees": 94.0 + "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.0160427807486627, + "waypointRelativePos": 3, "rotationDegrees": 0.0 }, { - "waypointRelativePos": 3.8502673796791456, + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, "rotationDegrees": 0.0 } ], @@ -120,7 +108,20 @@ "minWaypointRelativePos": 1.0508474576271185, "maxWaypointRelativePos": 2.1694915254237293, "constraints": { - "maxVelocity": 1.2, + "maxVelocity": 1.7, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, + "constraints": { + "maxVelocity": 2.3, "maxAcceleration": 6, "maxAngularVelocity": 540, "maxAngularAcceleration": 720, @@ -132,16 +133,16 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 37.717000000000006 + "rotation": -37.18470645323313 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path index dccdb4e65..a28ef683d 100644 --- a/src/main/deploy/pathplanner/paths/B-Cycle2.path +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -3,121 +3,121 @@ "waypoints": [ { "anchor": { - "x": 3.4510554561717353, - "y": 3.2642933810375667 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": null, "nextControl": { - "x": 3.2421167389384307, - "y": 2.967186063127149 + "x": 2.267699809926946, + "y": 1.9082942897883557 }, "isLocked": false, - "linkedName": "B-Cycle-End" + "linkedName": "A-Cycle1-End" }, { "anchor": { - "x": 3.256350626118068, - "y": 0.8142576028622539 + "x": 2.5075783475783475, + "y": 0.6212390350877186 }, "prevControl": { - "x": 1.5889035900296817, - "y": 2.0483193166651414 + "x": 2.0694924799575967, + "y": 0.717591450114554 }, "nextControl": { - "x": 3.457302404516393, - "y": 0.6655351003330456 + "x": 2.7903904157463497, + "y": 0.5590374719222275 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.4525328947368426, - "y": 0.8142576028622539 + "x": 5.634273504273503, + "y": 0.6212390350877186 }, "prevControl": { - "x": 5.752347562091977, - "y": 0.24039328263318283 + "x": 4.144916933849193, + "y": 0.5386677510320939 }, "nextControl": { - "x": 7.910712691624192, - "y": 2.0093659818847356 + "x": 6.494219837010534, + "y": 0.6689152426011367 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.327521929824561, - "y": 3.6599671052631573 + "x": 6.202763532763532, + "y": 4.008159544159544 }, "prevControl": { - "x": 6.59100273960581, - "y": 3.4940005006714894 + "x": 6.215683760683759, + "y": 4.499128205128206 }, "nextControl": { - "x": 5.920916329346852, - "y": 3.9160880386065315 + "x": 6.1825608953181, + "y": 3.2404593212330894 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 3.515723684210526 + "x": 5.866837606837606, + "y": 2.458000000000001 }, "prevControl": { - "x": 5.837373947319441, - "y": 3.7589081110194775 + "x": 6.590370370370371, + "y": 2.444811965811966 }, "nextControl": { - "x": 5.6945270228567875, - "y": 3.159737112788334 + "x": 5.351084085527113, + "y": 2.4674007837854255 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.779396929824561, - "y": 0.6212390350877186 + "x": 4.755698005698005, + "y": 2.458000000000001 }, "prevControl": { - "x": 6.736601761736383, - "y": 1.2079600435401154 + "x": 5.905598290598291, + "y": 2.470920227920229 }, "nextControl": { - "x": 5.5662512875867165, - "y": 0.4905909000263373 + "x": 4.148936797920834, + "y": 2.4511824583395834 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.1445504385964913, - "y": 0.6212390350877186 + "x": 3.760840455840455, + "y": 2.458000000000001 }, "prevControl": { - "x": 3.5589239547141958, - "y": 0.44627660534504465 + "x": 4.6006552706552695, + "y": 2.4706524216524226 }, "nextControl": { - "x": 2.8278583396709256, - "y": 0.7549570864425861 + "x": 3.34446165048371, + "y": 2.4517269501328505 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.548344298245614, - "y": 1.7848026315789465 + "x": 2.533418803418803, + "y": 2.4577321937321948 }, "prevControl": { - "x": 1.9675297251591926, - "y": 1.3628954494520977 + "x": 3.476099715099715, + "y": 2.4445441595441597 }, "nextControl": null, "isLocked": false, @@ -139,26 +139,26 @@ }, { "waypointRelativePos": 2.9433962264150932, - "rotationDegrees": 119.99999999999999 + "rotationDegrees": 90.0 }, { - "waypointRelativePos": 3.962264150943403, - "rotationDegrees": -100.0 + "waypointRelativePos": 3.9622641509434033, + "rotationDegrees": 0.0 }, { "waypointRelativePos": 4.688679245283029, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 }, { "waypointRelativePos": 5.734276729559759, - "rotationDegrees": -90.0 + "rotationDegrees": 0.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 2.526032315978456, - "maxWaypointRelativePos": 4.235188509874326, + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 6, @@ -167,27 +167,53 @@ "nominalVoltage": 12, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } } ], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 8.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 38.90134455575158 + "rotation": 52.804773309737485 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 37.71655912222805 + "rotation": -37.18470645323313 }, "useDefaultConstraints": true } \ No newline at end of file From ef99e0c6adab1a171211d0f92773e48893915d7c Mon Sep 17 00:00:00 2001 From: ShayanHasNo <147673319+ShayanHasNo@users.noreply.github.com> Date: Wed, 15 Apr 2026 18:33:31 -0400 Subject: [PATCH 33/36] depot auto --- src/main/deploy/pathplanner/paths/Depot.path | 109 ++++++++++++++++++ .../java/frc/robot/commands/auto/Autos.java | 7 ++ 2 files changed, 116 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/Depot.path diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path new file mode 100644 index 000000000..68d658773 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -0,0 +1,109 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63, + "y": 5.844 + }, + "prevControl": null, + "nextControl": { + "x": 2.9647584973166365, + "y": 5.90890161001789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.282826475849731, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 3.2798809613513087, + "y": 5.881013697895228 + }, + "nextControl": { + "x": 1.8609660107334522, + "y": 5.990161001788907 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5629338103756705, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 1.6650123660517429, + "y": 5.905230265557292 + }, + "nextControl": { + "x": 0.22220035778175273, + "y": 5.973935599284436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9480679785330945, + "y": 4.611001788908765 + }, + "prevControl": { + "x": 1.5202325581395346, + "y": 5.324919499105545 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0160427807486627, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.9358288770053491, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.799999999999997, + "maxWaypointRelativePos": 2.450847457627116, + "constraints": { + "maxVelocity": 3.58, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.897271030947596 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index a2cb262c9..bffb1709e 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -99,6 +99,7 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); + private static final Supplier startDepot = ()->AllianceFlipUtil.apply(new Pose2d(3.630,5.844,new Rotation2d(0.000))); /** * Helper function for a single cycle auto with a constant shooting speed @@ -202,6 +203,12 @@ private Command pp2CycleRegression(String path1, String path2, Supplier .andThen(ppCycleRegressionConnect(path2)); } + /** + * + * Depot Auto + * + */ + public Command ppDepot(){return ppCycleRegression("A-Cycle-LeftSweep",startDepot);}; /** * Left side pathplanner auto for a single cycle with fixed shooting distance * From 25f3db1f66f01d3c00b34f7a56ca209eb726baec Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Wed, 15 Apr 2026 17:34:45 -0500 Subject: [PATCH 34/36] Depo auto cycle command sequence --- .../java/frc/robot/commands/auto/Autos.java | 21 +++++++++++++++++++ .../subsystems/intake/IntakeConstants.java | 2 +- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index a2cb262c9..75779d1d7 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -166,6 +166,27 @@ private Command ppCycleRegression(String path, Supplier startPose) { .withTimeout(1.6))); } + private Command ppCycleRegressionDepo(String path, Supplier startPose) { + return Commands.sequence( + Commands.runOnce(() -> drive.setPose(startPose.get())), + Commands.deadline( + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .withTimeout(15.5), + Commands.deadline( + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); + } + private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 426486a16..263ce3b4d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -23,7 +23,7 @@ public class IntakeConstants { public static final double PID_D = 0.00001; public static final double EXTEND_POS = -13; // TODO: change the actual values - public static final double FUNNEL_POS = -7; + public static final double FUNNEL_POS = -10; // shake around the funnel pos by this much public static final double SHAKE_POS_OFFSET = 1; From a48e17cec21b770bc89f2dea3ea4e84accf1bf95 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Wed, 15 Apr 2026 17:43:12 -0500 Subject: [PATCH 35/36] better depo path --- src/main/deploy/pathplanner/paths/Depot.path | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path index 68d658773..90ed3dfca 100644 --- a/src/main/deploy/pathplanner/paths/Depot.path +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 0.5629338103756705, + "x": 0.685826210826211, "y": 5.9577101967799635 }, "prevControl": { - "x": 1.6650123660517429, - "y": 5.905230265557292 + "x": 1.7065242165242167, + "y": 5.947193732193732 }, "nextControl": { - "x": 0.22220035778175273, - "y": 5.973935599284436 + "x": 0.3447247623997972, + "y": 5.9612246361956025 }, "isLocked": false, "linkedName": null @@ -52,8 +52,8 @@ "y": 4.611001788908765 }, "prevControl": { - "x": 1.5202325581395346, - "y": 5.324919499105545 + "x": 2.365455840455841, + "y": 6.218518518518518 }, "nextControl": null, "isLocked": false, @@ -68,6 +68,10 @@ { "waypointRelativePos": 1.9358288770053491, "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.660980810234532, + "rotationDegrees": 180.0 } ], "constraintZones": [ From eba0a31609d793c83ec39b5156663c6342c7e479 Mon Sep 17 00:00:00 2001 From: Elliot Moore Date: Wed, 15 Apr 2026 17:50:36 -0500 Subject: [PATCH 36/36] spotless apply --- src/main/java/frc/robot/commands/auto/Autos.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index beba3c9a5..06eb4f0a5 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -99,7 +99,8 @@ public Autos( () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); private static final Supplier startBside = () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); - private static final Supplier startDepot = ()->AllianceFlipUtil.apply(new Pose2d(3.630,5.844,new Rotation2d(0.000))); + private static final Supplier startDepot = + () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); /** * Helper function for a single cycle auto with a constant shooting speed @@ -224,12 +225,12 @@ private Command pp2CycleRegression(String path1, String path2, Supplier .andThen(ppCycleRegressionConnect(path2)); } - /** - * - * Depot Auto - * - */ - public Command ppDepot(){return ppCycleRegression("A-Cycle-LeftSweep",startDepot);}; + /** Depot Auto */ + public Command ppDepot() { + return ppCycleRegression("A-Cycle-LeftSweep", startDepot); + } + ; + /** * Left side pathplanner auto for a single cycle with fixed shooting distance *