From fe6c579cbf714f7afc8e7362ccfed6d02475861b Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 18 Sep 2025 17:11:54 -0700 Subject: [PATCH 01/20] Added start for solo controller --- src/main/java/frc/robot/Controls.java | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index b7097488..57b539f2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,15 +1,17 @@ package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -36,7 +38,6 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; -import java.util.function.BooleanSupplier; public class Controls { private static final int DRIVER_CONTROLLER_PORT = 0; @@ -44,12 +45,14 @@ public class Controls { private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 2; private static final int ELEVATOR_CONTROLLER_PORT = 3; private static final int CLIMB_TEST_CONTROLLER_PORT = 4; + private static final int SOLO_CONTROLLER_PORT = 5; private final CommandXboxController driverController; private final CommandXboxController operatorController; private final CommandXboxController armPivotSpinnyClawController; private final CommandXboxController elevatorTestController; private final CommandXboxController climbTestController; + private final CommandXboxController soloController; private final Subsystems s; private final Sensors sensors; @@ -90,6 +93,7 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { armPivotSpinnyClawController = new CommandXboxController(ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT); elevatorTestController = new CommandXboxController(ELEVATOR_CONTROLLER_PORT); climbTestController = new CommandXboxController(CLIMB_TEST_CONTROLLER_PORT); + soloController = new CommandXboxController(SOLO_CONTROLLER_PORT); this.s = s; this.sensors = sensors; this.superStructure = superStructure; @@ -104,6 +108,7 @@ public Controls(Subsystems s, Sensors sensors, SuperStructure superStructure) { configureAutoAlignBindings(); configureGroundSpinnyBindings(); configureGroundArmBindings(); + configureSoloControllerBindings(); } private Trigger connected(CommandXboxController controller) { @@ -821,4 +826,7 @@ public void vibrateCoDriveController(double vibration) { operatorController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } + private void configureSoloControllerBindings() { + //TODO: Add bindings for solo controller + } } From e261298f58aa9c62efa4a5458d981f9140a696f8 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Sat, 20 Sep 2025 01:12:27 -0700 Subject: [PATCH 02/20] All controls for solo controller --- src/main/java/frc/robot/Controls.java | 184 ++++++++++++++++++++++++-- 1 file changed, 174 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 57b539f2..fbae7bc8 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -40,12 +40,12 @@ import frc.robot.util.ScoringMode; public class Controls { - private static final int DRIVER_CONTROLLER_PORT = 0; - private static final int OPERATOR_CONTROLLER_PORT = 1; - private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 2; - private static final int ELEVATOR_CONTROLLER_PORT = 3; - private static final int CLIMB_TEST_CONTROLLER_PORT = 4; - private static final int SOLO_CONTROLLER_PORT = 5; + private static final int DRIVER_CONTROLLER_PORT = 1; + private static final int OPERATOR_CONTROLLER_PORT = 2; + private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 3; + private static final int ELEVATOR_CONTROLLER_PORT = 4; + private static final int CLIMB_TEST_CONTROLLER_PORT = 5; + private static final int SOLO_CONTROLLER_PORT = 0; private final CommandXboxController driverController; private final CommandXboxController operatorController; @@ -161,9 +161,10 @@ private void configureDrivebaseBindings() { .applyRequest( () -> drive - .withVelocityX(getDriveX()) - .withVelocityY(getDriveY()) - .withRotationalRate(getDriveRotate())) + .withVelocityX(soloController.isConnected() ? getSoloDriveX() : getDriveX()) + .withVelocityY(soloController.isConnected() ? getSoloDriveY() : getDriveY()) + .withRotationalRate( + soloController.isConnected() ? getSoloDriveRotate() : getDriveRotate())) .withName("Drive")); // various former controls that were previously used and could be referenced in the future @@ -826,7 +827,170 @@ public void vibrateCoDriveController(double vibration) { operatorController.getHID().setRumble(RumbleType.kBothRumble, vibration); } } + + // Drive for Solo controller + // takes the X value from the joystic, and applies a deadband and input scaling + private double getSoloDriveX() { + // Joystick +Y is back + // Robot +X is forward + double input = MathUtil.applyDeadband(-soloController.getLeftY(), 0.1); + return input * MaxSpeed; + } + + // takes the Y value from the joystic, and applies a deadband and input scaling + private double getSoloDriveY() { + // Joystick +X is right + // Robot +Y is left + double input = MathUtil.applyDeadband(-soloController.getLeftX(), 0.1); + return input * MaxSpeed; + } + + // takes the rotation value from the joystic, and applies a deadband and input scaling + private double getSoloDriveRotate() { + // Joystick +X is right + // Robot +angle is CCW (left) + double input = MathUtil.applyDeadband(-soloController.getRightX(), 0.1); + return input * MaxSpeed; + } + private void configureSoloControllerBindings() { - //TODO: Add bindings for solo controller + // Barge + Auto align left + soloController + .leftTrigger() + .onTrue( + Commands.deferredProxy( + () -> + switch (scoringMode) { + case CORAL -> getCoralBranchHeightCommand(); + case ALGAE -> Commands.sequence( + BargeAlign.bargeScore( + s.drivebaseSubsystem, + superStructure, + () -> getSoloDriveX(), + () -> getSoloDriveY(), + () -> getSoloDriveRotate(), + soloController.rightBumper()), + getAlgaeIntakeCommand()) + .withName("Algae score then intake"); + }) + .withName("Schedule processor score")); + soloController + .leftTrigger() + .and(() -> scoringMode == ScoringMode.CORAL) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); + // Processor + Auto align right + soloController + .rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + Command scoreCommand = + switch (scoringMode) { + case CORAL -> getCoralBranchHeightCommand(); + case ALGAE -> Commands.sequence( + superStructure.algaeProcessorScore( + soloController.rightBumper()), + Commands.waitSeconds(0.7), + getAlgaeIntakeCommand()) + .withName("Processor score"); + }; + CommandScheduler.getInstance().schedule(scoreCommand); + }) + .withName("score")); + soloController + .rightTrigger() + .and(() -> scoringMode == ScoringMode.CORAL) + .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) + .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); + // Ground Intake + soloController + .leftBumper() + .onTrue( + superStructure + .quickGroundIntake(soloController.leftBumper()) + .withName("Quick Gound intake")); + // Scoring levels coral and algae intake heights + soloController + .y() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_FOUR, AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR) + .withName("coral level 4, algae level 3-4")); + soloController + .x() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_THREE, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 3, algae level 2-3")); + soloController + .b() + .onTrue( + selectScoringHeight( + BranchHeight.CORAL_LEVEL_TWO, AlgaeIntakeHeight.ALGAE_LEVEL_TWO_THREE) + .withName("coral level 2, algae level 2-3")); + soloController + .a() + .onTrue( + selectScoringHeight(BranchHeight.CORAL_LEVEL_ONE, AlgaeIntakeHeight.ALGAE_LEVEL_GROUND) + .withName("coral level 1, algae ground level")); + // Zero Elevator + soloController + .back() + .onTrue( + Commands.parallel( + s.elevatorSubsystem.resetPosZero(), + rumble(soloController, 0.5, Seconds.of(0.3))) + .ignoringDisable(true) + .withName("Reset elevator zero")); + // Reset gyro + soloController + .start() + .onTrue( + s.drivebaseSubsystem + .runOnce(() -> s.drivebaseSubsystem.seedFieldCentric()) + .alongWith(rumble(soloController, 0.5, Seconds.of(0.3))) + .withName("Reset gyro")); + // Scoring mode toggle + soloController + .povDown() + .onTrue( + Commands.runOnce( + () -> { + switch (scoringMode) { + case ALGAE: + scoringMode = ScoringMode.CORAL; + CommandScheduler.getInstance().schedule(superStructure.coralPreIntake()); + CommandScheduler.getInstance().schedule(s.climbPivotSubsystem.toStow()); + break; + case CORAL: + scoringMode = ScoringMode.ALGAE; + CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()); + break; + } + }) + .alongWith(scoringModeSelectRumble()) + .withName("Toggle Scoring Mode")); + // Funnel Out + soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); + // Funnel Climbed + soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); + // Funnel Stow + soloController.povUp().onTrue(superStructure.coralStow()); + // Arm manual + soloController + .rightStick() + .whileTrue( + s.armPivotSubsystem + .startMovingVoltage(() -> Volts.of(3 * soloController.getRightY())) + .withName("Arm Manual Control")); + // Elevator manual + soloController + .leftStick() + .whileTrue( + s.elevatorSubsystem + .startMovingVoltage( + () -> Volts.of(ElevatorSubsystem.UP_VOLTAGE * -soloController.getLeftY())) + .withName("Elevator Manual Control")); } } From d66fb276cd66b08d81ed634df888177874df5d80 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Sat, 20 Sep 2025 01:23:00 -0700 Subject: [PATCH 03/20] spot --- src/main/java/frc/robot/Controls.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 7fdb3148..85fbe49f 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,17 +1,15 @@ package frc.robot; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -38,6 +36,7 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; +import java.util.function.BooleanSupplier; public class Controls { private static final int DRIVER_CONTROLLER_PORT = 1; From 8a941baea41108e2d7e18aa9c11b6bf76934f611 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 20 Sep 2025 17:22:55 -0700 Subject: [PATCH 04/20] drive pratice changes --- src/main/java/frc/robot/Controls.java | 46 +++++++++++-------- .../java/frc/robot/sensors/IntakeSensor.java | 2 +- .../java/frc/robot/subsystems/ArmPivot.java | 4 +- .../java/frc/robot/subsystems/ClimbPivot.java | 2 +- 4 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 85fbe49f..d70f0ce7 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -426,6 +426,15 @@ private Command getCoralBranchHeightCommand() { }; } + private Command getSoloCoralBranchHeightCommand() { + return switch (branchHeight) { + case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(soloController.rightBumper()); + case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(soloController.rightBumper()); + case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(soloController.rightBumper()); + case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(soloController.rightBumper()); + }; + } + private void configureElevatorBindings() { if (s.elevatorSubsystem == null) { return; @@ -869,7 +878,7 @@ private void configureSoloControllerBindings() { Commands.deferredProxy( () -> switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getSoloCoralBranchHeightCommand(); case ALGAE -> Commands.sequence( BargeAlign.bargeScore( s.drivebaseSubsystem, @@ -895,7 +904,7 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand = switch (scoringMode) { - case CORAL -> getCoralBranchHeightCommand(); + case CORAL -> getSoloCoralBranchHeightCommand(); case ALGAE -> Commands.sequence( superStructure.algaeProcessorScore( soloController.rightBumper()), @@ -963,28 +972,25 @@ private void configureSoloControllerBindings() { soloController .povDown() .onTrue( - Commands.runOnce( - () -> { - switch (scoringMode) { - case ALGAE: - scoringMode = ScoringMode.CORAL; - CommandScheduler.getInstance().schedule(superStructure.coralPreIntake()); - CommandScheduler.getInstance().schedule(s.climbPivotSubsystem.toStow()); - break; - case CORAL: - scoringMode = ScoringMode.ALGAE; - CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()); - break; - } - }) + Commands.runOnce(() -> scoringMode = ScoringMode.ALGAE) .alongWith(scoringModeSelectRumble()) - .withName("Toggle Scoring Mode")); + .withName("Algae Scoring Mode")) + .onTrue( + Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) + .withName("run algae intake")); // Funnel Out - soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); + soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbed()); // Funnel Climbed - soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); + soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbOut()); // Funnel Stow - soloController.povUp().onTrue(superStructure.coralStow()); + soloController + .povUp() + .onTrue( + Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode")) + .onTrue(superStructure.coralPreIntake()) + .onTrue(s.climbPivotSubsystem.toStow()); // Arm manual soloController .rightStick() diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 314c6035..34d9e9b6 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 1b08d0b9..cb9958e6 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; // starting low for testing + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 20; // Low b/c belt broke talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 40; // starting low for testing + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 40; // Tuned talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID diff --git a/src/main/java/frc/robot/subsystems/ClimbPivot.java b/src/main/java/frc/robot/subsystems/ClimbPivot.java index f4f82c82..80a4919d 100644 --- a/src/main/java/frc/robot/subsystems/ClimbPivot.java +++ b/src/main/java/frc/robot/subsystems/ClimbPivot.java @@ -56,7 +56,7 @@ public enum TargetPositions { private final double CLIMB_HOLD_CLIMBED = -0.0705; private final double CLIMB_IN_SPEED = -0.75; - private final double climbInKp = 30; + private final double climbInKp = 50; private final double climbOutKp = 50; // positions for relation between motor encoder and WCP encoder From c5f6f8e9c00d0864eb74132f38fcd00892560278 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Wed, 24 Sep 2025 13:56:35 -0700 Subject: [PATCH 05/20] V2 Set up for solo controls --- src/main/java/frc/robot/Controls.java | 124 +++++++++++------- .../java/frc/robot/util/SoloScoringMode.java | 7 + 2 files changed, 82 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/util/SoloScoringMode.java diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index d70f0ce7..df04acb9 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -36,6 +36,7 @@ import frc.robot.util.BranchHeight; import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; +import frc.robot.util.SoloScoringMode; import java.util.function.BooleanSupplier; public class Controls { @@ -59,6 +60,8 @@ public class Controls { private BranchHeight branchHeight = BranchHeight.CORAL_LEVEL_FOUR; private ScoringMode scoringMode = ScoringMode.CORAL; + private ScoringMode intakeMode = ScoringMode.CORAL; + private SoloScoringMode soloScoringMode = SoloScoringMode.NO_GAME_PIECE; private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR; // Swerve stuff @@ -377,6 +380,23 @@ private void configureSuperStructureBindings() { .withName("Automatic Intake")); } + if (sensors.armSensor != null) { + sensors + .armSensor + .inClaw() + .and(RobotModeTriggers.teleop()) + .onTrue( + Commands.runOnce( + () -> { + if (intakeMode == ScoringMode.CORAL) { + soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; + } else { + soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; + } + }) + .withName("Set solo scoring mode")); + } + if (s.climbPivotSubsystem.sensor != null) { s.climbPivotSubsystem .cageDetected() @@ -428,10 +448,18 @@ private Command getCoralBranchHeightCommand() { private Command getSoloCoralBranchHeightCommand() { return switch (branchHeight) { - case CORAL_LEVEL_FOUR -> superStructure.coralLevelFour(soloController.rightBumper()); - case CORAL_LEVEL_THREE -> superStructure.coralLevelThree(soloController.rightBumper()); - case CORAL_LEVEL_TWO -> superStructure.coralLevelTwo(soloController.rightBumper()); - case CORAL_LEVEL_ONE -> superStructure.coralLevelOne(soloController.rightBumper()); + case CORAL_LEVEL_FOUR -> superStructure + .coralLevelFour(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_THREE -> superStructure + .coralLevelThree(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_TWO -> superStructure + .coralLevelTwo(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); + case CORAL_LEVEL_ONE -> superStructure + .coralLevelOne(soloController.rightBumper()) + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE); }; } @@ -805,7 +833,7 @@ private Command selectScoringHeight(BranchHeight b, AlgaeIntakeHeight a) { () -> { branchHeight = b; algaeIntakeHeight = a; - if (scoringMode == ScoringMode.ALGAE + if (intakeMode == ScoringMode.ALGAE && (sensors.armSensor == null || !sensors.armSensor.booleanInClaw())) { CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand()); } @@ -871,53 +899,70 @@ private double getSoloDriveRotate() { } private void configureSoloControllerBindings() { - // Barge + Auto align left + // Barge + Auto align left + Select scoring mode Algae soloController .leftTrigger() .onTrue( Commands.deferredProxy( - () -> - switch (scoringMode) { - case CORAL -> getSoloCoralBranchHeightCommand(); - case ALGAE -> Commands.sequence( - BargeAlign.bargeScore( - s.drivebaseSubsystem, - superStructure, - () -> getSoloDriveX(), - () -> getSoloDriveY(), - () -> getSoloDriveRotate(), - soloController.rightBumper()), - getAlgaeIntakeCommand()) - .withName("Algae score then intake"); - }) - .withName("Schedule processor score")); + () -> + switch (soloScoringMode) { + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand(); + case ALGAE_IN_CLAW -> Commands.sequence( + BargeAlign.bargeScore( + s.drivebaseSubsystem, + superStructure, + () -> getSoloDriveX(), + () -> getSoloDriveY(), + () -> getSoloDriveRotate(), + soloController.rightBumper()), + getAlgaeIntakeCommand() + .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + .withName("Algae score then intake"); + case NO_GAME_PIECE -> Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) + .alongWith(scoringModeSelectRumble()) + .withName("Algae Scoring Mode"), + Commands.runOnce( + () -> + CommandScheduler.getInstance() + .schedule(getAlgaeIntakeCommand())) + .withName("Run Algae Intake")); + })); soloController .leftTrigger() - .and(() -> scoringMode == ScoringMode.CORAL) + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignLeft(s.drivebaseSubsystem, this)); - // Processor + Auto align right + // Processor + Auto align right + Select scoring mode Coral soloController .rightTrigger() .onTrue( Commands.runOnce( () -> { Command scoreCommand = - switch (scoringMode) { - case CORAL -> getSoloCoralBranchHeightCommand(); - case ALGAE -> Commands.sequence( + switch (soloScoringMode) { + case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand(); + case ALGAE_IN_CLAW -> Commands.sequence( superStructure.algaeProcessorScore( soloController.rightBumper()), Commands.waitSeconds(0.7), - getAlgaeIntakeCommand()) + getAlgaeIntakeCommand() + .andThen( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) .withName("Processor score"); + case NO_GAME_PIECE -> Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) + .alongWith(scoringModeSelectRumble()) + .withName("Coral Scoring Mode"), + superStructure.coralPreIntake(), + s.climbPivotSubsystem.toStow()); }; CommandScheduler.getInstance().schedule(scoreCommand); }) .withName("score")); soloController .rightTrigger() - .and(() -> scoringMode == ScoringMode.CORAL) + .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) .and(() -> branchHeight != BranchHeight.CORAL_LEVEL_ONE) .whileTrue(AutoAlign.autoAlignRight(s.drivebaseSubsystem, this)); // Ground Intake @@ -968,29 +1013,10 @@ private void configureSoloControllerBindings() { .runOnce(() -> s.drivebaseSubsystem.seedFieldCentric()) .alongWith(rumble(soloController, 0.5, Seconds.of(0.3))) .withName("Reset gyro")); - // Scoring mode toggle - soloController - .povDown() - .onTrue( - Commands.runOnce(() -> scoringMode = ScoringMode.ALGAE) - .alongWith(scoringModeSelectRumble()) - .withName("Algae Scoring Mode")) - .onTrue( - Commands.runOnce(() -> CommandScheduler.getInstance().schedule(getAlgaeIntakeCommand())) - .withName("run algae intake")); // Funnel Out - soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbed()); + soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); // Funnel Climbed - soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbOut()); - // Funnel Stow - soloController - .povUp() - .onTrue( - Commands.runOnce(() -> scoringMode = ScoringMode.CORAL) - .alongWith(scoringModeSelectRumble()) - .withName("Coral Scoring Mode")) - .onTrue(superStructure.coralPreIntake()) - .onTrue(s.climbPivotSubsystem.toStow()); + soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); // Arm manual soloController .rightStick() diff --git a/src/main/java/frc/robot/util/SoloScoringMode.java b/src/main/java/frc/robot/util/SoloScoringMode.java new file mode 100644 index 00000000..529fc716 --- /dev/null +++ b/src/main/java/frc/robot/util/SoloScoringMode.java @@ -0,0 +1,7 @@ +package frc.robot.util; + +public enum SoloScoringMode { + CORAL_IN_CLAW, + ALGAE_IN_CLAW, + NO_GAME_PIECE; +} From e96486b853208012ca2c7df8743f6e364a152988 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Wed, 24 Sep 2025 14:01:03 -0700 Subject: [PATCH 06/20] Override on driving when sticks are press down --- src/main/java/frc/robot/Controls.java | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index df04acb9..1044e3e9 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,15 +1,17 @@ package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -37,7 +39,6 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; -import java.util.function.BooleanSupplier; public class Controls { private static final int DRIVER_CONTROLLER_PORT = 1; @@ -876,6 +877,9 @@ public void vibrateCoDriveController(double vibration) { // Drive for Solo controller // takes the X value from the joystic, and applies a deadband and input scaling private double getSoloDriveX() { + if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { + return 0; // stop driving if either stick is pressed + } // Joystick +Y is back // Robot +X is forward double input = MathUtil.applyDeadband(-soloController.getLeftY(), 0.1); @@ -884,6 +888,9 @@ private double getSoloDriveX() { // takes the Y value from the joystic, and applies a deadband and input scaling private double getSoloDriveY() { + if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { + return 0; // stop driving if either stick is pressed + } // Joystick +X is right // Robot +Y is left double input = MathUtil.applyDeadband(-soloController.getLeftX(), 0.1); @@ -892,6 +899,9 @@ private double getSoloDriveY() { // takes the rotation value from the joystic, and applies a deadband and input scaling private double getSoloDriveRotate() { + if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { + return 0; // stop driving if either stick is pressed + } // Joystick +X is right // Robot +angle is CCW (left) double input = MathUtil.applyDeadband(-soloController.getRightX(), 0.1); From d5346684ff1b2bdb51b385362699575efc535da8 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Wed, 24 Sep 2025 14:12:37 -0700 Subject: [PATCH 07/20] dpot --- src/main/java/frc/robot/Controls.java | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 1044e3e9..98663186 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,17 +1,15 @@ package frc.robot; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -39,6 +37,7 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; +import java.util.function.BooleanSupplier; public class Controls { private static final int DRIVER_CONTROLLER_PORT = 1; @@ -878,7 +877,7 @@ public void vibrateCoDriveController(double vibration) { // takes the X value from the joystic, and applies a deadband and input scaling private double getSoloDriveX() { if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { - return 0; // stop driving if either stick is pressed + return 0; // stop driving if either stick is pressed } // Joystick +Y is back // Robot +X is forward @@ -889,7 +888,7 @@ private double getSoloDriveX() { // takes the Y value from the joystic, and applies a deadband and input scaling private double getSoloDriveY() { if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { - return 0; // stop driving if either stick is pressed + return 0; // stop driving if either stick is pressed } // Joystick +X is right // Robot +Y is left @@ -900,7 +899,7 @@ private double getSoloDriveY() { // takes the rotation value from the joystic, and applies a deadband and input scaling private double getSoloDriveRotate() { if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { - return 0; // stop driving if either stick is pressed + return 0; // stop driving if either stick is pressed } // Joystick +X is right // Robot +angle is CCW (left) From fc7533b50f9f83382b861b7b6fef3b61d5625f01 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 24 Sep 2025 14:46:54 -0700 Subject: [PATCH 08/20] LEDs for solo mode --- src/main/java/frc/robot/Controls.java | 2 +- src/main/java/frc/robot/sensors/ElevatorLight.java | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 98663186..d99d633b 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -719,7 +719,7 @@ private void configureElevatorLEDBindings() { } s.elevatorLEDSubsystem.setDefaultCommand( - s.elevatorLEDSubsystem.showScoringMode(() -> scoringMode)); + s.elevatorLEDSubsystem.showScoringMode(() -> soloScoringMode)); if (s.elevatorSubsystem != null) { Trigger hasBeenZeroed = new Trigger(s.elevatorSubsystem::getHasBeenZeroed); diff --git a/src/main/java/frc/robot/sensors/ElevatorLight.java b/src/main/java/frc/robot/sensors/ElevatorLight.java index bba94b64..f6139f74 100644 --- a/src/main/java/frc/robot/sensors/ElevatorLight.java +++ b/src/main/java/frc/robot/sensors/ElevatorLight.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Hardware; -import frc.robot.util.ScoringMode; +import frc.robot.util.SoloScoringMode; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -82,13 +82,15 @@ public Command animate(LEDPattern animation, String name) { .withName("Animate" + name); } - public Command showScoringMode(Supplier scoringMode) { + public Command showScoringMode(Supplier scoringMode) { return run(() -> { - ScoringMode currentMode = scoringMode.get(); - if (currentMode == ScoringMode.ALGAE) { + SoloScoringMode currentMode = scoringMode.get(); + if (currentMode == SoloScoringMode.ALGAE_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kTeal)); - } else { + } else if (currentMode == SoloScoringMode.CORAL_IN_CLAW) { updateLEDs(LEDPattern.solid(Color.kWhite)); + } else if (currentMode == SoloScoringMode.NO_GAME_PIECE) { + updateLEDs(LEDPattern.solid(Color.kDeepPink)); } }) .withName("Animate Scoring Mode"); From 460a6872984870a13a7c35e907e3c040f70602b5 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Wed, 24 Sep 2025 18:53:07 -0700 Subject: [PATCH 09/20] Drive practice changes --- src/main/java/frc/robot/Controls.java | 4 +++- src/main/java/frc/robot/sensors/IntakeSensor.java | 4 ++-- src/main/java/frc/robot/subsystems/ArmPivot.java | 4 ++-- src/main/java/frc/robot/subsystems/SpinnyClaw.java | 2 +- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index d99d633b..7380ab27 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -390,8 +390,10 @@ private void configureSuperStructureBindings() { () -> { if (intakeMode == ScoringMode.CORAL) { soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; - } else { + } else if (intakeMode == ScoringMode.ALGAE) { soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; + } else { + soloScoringMode = SoloScoringMode.NO_GAME_PIECE; } }) .withName("Set solo scoring mode")); diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 34d9e9b6..ce72b48e 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -15,7 +15,7 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS - private static final double INTAKE_LOWER_LIMIT = 0.010; + private static final double INTAKE_LOWER_LIMIT = 0.005; private static final double INTAKE_UPPER_LIMIT = 0.1; public IntakeSensor() { @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index cb9958e6..7ed8c5e5 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 20; // Low b/c belt broke + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 40; // Low b/c belt broke talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 40; // Tuned + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; // Tuned talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index e8c15b5f..34d1cc8d 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -82,7 +82,7 @@ public void configMotors() { configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.apply(configuration); // enabling stator current limits - currentLimits.StatorCurrentLimit = 70; // subject to change + currentLimits.StatorCurrentLimit = 60; // subject to change currentLimits.StatorCurrentLimitEnable = true; currentLimits.SupplyCurrentLimit = 40; // subject to change currentLimits.SupplyCurrentLimitEnable = true; From 67b5733af9a71a7db2b3266c6af310875173bb06 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Wed, 24 Sep 2025 20:15:16 -0700 Subject: [PATCH 10/20] Drive pracitce changes --- src/main/java/frc/robot/Controls.java | 24 ++++++++++++++----- .../java/frc/robot/sensors/IntakeSensor.java | 4 ++-- .../java/frc/robot/subsystems/SpinnyClaw.java | 2 +- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 7380ab27..3fbf747f 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -926,8 +926,8 @@ private void configureSoloControllerBindings() { () -> getSoloDriveY(), () -> getSoloDriveRotate(), soloController.rightBumper()), - getAlgaeIntakeCommand() - .andThen(() -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) .withName("Algae score then intake"); case NO_GAME_PIECE -> Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) @@ -957,9 +957,8 @@ private void configureSoloControllerBindings() { superStructure.algaeProcessorScore( soloController.rightBumper()), Commands.waitSeconds(0.7), - getAlgaeIntakeCommand() - .andThen( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) .withName("Processor score"); case NO_GAME_PIECE -> Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.CORAL) @@ -981,7 +980,7 @@ private void configureSoloControllerBindings() { .leftBumper() .onTrue( superStructure - .quickGroundIntake(soloController.leftBumper()) + .quickGroundIntake(soloController.povUp()) .withName("Quick Gound intake")); // Scoring levels coral and algae intake heights soloController @@ -1028,6 +1027,19 @@ private void configureSoloControllerBindings() { soloController.povLeft().onTrue(s.climbPivotSubsystem.toClimbOut()); // Funnel Climbed soloController.povRight().onTrue(s.climbPivotSubsystem.toClimbed()); + // Intake button + soloController + .povDown() + .onTrue( + superStructure + .coralIntake() + .alongWith( + s.elevatorLEDSubsystem != null + ? s.elevatorLEDSubsystem + .tripleBlink(255, 92, 0, "Orange - Manual Coral Intake") + .asProxy() + : Commands.none()) + .withName("Manual Coral Intake")); // Arm manual soloController .rightStick() diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index ce72b48e..5d77eddc 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -16,7 +16,7 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS private static final double INTAKE_LOWER_LIMIT = 0.005; - private static final double INTAKE_UPPER_LIMIT = 0.1; + private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); diff --git a/src/main/java/frc/robot/subsystems/SpinnyClaw.java b/src/main/java/frc/robot/subsystems/SpinnyClaw.java index 34d1cc8d..e8c15b5f 100644 --- a/src/main/java/frc/robot/subsystems/SpinnyClaw.java +++ b/src/main/java/frc/robot/subsystems/SpinnyClaw.java @@ -82,7 +82,7 @@ public void configMotors() { configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; cfg.apply(configuration); // enabling stator current limits - currentLimits.StatorCurrentLimit = 60; // subject to change + currentLimits.StatorCurrentLimit = 70; // subject to change currentLimits.StatorCurrentLimitEnable = true; currentLimits.SupplyCurrentLimit = 40; // subject to change currentLimits.SupplyCurrentLimitEnable = true; From 94dc7fc5b2293cc41cb7239e6e590f3aa0a5e160 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 25 Sep 2025 01:17:13 -0700 Subject: [PATCH 11/20] Rebump up current limits --- src/main/java/frc/robot/sensors/IntakeSensor.java | 6 +++--- src/main/java/frc/robot/subsystems/ArmPivot.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 5d77eddc..94eedf19 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -15,8 +15,8 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS - private static final double INTAKE_LOWER_LIMIT = 0.005; - private static final double INTAKE_UPPER_LIMIT = 0.05; + private static final double INTAKE_LOWER_LIMIT = 0.01; + private static final double INTAKE_UPPER_LIMIT = 0.1; public IntakeSensor() { intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 7ed8c5e5..19ea07fc 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 40; // Low b/c belt broke + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; // Tuned + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID From 7d21c83f0c73b752957eff4d60d6841e6f940c0c Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 25 Sep 2025 01:28:58 -0700 Subject: [PATCH 12/20] NO_GAME_PIECE mode actually works --- src/main/java/frc/robot/Controls.java | 11 +++++++++++ src/main/java/frc/robot/subsystems/ArmPivot.java | 4 ++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 3fbf747f..c27ce1b4 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -397,6 +397,17 @@ private void configureSuperStructureBindings() { } }) .withName("Set solo scoring mode")); + + sensors + .armSensor + .inClaw() + .and(RobotModeTriggers.teleop()) + .onFalse( + Commands.runOnce( + () -> { + soloScoringMode = SoloScoringMode.NO_GAME_PIECE; + }) + .withName("Clear solo scoring mode")); } if (s.climbPivotSubsystem.sensor != null) { diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java index 19ea07fc..880a9f82 100644 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ b/src/main/java/frc/robot/subsystems/ArmPivot.java @@ -195,9 +195,9 @@ public void factoryDefaults() { talonFXConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake; // enabling current limits - talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; + talonFXConfiguration.CurrentLimits.StatorCurrentLimit = 80; talonFXConfiguration.CurrentLimits.StatorCurrentLimitEnable = true; - talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; + talonFXConfiguration.CurrentLimits.SupplyCurrentLimit = 20; talonFXConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true; // PID From e4a648d82802ed0ea10eb61c057fbbe45a098167 Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Sat, 27 Sep 2025 20:03:48 -0700 Subject: [PATCH 13/20] Drive c --- src/main/java/frc/robot/generated/CompTunerConstants.java | 2 +- src/main/java/frc/robot/sensors/IntakeSensor.java | 6 +++--- src/main/java/frc/robot/subsystems/GroundSpinny.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/generated/CompTunerConstants.java b/src/main/java/frc/robot/generated/CompTunerConstants.java index 978ac85d..f55ad969 100644 --- a/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -55,7 +55,7 @@ public class CompTunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. diff --git a/src/main/java/frc/robot/sensors/IntakeSensor.java b/src/main/java/frc/robot/sensors/IntakeSensor.java index 94eedf19..5d77eddc 100644 --- a/src/main/java/frc/robot/sensors/IntakeSensor.java +++ b/src/main/java/frc/robot/sensors/IntakeSensor.java @@ -15,8 +15,8 @@ public class IntakeSensor { private final LaserCan intakeSensor; // VALUES ARE IN METERS - private static final double INTAKE_LOWER_LIMIT = 0.01; - private static final double INTAKE_UPPER_LIMIT = 0.1; + private static final double INTAKE_LOWER_LIMIT = 0.005; + private static final double INTAKE_UPPER_LIMIT = 0.05; public IntakeSensor() { intakeSensor = new LaserCan(Hardware.GROUND_INTAKE_SENSOR); @@ -29,7 +29,7 @@ public IntakeSensor() { private void ConfigureSensor(LaserCan Sensor) { try { Sensor.setRangingMode(LaserCan.RangingMode.SHORT); - Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16)); + Sensor.setRegionOfInterest(new LaserCan.RegionOfInterest(4, 4, 16, 16)); Sensor.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS); } catch (ConfigurationFailedException e) { System.out.println("Configuration Failed! " + e); diff --git a/src/main/java/frc/robot/subsystems/GroundSpinny.java b/src/main/java/frc/robot/subsystems/GroundSpinny.java index 93ce77e4..96182264 100644 --- a/src/main/java/frc/robot/subsystems/GroundSpinny.java +++ b/src/main/java/frc/robot/subsystems/GroundSpinny.java @@ -16,7 +16,7 @@ public class GroundSpinny extends SubsystemBase { public static final double GROUND_INTAKE_JITTER_SPEED = 1; public static final double FUNNEL_INTAKE_SPEED = -2; public static final double QUICK_HANDOFF_EXTAKE_SPEED = 1; - private static final double STATOR_CURRENT_STALL_THRESHOLD = 20; + private static final double STATOR_CURRENT_STALL_THRESHOLD = 50; // TalonFX private final TalonFX motor; From b2d7d37bc0ec14062b35a25ef974bcb38ca607ac Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Sat, 27 Sep 2025 23:06:54 -0700 Subject: [PATCH 14/20] Code Clean up --- src/main/java/frc/robot/Controls.java | 48 ++++++++++++--------------- 1 file changed, 21 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index c27ce1b4..49079b00 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -40,12 +40,12 @@ import java.util.function.BooleanSupplier; public class Controls { + private static final int SOLO_CONTROLLER_PORT = 0; private static final int DRIVER_CONTROLLER_PORT = 1; private static final int OPERATOR_CONTROLLER_PORT = 2; private static final int ARM_PIVOT_SPINNY_CLAW_CONTROLLER_PORT = 3; private static final int ELEVATOR_CONTROLLER_PORT = 4; private static final int CLIMB_TEST_CONTROLLER_PORT = 5; - private static final int SOLO_CONTROLLER_PORT = 0; private final CommandXboxController driverController; private final CommandXboxController operatorController; @@ -117,7 +117,7 @@ private Trigger connected(CommandXboxController controller) { return new Trigger(() -> controller.isConnected()); } - // takes the X value from the joystic, and applies a deadband and input scaling + // takes the X value from the joystick, and applies a deadband and input scaling private double getDriveX() { // Joystick +Y is back // Robot +X is forward @@ -126,7 +126,7 @@ private double getDriveX() { return input * MaxSpeed * inputScale; } - // takes the Y value from the joystic, and applies a deadband and input scaling + // takes the Y value from the joystick, and applies a deadband and input scaling private double getDriveY() { // Joystick +X is right // Robot +Y is left @@ -135,7 +135,7 @@ private double getDriveY() { return input * MaxSpeed * inputScale; } - // takes the rotation value from the joystic, and applies a deadband and input scaling + // takes the rotation value from the joystick, and applies a deadband and input scaling private double getDriveRotate() { // Joystick +X is right // Robot +angle is CCW (left) @@ -388,12 +388,9 @@ private void configureSuperStructureBindings() { .onTrue( Commands.runOnce( () -> { - if (intakeMode == ScoringMode.CORAL) { - soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; - } else if (intakeMode == ScoringMode.ALGAE) { - soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; - } else { - soloScoringMode = SoloScoringMode.NO_GAME_PIECE; + switch (intakeMode) { + case CORAL -> soloScoringMode = SoloScoringMode.CORAL_IN_CLAW; + case ALGAE -> soloScoringMode = SoloScoringMode.ALGAE_IN_CLAW; } }) .withName("Set solo scoring mode")); @@ -886,38 +883,35 @@ public void vibrateCoDriveController(double vibration) { } } - // Drive for Solo controller - // takes the X value from the joystic, and applies a deadband and input scaling - private double getSoloDriveX() { + private double getJoystickInput(double input) { if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { return 0; // stop driving if either stick is pressed } + // Apply a deadband to the joystick input + double deadbandedInput = MathUtil.applyDeadband(input, 0.1); + return deadbandedInput; + } + + // Drive for Solo controller + // takes the X value from the joystick, and applies a deadband and input scaling + private double getSoloDriveX() { // Joystick +Y is back // Robot +X is forward - double input = MathUtil.applyDeadband(-soloController.getLeftY(), 0.1); - return input * MaxSpeed; + return getJoystickInput(-soloController.getLeftY()) * MaxSpeed; } - // takes the Y value from the joystic, and applies a deadband and input scaling + // takes the Y value from the joystick, and applies a deadband and input scaling private double getSoloDriveY() { - if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { - return 0; // stop driving if either stick is pressed - } // Joystick +X is right // Robot +Y is left - double input = MathUtil.applyDeadband(-soloController.getLeftX(), 0.1); - return input * MaxSpeed; + return getJoystickInput(-soloController.getLeftX()) * MaxSpeed; } - // takes the rotation value from the joystic, and applies a deadband and input scaling + // takes the rotation value from the joystick, and applies a deadband and input scaling private double getSoloDriveRotate() { - if (soloController.leftStick().getAsBoolean() || soloController.rightStick().getAsBoolean()) { - return 0; // stop driving if either stick is pressed - } // Joystick +X is right // Robot +angle is CCW (left) - double input = MathUtil.applyDeadband(-soloController.getRightX(), 0.1); - return input * MaxSpeed; + return getJoystickInput(-soloController.getRightX()) * MaxSpeed; } private void configureSoloControllerBindings() { From a374024e016e8b6d155252da4974321ae48d7fe4 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Mon, 29 Sep 2025 03:57:05 -0700 Subject: [PATCH 15/20] New command that in theory changes the algae intake command based on what is the closest algae --- src/main/java/frc/robot/Controls.java | 7 +- .../subsystems/auto/AutoAlgaeHeights.java | 100 ++++++++++++++++++ 2 files changed, 103 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 49079b00..ad9a0144 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -30,6 +30,7 @@ import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.GroundArm; import frc.robot.subsystems.SuperStructure; +import frc.robot.subsystems.auto.AutoAlgaeHeights; import frc.robot.subsystems.auto.AutoAlign; import frc.robot.subsystems.auto.BargeAlign; import frc.robot.util.AlgaeIntakeHeight; @@ -938,10 +939,8 @@ private void configureSoloControllerBindings() { Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) .alongWith(scoringModeSelectRumble()) .withName("Algae Scoring Mode"), - Commands.runOnce( - () -> - CommandScheduler.getInstance() - .schedule(getAlgaeIntakeCommand())) + AutoAlgaeHeights.autoAlgaeIntakeCommand( + s.drivebaseSubsystem, superStructure) .withName("Run Algae Intake")); })); soloController diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java new file mode 100644 index 00000000..45deda2c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -0,0 +1,100 @@ +package frc.robot.subsystems.auto; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SuperStructure; +import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import java.util.List; + +/** + * Command that automatically moves the elevator to the proper height for the nearest algae based on + * alliance and current robot pose. + */ +public class AutoAlgaeHeights extends Command { + + private final CommandSwerveDrivetrain drivebase; + private final SuperStructure s; + + private static final AprilTagFieldLayout aprilTagFieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + private static final List blueAlgaePoses = + List.of( + aprilTagFieldLayout.getTagPose(17).get().toPose2d(), + aprilTagFieldLayout.getTagPose(19).get().toPose2d(), + aprilTagFieldLayout.getTagPose(21).get().toPose2d(), + aprilTagFieldLayout.getTagPose(18).get().toPose2d(), + aprilTagFieldLayout.getTagPose(20).get().toPose2d(), + aprilTagFieldLayout.getTagPose(22).get().toPose2d()); + + private static final List redAlgaePoses = + List.of( + aprilTagFieldLayout.getTagPose(6).get().toPose2d(), + aprilTagFieldLayout.getTagPose(8).get().toPose2d(), + aprilTagFieldLayout.getTagPose(10).get().toPose2d(), + aprilTagFieldLayout.getTagPose(7).get().toPose2d(), + aprilTagFieldLayout.getTagPose(9).get().toPose2d(), + aprilTagFieldLayout.getTagPose(11).get().toPose2d()); + + public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s) { + this.drivebase = drivebase; + this.s = s; + } + + public static Command autoAlgaeIntakeCommand( + CommandSwerveDrivetrain drivebase, SuperStructure s) { + return new AutoAlgaeHeights(drivebase, s); + } + + private static boolean isBlue() { + return DriverStation.getAlliance() + .map(alliance -> alliance.equals(Alliance.Blue)) + .orElse(false); + } + + private static Pose2d getNearestAlgae(Pose2d robotPose) { + List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + return robotPose.nearest(poses); + } + + private String aprilTagToAlgaeHeight(Pose2d algaePose) { + // Map poses to two heights only + List poses = isBlue() ? blueAlgaePoses : redAlgaePoses; + int index = poses.indexOf(algaePose); + + if (index >= 3) { + return "top"; // upper reef + } else { + return "low"; // lower reef + } + } + + private String activeCommand = ""; + + @Override + public void execute() { + Pose2d robotPose = drivebase.getState().Pose; + Pose2d nearestAlgae = getNearestAlgae(robotPose); + + String targetHeight = aprilTagToAlgaeHeight(nearestAlgae); + + if (!(activeCommand.equals(targetHeight))) { + if (targetHeight.equals("top")) { + s.algaeLevelThreeFourIntake().schedule(); + activeCommand = "top"; + } else { + s.algaeLevelTwoThreeIntake().schedule(); + activeCommand = "low"; + } + } + } + + @Override + public boolean isFinished() { + return false; + } +} From 7f8c42114e194247682e4377bde529c7f0f050f2 Mon Sep 17 00:00:00 2001 From: Tristan <50113330+TrisDooley@users.noreply.github.com> Date: Wed, 1 Oct 2025 19:39:23 -0700 Subject: [PATCH 16/20] Change so auto evelator movements get canceled when you have a game piece --- src/main/java/frc/robot/Controls.java | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index ad9a0144..db89fd64 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -920,29 +920,32 @@ private void configureSoloControllerBindings() { soloController .leftTrigger() .onTrue( - Commands.deferredProxy( - () -> + Commands.runOnce( + () -> { + Command scoreCommand; switch (soloScoringMode) { - case CORAL_IN_CLAW -> getSoloCoralBranchHeightCommand(); - case ALGAE_IN_CLAW -> Commands.sequence( + case CORAL_IN_CLAW -> scoreCommand = getSoloCoralBranchHeightCommand(); + case ALGAE_IN_CLAW -> Command bargeScoreCommand = BargeAlign.bargeScore( s.drivebaseSubsystem, superStructure, () -> getSoloDriveX(), () -> getSoloDriveY(), () -> getSoloDriveRotate(), - soloController.rightBumper()), - Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)) + soloController.rightBumper()) .withName("Algae score then intake"); - case NO_GAME_PIECE -> Commands.parallel( + scoreCommand = Commands.sequence(bargeScoreCommand, Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); + case NO_GAME_PIECE -> scoreCommand = Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) .alongWith(scoringModeSelectRumble()) .withName("Algae Scoring Mode"), AutoAlgaeHeights.autoAlgaeIntakeCommand( - s.drivebaseSubsystem, superStructure) - .withName("Run Algae Intake")); - })); + s.drivebaseSubsystem, superStructure).withTimeout(sensors.armSesnsor.booleanInClaw())); + default -> scoreCommand = Commands.none(); + } + CommandScheduler.getInstance().schedule(scoreCommand); + })); soloController .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) From 9ce98666ec84da587ca1d52db77478ffb3b68b1b Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 2 Oct 2025 00:06:00 -0700 Subject: [PATCH 17/20] oops i did withtimeout instead of until --- src/main/java/frc/robot/Controls.java | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index db89fd64..eb32e3ef 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,15 +1,17 @@ package frc.robot; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -38,7 +40,6 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; -import java.util.function.BooleanSupplier; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; @@ -924,8 +925,8 @@ private void configureSoloControllerBindings() { () -> { Command scoreCommand; switch (soloScoringMode) { - case CORAL_IN_CLAW -> scoreCommand = getSoloCoralBranchHeightCommand(); - case ALGAE_IN_CLAW -> Command bargeScoreCommand = + case CORAL_IN_CLAW -> {scoreCommand = getSoloCoralBranchHeightCommand();} + case ALGAE_IN_CLAW -> {Command bargeScoreCommand = BargeAlign.bargeScore( s.drivebaseSubsystem, superStructure, @@ -935,13 +936,13 @@ private void configureSoloControllerBindings() { soloController.rightBumper()) .withName("Algae score then intake"); scoreCommand = Commands.sequence(bargeScoreCommand, Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); - case NO_GAME_PIECE -> scoreCommand = Commands.parallel( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); } + case NO_GAME_PIECE -> {scoreCommand = Commands.parallel( Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) .alongWith(scoringModeSelectRumble()) .withName("Algae Scoring Mode"), AutoAlgaeHeights.autoAlgaeIntakeCommand( - s.drivebaseSubsystem, superStructure).withTimeout(sensors.armSesnsor.booleanInClaw())); + s.drivebaseSubsystem, superStructure).until(() -> sensors.armSensor.booleanInClaw()));} default -> scoreCommand = Commands.none(); } CommandScheduler.getInstance().schedule(scoreCommand); From 311ce021d0039dc9f7001d82a7094f8030e9f3d6 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 2 Oct 2025 00:07:05 -0700 Subject: [PATCH 18/20] spot --- src/main/java/frc/robot/Controls.java | 45 +++++++++++++++++---------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index eb32e3ef..2464bbe2 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -923,30 +923,41 @@ private void configureSoloControllerBindings() { .onTrue( Commands.runOnce( () -> { - Command scoreCommand; - switch (soloScoringMode) { - case CORAL_IN_CLAW -> {scoreCommand = getSoloCoralBranchHeightCommand();} - case ALGAE_IN_CLAW -> {Command bargeScoreCommand = - BargeAlign.bargeScore( + Command scoreCommand; + switch (soloScoringMode) { + case CORAL_IN_CLAW -> { + scoreCommand = getSoloCoralBranchHeightCommand(); + } + case ALGAE_IN_CLAW -> { + Command bargeScoreCommand = + BargeAlign.bargeScore( s.drivebaseSubsystem, superStructure, () -> getSoloDriveX(), () -> getSoloDriveY(), () -> getSoloDriveRotate(), soloController.rightBumper()) - .withName("Algae score then intake"); - scoreCommand = Commands.sequence(bargeScoreCommand, Commands.runOnce( - () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); } - case NO_GAME_PIECE -> {scoreCommand = Commands.parallel( - Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) - .alongWith(scoringModeSelectRumble()) - .withName("Algae Scoring Mode"), - AutoAlgaeHeights.autoAlgaeIntakeCommand( - s.drivebaseSubsystem, superStructure).until(() -> sensors.armSensor.booleanInClaw()));} - default -> scoreCommand = Commands.none(); + .withName("Algae score then intake"); + scoreCommand = + Commands.sequence( + bargeScoreCommand, + Commands.runOnce( + () -> soloScoringMode = soloScoringMode.NO_GAME_PIECE)); + } + case NO_GAME_PIECE -> { + scoreCommand = + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.ALGAE) + .alongWith(scoringModeSelectRumble()) + .withName("Algae Scoring Mode"), + AutoAlgaeHeights.autoAlgaeIntakeCommand( + s.drivebaseSubsystem, superStructure) + .until(() -> sensors.armSensor.booleanInClaw())); } - CommandScheduler.getInstance().schedule(scoreCommand); - })); + default -> scoreCommand = Commands.none(); + } + CommandScheduler.getInstance().schedule(scoreCommand); + })); soloController .leftTrigger() .and(() -> soloScoringMode == soloScoringMode.CORAL_IN_CLAW) From 49cb5bc10d01636c266202c836ce3e3ccb7312e4 Mon Sep 17 00:00:00 2001 From: Jetblackdragon <64041077+Jetblackdragon@users.noreply.github.com> Date: Thu, 2 Oct 2025 00:08:09 -0700 Subject: [PATCH 19/20] spotspot --- src/main/java/frc/robot/Controls.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index 2464bbe2..d9edf8c5 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -1,17 +1,15 @@ package frc.robot; -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -40,6 +38,7 @@ import frc.robot.util.RobotType; import frc.robot.util.ScoringMode; import frc.robot.util.SoloScoringMode; +import java.util.function.BooleanSupplier; public class Controls { private static final int SOLO_CONTROLLER_PORT = 0; From b91070cc38e32d32a6d9de8c50cbb9bc2dc50f87 Mon Sep 17 00:00:00 2001 From: RobototesProgrammers <194180343+RobototesProgrammers@users.noreply.github.com> Date: Thu, 2 Oct 2025 21:56:09 -0500 Subject: [PATCH 20/20] AutoAlgaeHeights Working --- src/main/java/frc/robot/Controls.java | 9 +++++---- .../frc/robot/subsystems/auto/AutoAlgaeHeights.java | 12 ++++++++---- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java index d9edf8c5..87c9ed8e 100644 --- a/src/main/java/frc/robot/Controls.java +++ b/src/main/java/frc/robot/Controls.java @@ -61,7 +61,7 @@ public class Controls { private BranchHeight branchHeight = BranchHeight.CORAL_LEVEL_FOUR; private ScoringMode scoringMode = ScoringMode.CORAL; - private ScoringMode intakeMode = ScoringMode.CORAL; + public ScoringMode intakeMode = ScoringMode.CORAL; private SoloScoringMode soloScoringMode = SoloScoringMode.NO_GAME_PIECE; private AlgaeIntakeHeight algaeIntakeHeight = AlgaeIntakeHeight.ALGAE_LEVEL_THREE_FOUR; @@ -950,7 +950,7 @@ private void configureSoloControllerBindings() { .alongWith(scoringModeSelectRumble()) .withName("Algae Scoring Mode"), AutoAlgaeHeights.autoAlgaeIntakeCommand( - s.drivebaseSubsystem, superStructure) + s.drivebaseSubsystem, superStructure, this) .until(() -> sensors.armSensor.booleanInClaw())); } default -> scoreCommand = Commands.none(); @@ -997,8 +997,9 @@ private void configureSoloControllerBindings() { soloController .leftBumper() .onTrue( - superStructure - .quickGroundIntake(soloController.povUp()) + Commands.parallel( + Commands.runOnce(() -> intakeMode = ScoringMode.CORAL), + superStructure.quickGroundIntake(soloController.povUp())) .withName("Quick Gound intake")); // Scoring levels coral and algae intake heights soloController diff --git a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java index 45deda2c..f3ee497a 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoAlgaeHeights.java @@ -6,8 +6,10 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Controls; import frc.robot.subsystems.SuperStructure; import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain; +import frc.robot.util.ScoringMode; import java.util.List; /** @@ -18,6 +20,7 @@ public class AutoAlgaeHeights extends Command { private final CommandSwerveDrivetrain drivebase; private final SuperStructure s; + private final Controls c; private static final AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); @@ -40,14 +43,15 @@ public class AutoAlgaeHeights extends Command { aprilTagFieldLayout.getTagPose(9).get().toPose2d(), aprilTagFieldLayout.getTagPose(11).get().toPose2d()); - public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s) { + public AutoAlgaeHeights(CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) { this.drivebase = drivebase; this.s = s; + this.c = c; } public static Command autoAlgaeIntakeCommand( - CommandSwerveDrivetrain drivebase, SuperStructure s) { - return new AutoAlgaeHeights(drivebase, s); + CommandSwerveDrivetrain drivebase, SuperStructure s, Controls c) { + return new AutoAlgaeHeights(drivebase, s, c); } private static boolean isBlue() { @@ -95,6 +99,6 @@ public void execute() { @Override public boolean isFinished() { - return false; + return c.intakeMode != ScoringMode.ALGAE; } }