diff --git a/build.gradle b/build.gradle index f3dc1fe..634acff 100644 --- a/build.gradle +++ b/build.gradle @@ -14,7 +14,7 @@ repositories { } -def ROBOT_MAIN_CLASS = "frc.robot.Main" +def ROBOT_MAIN_CLASS = "org.blackknights.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 98bb335..c9c5d57 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -1,11 +1,15 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights; +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.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import java.util.function.Supplier; import org.blackknights.commands.*; import org.blackknights.constants.ScoringConstants; @@ -28,8 +32,8 @@ public class RobotContainer { ButtonBoardSubsystem buttonBoardSubsystem = new ButtonBoardSubsystem(buttonBoard); // Controllers - Controller primaryController = new Controller(0); - Controller secondaryController = new Controller(1); + CommandXboxController primaryController = new CommandXboxController(0); + CommandXboxController secondaryController = new CommandXboxController(1); private final NetworkTablesUtils NTTune = NetworkTablesUtils.getTable("debug"); @@ -61,17 +65,33 @@ public RobotContainer() { SmartDashboard.putData(cqProfiles); SmartDashboard.putData("Auto Chooser", superSecretMissileTech); + // Autos superSecretMissileTech.addOption( - "Left", + "LEFT_3", new SequentialCommandGroup( getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4")), - getAutoIntakeCommand(), + getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("8L4")), - getAutoIntakeCommand(), + getAutoIntakeCommand(IntakeSides.LEFT), getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("9L4")))); superSecretMissileTech.addOption( - "One pose", getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("10L4"))); + "RIGHT_3", + new SequentialCommandGroup( + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("3L4")), + getAutoIntakeCommand(IntakeSides.RIGHT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("6L4")), + getAutoIntakeCommand(IntakeSides.RIGHT), + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("7L4")))); + + superSecretMissileTech.addOption( + "CENTER_LEFT", + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("12L4"))); + superSecretMissileTech.addOption( + "CENTER_RIGHT", + getLocationPlaceCommand(CoralQueue.CoralPosition.fromString("1L4"))); + + superSecretMissileTech.addOption("INTAKE_TEST", getAutoIntakeCommand(IntakeSides.RIGHT)); } /** Configure controller bindings */ @@ -82,33 +102,104 @@ private void configureBindings() { swerveSubsystem.setDefaultCommand( new DriveCommands( swerveSubsystem, - () -> primaryController.getLeftY() * 2.5, - () -> primaryController.getLeftX() * 2.5, - () -> -primaryController.getRightX() * Math.PI, + () -> + primaryController.getLeftY() + * ConfigManager.getInstance().get("driver_max_speed", 3.5), + () -> + primaryController.getLeftX() + * ConfigManager.getInstance().get("driver_max_speed", 3.5), + () -> + -primaryController.getRightX() + * Math.toRadians( + ConfigManager.getInstance() + .get("driver_max_speed_rot", 360)), true, true)); - primaryController.rightBumper.whileTrue( - getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); + primaryController + .leftBumper() + .whileTrue( + getPlaceCommand( + () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - primaryController.leftBumper.whileTrue( - new ParallelCommandGroup( - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false))); + primaryController + .rightBumper() + .whileTrue( + new ParallelCommandGroup( + new DriveCommands( + swerveSubsystem, + primaryController::getLeftY, + primaryController::getLeftX, + () -> -primaryController.getRightX() * Math.PI, + true, + true), + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE), + new IntakeCommand( + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); + primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + + secondaryController + .rightStick() + .onTrue(new InstantCommand(ScoringConstants::recomputeCoralPositions)); + + // SECONDARY CONTROLLER + climberSubsystem.setDefaultCommand( new ClimberCommand(climberSubsystem, secondaryController)); - primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + secondaryController + .a() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L1)); + + secondaryController + .b() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L2)); + + secondaryController + .x() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L3)); + + secondaryController + .y() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L4)); + + secondaryController + .leftBumper() + .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // - secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); - secondaryController.rightBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + secondaryController + .rightBumper() + .onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); + + secondaryController + .rightTrigger(0.2) + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); + + secondaryController + .leftTrigger(0.2) + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); } /** Runs once when the code starts */ @@ -125,7 +216,10 @@ public void robotPeriodic() { /** Runs ones when enabled in teleop */ public void teleopInit() { - CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); + buttonBoardSubsystem.bind(); + + if (cqProfiles.getSelected() != null) + CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); } /** @@ -171,22 +265,32 @@ private Command getPlaceCommand( false, "rough"), new BaseCommand(elevatorSubsystem, armSubsystem)), - new ParallelCommandGroup( - new SequentialCommandGroup( - new AlignCommand( + new ParallelRaceGroup( + new AlignCommand( swerveSubsystem, () -> currentSupplier.get().getPose(), true, - "fine"), - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.OUTTAKE, - () -> armSubsystem.hasGamePiece()) - .withTimeout(2)), + "fine") + .withTimeout( + ConfigManager.getInstance() + .get("align_fine_max_time", 3.0)), + new RunCommand( + () -> + intakeSubsystem.setSpeed( + ConfigManager.getInstance() + .get("intake_slow_voltage", -2.0)), + intakeSubsystem), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> nextSupplier.get().getHeight()))); + () -> currentSupplier.get().getHeight())), + new ParallelRaceGroup( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> nextSupplier.get().getHeight()), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) + .withTimeout(1))); } /** @@ -204,27 +308,52 @@ private Command getLocationPlaceCommand(CoralQueue.CoralPosition position) { * * @return The command to goto the feeder */ - private Command getAutoIntakeCommand() { - return new ParallelDeadlineGroup( + private Command getAutoIntakeCommand(IntakeSides side) { + Pose2d intakePose = getPose2d(side); + + Pose2d intakePoseFinal = + intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); + + return new ParallelRaceGroup( new SequentialCommandGroup( new AlignCommand( swerveSubsystem, () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() - == DriverStation.Alliance.Blue - ? ScoringConstants.INTAKE_BLUE - : ScoringConstants.INTAKE_RED, - true, - "rough"), - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.INTAKE, - () -> armSubsystem.hasGamePiece()) - .withTimeout(2)), + AlignUtils.getXDistBack( + intakePoseFinal, + ConfigManager.getInstance() + .get("autointake_first_back", 1.0)), + false, + "auto"), + new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE)); + () -> ScoringConstants.ScoringHeights.INTAKE), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + } + + private static Pose2d getPose2d(IntakeSides side) { + Pose2d intakePose; + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + if (side == IntakeSides.LEFT) { + intakePose = ScoringConstants.INTAKE_BLUE_LEFT; + } else { + intakePose = ScoringConstants.INTAKE_BLUE_RIGHT; + } + } else { + if (side == IntakeSides.LEFT) { + intakePose = ScoringConstants.INTAKE_RED_LEFT; + } else { + intakePose = ScoringConstants.INTAKE_RED_RIGHT; + } + } + return intakePose; + } + + private enum IntakeSides { + LEFT, + RIGHT } } diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 3108929..08eb67b 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -252,6 +252,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - if (stopWhenFinished) swerveSubsystem.drive(0, 0, 0, false, false, false); + if (stopWhenFinished) swerveSubsystem.zeroVoltage(); } } diff --git a/src/main/java/org/blackknights/commands/BaseCommand.java b/src/main/java/org/blackknights/commands/BaseCommand.java index 1053b0e..6c6c191 100644 --- a/src/main/java/org/blackknights/commands/BaseCommand.java +++ b/src/main/java/org/blackknights/commands/BaseCommand.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import org.blackknights.subsystems.ArmSubsystem; import org.blackknights.subsystems.ElevatorSubsystem; +import org.blackknights.utils.ConfigManager; /** Default command to keep the elevator and arm at rest */ public class BaseCommand extends Command { @@ -30,7 +31,7 @@ public void initialize() { @Override public void execute() { - armSubsystem.setPivotAngle(-0.1); + armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1)); if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) { elevatorSubsystem.holdPosition(); } else { diff --git a/src/main/java/org/blackknights/commands/ClimberCommand.java b/src/main/java/org/blackknights/commands/ClimberCommand.java index 06d49cf..2bf39f5 100644 --- a/src/main/java/org/blackknights/commands/ClimberCommand.java +++ b/src/main/java/org/blackknights/commands/ClimberCommand.java @@ -1,22 +1,23 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights.commands; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.blackknights.subsystems.ClimberSubsystem; -import org.blackknights.utils.Controller; /** Climber command to control the climber */ public class ClimberCommand extends Command { public ClimberSubsystem climberSubsystem; - public Controller controller; + public CommandXboxController controller; /** * Command to controller the climber, right now over pure voltage * * @param climberSubsystem The instance of {@link ClimberSubsystem} - * @param controller A {@link Controller} to control the climber + * @param controller A {@link CommandXboxController} to control the climber */ - public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) { + public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController controller) { this.climberSubsystem = climberSubsystem; this.controller = controller; addRequirements(climberSubsystem); @@ -24,16 +25,17 @@ public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) @Override public void execute() { - if (controller.dpadDown.getAsBoolean()) { - climberSubsystem.setClimberSpeed(1); - } else if (controller.dpadUp.getAsBoolean()) { - climberSubsystem.setClimberSpeed(-1); - } else if (controller.dpadLeft.getAsBoolean()) { + if (!MathUtil.isNear(controller.getLeftY(), 0.0, 0.1)) { + climberSubsystem.setClimberSpeed(controller.getLeftY()); + } else { + climberSubsystem.setClimberSpeed(0); + } + + if (controller.povLeft().getAsBoolean()) { climberSubsystem.setLockSpeed(0.5); - } else if (controller.dpadRight.getAsBoolean()) { + } else if (controller.povRight().getAsBoolean()) { climberSubsystem.setLockSpeed(-0.5); } else { - climberSubsystem.setClimberSpeed(0); climberSubsystem.setLockSpeed(0); } } diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index 5f03da4..d9114a5 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -2,7 +2,6 @@ package org.blackknights.commands; import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.BooleanSupplier; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; @@ -10,20 +9,16 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; - private final BooleanSupplier hasGamePiece; /** * Create a new intake command * * @param intakeSubsystem The instance of {@link IntakeSubsystem} * @param mode The intake mode ({@link IntakeMode}) - * @param hasGamePiece A {@link BooleanSupplier} supplying if we currently have a coral */ - public IntakeCommand( - IntakeSubsystem intakeSubsystem, IntakeMode mode, BooleanSupplier hasGamePiece) { + public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { this.intakeSubsystem = intakeSubsystem; this.mode = mode; - this.hasGamePiece = hasGamePiece; addRequirements(intakeSubsystem); } @@ -50,10 +45,10 @@ public void end(boolean interrupted) { intakeSubsystem.setVoltage(0); } - // @Override - // public boolean isFinished() { - // return mode.equals(IntakeMode.INTAKE) && hasGamePiece.getAsBoolean(); - // } + @Override + public boolean isFinished() { + return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak(); + } /** Enum of the different intake modes */ public enum IntakeMode { diff --git a/src/main/java/org/blackknights/constants/ArmConstants.java b/src/main/java/org/blackknights/constants/ArmConstants.java index b02a1d5..8883c10 100644 --- a/src/main/java/org/blackknights/constants/ArmConstants.java +++ b/src/main/java/org/blackknights/constants/ArmConstants.java @@ -14,7 +14,7 @@ public class ArmConstants { public static final TrapezoidProfile.Constraints PIVOT_CONSTRAINTS = new TrapezoidProfile.Constraints(PIVOT_MAX_VELOCITY, PIVOT_MAX_ACCELERATION); - public static final double PIVOT_ENCODER_OFFSET = 5.167; + public static final double PIVOT_ENCODER_OFFSET = 0.0; // 1.581 - 0.092; // 5.167 public static final double PIVOT_KS = 0.0; public static final double PIVOT_KG = 0.0; diff --git a/src/main/java/org/blackknights/constants/ScoringConstants.java b/src/main/java/org/blackknights/constants/ScoringConstants.java index 09a150f..c03bbbe 100644 --- a/src/main/java/org/blackknights/constants/ScoringConstants.java +++ b/src/main/java/org/blackknights/constants/ScoringConstants.java @@ -9,6 +9,7 @@ import java.util.List; import java.util.Map; import org.blackknights.framework.CoralQueue; +import org.blackknights.utils.AlignUtils; import org.blackknights.utils.ConfigManager; /** Scoring related constants */ @@ -16,43 +17,76 @@ public class ScoringConstants { private static final List aprilPoses = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark).getTags(); - public static final Pose2d[] CORAL_POSITIONS = - new Pose2d[] { - getPoseFromTag("right", 10), // R1 - getPoseFromTag("left", 9), // R2 - getPoseFromTag("right", 9), // R3 - getPoseFromTag("left", 8), // R4 - getPoseFromTag("right", 8), // R5 - getPoseFromTag("left", 7), // R6 - getPoseFromTag("right", 7), // R7 - getPoseFromTag("left", 6), // R8 - getPoseFromTag("right", 6), // R9 - getPoseFromTag("left", 11), // R10 - getPoseFromTag("right", 11), // R11 - getPoseFromTag("left", 10), // R12 - getPoseFromTag("right", 21), // B1 - getPoseFromTag("left", 22), // B2 - getPoseFromTag("right", 22), // B3 - getPoseFromTag("left", 17), // B4 - getPoseFromTag("right", 17), // B5 - getPoseFromTag("left", 18), // B6 - getPoseFromTag("right", 18), // B7 - getPoseFromTag("left", 19), // B8 - getPoseFromTag("right", 19), // B9 - getPoseFromTag("left", 20), // B10 - getPoseFromTag("right", 20), // B11 - getPoseFromTag("left", 21), // B12 - }; + public static Pose2d[] CORAL_POSITIONS = new Pose2d[] {}; - public static final Pose2d INTAKE_RED = new Pose2d(); - public static final Pose2d INTAKE_BLUE = new Pose2d(); + public static void recomputeCoralPositions() { + CORAL_POSITIONS = + new Pose2d[] { + getPoseFromTag("right", 10), // R1 + getPoseFromTag("left", 9), // R2 + getPoseFromTag("right", 9), // R3 + getPoseFromTag("left", 8), // R4 + getPoseFromTag("right", 8), // R5 + getPoseFromTag("left", 7), // R6 + getPoseFromTag("right", 7), // R7 + getPoseFromTag("left", 6), // R8 + getPoseFromTag("right", 6), // R9 + getPoseFromTag("left", 11), // R10 + getPoseFromTag("right", 11), // R11 + getPoseFromTag("left", 10), // R12 + getPoseFromTag("right", 21), // B1 + getPoseFromTag("left", 22), // B2 + getPoseFromTag("right", 22), // B3 + getPoseFromTag("left", 17), // B4 + getPoseFromTag("right", 17), // B5 + getPoseFromTag("left", 18), // B6 + getPoseFromTag("right", 18), // B7 + getPoseFromTag("left", 19), // B8 + getPoseFromTag("right", 19), // B9 + getPoseFromTag("left", 20), // B10 + getPoseFromTag("right", 20), // B11 + getPoseFromTag("left", 21) // B12 + }; + } + + public static final Pose2d INTAKE_RED_LEFT = + AlignUtils.getXDistBack( + aprilPoses.get(0).pose.toPose2d(), + ConfigManager.getInstance().get("autointake_dist_back", 0.41)); + + public static final Pose2d INTAKE_RED_RIGHT = + AlignUtils.getXDistBack( + aprilPoses.get(1).pose.toPose2d(), + ConfigManager.getInstance().get("autointake_dist_back", 0.41)); + + public static final Pose2d INTAKE_BLUE_LEFT = + AlignUtils.getXDistBack( + aprilPoses.get(12).pose.toPose2d(), + ConfigManager.getInstance().get("autointake_dist_back", 0.41)); + + public static final Pose2d INTAKE_BLUE_RIGHT = + AlignUtils.getXDistBack( + aprilPoses.get(11).pose.toPose2d(), + ConfigManager.getInstance().get("autointake_dist_back", 0.41)); public static final Map PROFILES = new HashMap<>(); static { + recomputeCoralPositions(); + + PROFILES.put( + "RIGHT", + CoralQueue.CoralQueueProfile.fromString( + "2L4,3L4,4L4,5L4,1L4,5L3,4L3,3L3,2L3,1L3,5L2,4L2,3L2,2L2,1L2,4L1,3L1,2L1")); + PROFILES.put( + "LEFT", + CoralQueue.CoralQueueProfile.fromString( + "8L4,9L4,10L4,11L4,12L4,8L3,9L3,10L3,11L3,12L3,8L2,9L2,10L2,11L2,12L2,8L1,9L1,10L1,11L1,12L1")); + PROFILES.put( - "PROFILE_1", CoralQueue.CoralQueueProfile.fromString("6L4,7L4,8L4,9L4,10L4,11L4")); - PROFILES.put("PROFILE_2", CoralQueue.CoralQueueProfile.fromString("0L3,1L4")); + "ALL_L4", + CoralQueue.CoralQueueProfile.fromString( + "2L4,3L4,4L4,5L4,6L4,7L4,8L4,9L4,10L4,11L4,12L4,1L4")); } /** Different scoring heights */ diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index e20350c..4e94d5c 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -13,5 +13,5 @@ public class VisionConstants { new Transform3d(0.235, 0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(-13))); public static final Transform3d RIGHT_CAM_TRANSFORM = - new Transform3d(0.235, -0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(13))); + new Transform3d(0.235, -0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(10))); } diff --git a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java index 4947959..7ede542 100644 --- a/src/main/java/org/blackknights/controllers/MAXSwerveModule.java +++ b/src/main/java/org/blackknights/controllers/MAXSwerveModule.java @@ -123,4 +123,22 @@ public void reconfigure(SparkFlexConfig drivingConfig, SparkMaxConfig turningCon public void resetEncoders() { drivingEncoder.setPosition(0); } + + /** + * Set the voltage of the turning motor + * + * @param voltage The target voltage + */ + public void setTurningVoltage(double voltage) { + this.turningSpark.setVoltage(voltage); + } + + /** + * Set the voltage of the driving motor + * + * @param voltage The target voltage + */ + public void setDrivingVoltage(double voltage) { + this.drivingSpark.setVoltage(voltage); + } } diff --git a/src/main/java/org/blackknights/framework/Odometry.java b/src/main/java/org/blackknights/framework/Odometry.java index 876dfcc..4a3c5d9 100644 --- a/src/main/java/org/blackknights/framework/Odometry.java +++ b/src/main/java/org/blackknights/framework/Odometry.java @@ -183,7 +183,7 @@ public void periodic() { debug.setEntry(String.format("%s_dist_to_target", c.getName()), dist); if (dist <= ConfigManager.getInstance().get("vision_cutoff_distance", 3) - && dist > 0.1) { + && dist > ConfigManager.getInstance().get("vision_min_distance", 0.5)) { this.hasSeenTarget = true; LOGGER.debug("Added vision measurement from `{}`", c.getName()); diff --git a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java index a5f0c97..51db177 100644 --- a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java @@ -4,18 +4,17 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ArmConstants; import org.blackknights.utils.ConfigManager; +import org.blackknights.utils.NetworkTablesUtils; /** Subsystem for controlling arm */ public class ArmSubsystem extends SubsystemBase { @@ -24,19 +23,7 @@ public class ArmSubsystem extends SubsystemBase { private final AbsoluteEncoder pivotAbsEncoder = pivotMotor.getAbsoluteEncoder(); - private final SparkLimitSwitch armLinebreak; - - private final DoubleEntry armEncoderPos = - NetworkTableInstance.getDefault() - .getTable("Arm") - .getDoubleTopic("EncoderPos") - .getEntry(getPivotAngle()); - - private final DoubleEntry armEncoderVelocity = - NetworkTableInstance.getDefault() - .getTable("Arm") - .getDoubleTopic("EncoderVelocity") - .getEntry(getPivotSpeed()); + private final NetworkTablesUtils NTDebug = NetworkTablesUtils.getTable("debug"); private final ProfiledPIDController pivotPID = new ProfiledPIDController( @@ -82,8 +69,6 @@ public ArmSubsystem() { pivotConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); - - armLinebreak = pivotMotor.getForwardLimitSwitch(); } /** @@ -95,15 +80,6 @@ public void setPivotSpeed(double speed) { pivotMotor.set(speed); } - /** - * Checks if the hand has a game piece - * - * @return returns if the hand has a game piece - */ - public boolean hasGamePiece() { - return armLinebreak.isPressed(); - } - /** * moves arm to a set angle * @@ -115,6 +91,9 @@ public void setPivotAngle(double angle) { double pidValue = pivotPID.calculate(getPivotAngle(), angle); double ffValue = pivotFF.calculate(angle, 0); + NTDebug.setEntry("Arm ff out", ffValue); + NTDebug.setEntry("Arm pid out", ffValue); + setPivotSpeed(pidValue + ffValue); } @@ -128,7 +107,10 @@ public double getPivotAngle() { // ? pivotAbsEncoder.getPosition() - 2 * Math.PI - // ArmConstants.PIVOT_ENCODER_OFFSET // : pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET; - return Math.PI * 2 - pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET; + return Math.PI * 2 + - pivotAbsEncoder.getPosition() + - ConfigManager.getInstance() + .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET); } public double getPivotSpeed() { @@ -136,8 +118,28 @@ public double getPivotSpeed() { } public void periodic() { - armEncoderPos.set(getPivotAngle()); - armEncoderVelocity.set(getPivotSpeed()); + NTDebug.setEntry("Arm Encoder Pos", getPivotAngle()); + NTDebug.setEntry("Arm Encoder Speed", getPivotSpeed()); + + NTDebug.setEntry("Arm PID Error", pivotPID.getPositionError()); + NTDebug.setEntry("Arm PID Setpoint", pivotPID.getGoal().position); + + pivotPID.setConstraints( + new TrapezoidProfile.Constraints( + Math.toRadians( + ConfigManager.getInstance() + .get( + "arm_max_vel_degs", + Math.toDegrees(ArmConstants.PIVOT_MAX_VELOCITY))), + ConfigManager.getInstance() + .get( + "arm_max_accel_degs", + Math.toDegrees(ArmConstants.PIVOT_MAX_ACCELERATION)))); + + pivotPID.setTolerance( + Math.toRadians( + ConfigManager.getInstance().get("arm_tol", ArmConstants.PIVOT_TOLERANCE))); + pivotPID.setP(ConfigManager.getInstance().get("arm_p", ArmConstants.PIVOT_P)); pivotPID.setI(ConfigManager.getInstance().get("arm_i", ArmConstants.PIVOT_I)); pivotPID.setD(ConfigManager.getInstance().get("arm_d", ArmConstants.PIVOT_D)); diff --git a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java index b18cc15..c36c84a 100644 --- a/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ButtonBoardSubsystem.java @@ -16,6 +16,8 @@ public class ButtonBoardSubsystem extends SubsystemBase { private final EventLoop loop = new EventLoop(); private static final Logger LOGGER = LogManager.getLogger(); + private final GenericHID hidDevice; + private Pose2d currentPose = null; private ScoringConstants.ScoringHeights currentHeight = null; @@ -25,6 +27,26 @@ public class ButtonBoardSubsystem extends SubsystemBase { * @param hidDevice A {@link GenericHID} corresponding to the button board */ public ButtonBoardSubsystem(GenericHID hidDevice) { + this.hidDevice = hidDevice; + this.bind(); + } + + @Override + public void periodic() { + loop.poll(); + + // Check if both a height and pose button has been pressed, if so, interrupt the queue then + // reset currentPose and currentHeight + if (this.currentHeight != null && this.currentPose != null) { + coralQueue.interruptQueue( + new CoralQueue.CoralPosition( + "BBInterrupt", this.currentPose, this.currentHeight)); + this.currentPose = null; + this.currentHeight = null; + } + } + + public void bind() { boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue; @@ -60,21 +82,6 @@ public ButtonBoardSubsystem(GenericHID hidDevice) { } } - @Override - public void periodic() { - loop.poll(); - - // Check if both a height and pose button has been pressed, if so, interrupt the queue then - // reset currentPose and currentHeight - if (this.currentHeight != null && this.currentPose != null) { - coralQueue.interruptQueue( - new CoralQueue.CoralPosition( - "BBInterrupt", this.currentPose, this.currentHeight)); - this.currentPose = null; - this.currentHeight = null; - } - } - /** * Get the current pose * diff --git a/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java b/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java index f9a7a92..290c55e 100644 --- a/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java @@ -2,7 +2,10 @@ package org.blackknights.subsystems; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** Subsystem to controller the climber */ @@ -10,6 +13,17 @@ public class ClimberSubsystem extends SubsystemBase { public SparkFlex climberMotor = new SparkFlex(15, SparkFlex.MotorType.kBrushless); public TalonSRX lockMotor = new TalonSRX(16); + public SparkFlexConfig climberConfig = new SparkFlexConfig(); + + public ClimberSubsystem() { + climberConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); + + climberMotor.configure( + climberConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + } + /** * Set the speed of the climber motor * diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java index 9f96d79..553a444 100644 --- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java @@ -12,7 +12,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ElevatorConstants; import org.blackknights.utils.ConfigManager; @@ -32,9 +31,6 @@ public class ElevatorSubsystem extends SubsystemBase { private final RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); // Linebreaks - private final DigitalInput topLineBreak = new DigitalInput(ElevatorConstants.TOP_LINEBREAK_ID); - private final DigitalInput bottomLineBreak = - new DigitalInput(ElevatorConstants.BOTTOM_LINEBREAK_ID); private final ProfiledPIDController elevatorPID = new ProfiledPIDController( @@ -210,10 +206,6 @@ public double getElevatorVelocity() { return encoderAverageVel * ElevatorConstants.ROTATIONS_TO_METERS; } - public boolean getBottomLinebreak() { - return !bottomLineBreak.get(); - } - /** * Check if elevator is at target position * diff --git a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java index 548479a..27a10de 100644 --- a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java @@ -2,6 +2,7 @@ package org.blackknights.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ArmConstants; @@ -9,9 +10,14 @@ public class IntakeSubsystem extends SubsystemBase { private final WPI_TalonSRX motor = new WPI_TalonSRX(ArmConstants.MOTOR_ID); + private final DigitalInput intakeLinebreak = new DigitalInput(0); + /** Create a new intake subsystem */ public IntakeSubsystem() { - motor.setInverted(false); + motor.setInverted(true); + motor.enableCurrentLimit(true); + motor.configContinuousCurrentLimit(20); + motor.configPeakCurrentLimit(0); } /** @@ -31,4 +37,8 @@ public void setSpeed(double speed) { public void setVoltage(double voltage) { motor.setVoltage(voltage); } + + public boolean getLinebreak() { + return !intakeLinebreak.get(); + } } diff --git a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java index d5df103..9d7df57 100644 --- a/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/SwerveSubsystem.java @@ -425,6 +425,21 @@ public void driveRobotRelative(ChassisSpeeds chassisSpeeds) { drive(forward, sideways, rotation, false, false, true); // ratelimit was true, to be tested } + /** Zero the swerve via voltage */ + public void zeroVoltage() { + frontLeft.setDrivingVoltage(0.0); + frontLeft.setTurningVoltage(0.0); + + frontRight.setDrivingVoltage(0.0); + frontRight.setTurningVoltage(0.0); + + rearLeft.setDrivingVoltage(0.0); + rearLeft.setTurningVoltage(0.0); + + rearRight.setDrivingVoltage(0.0); + rearRight.setTurningVoltage(0.0); + } + /** Reset the gyro */ public void zeroGyro() { gyro.reset(); diff --git a/src/main/java/org/blackknights/utils/ConfigManager.java b/src/main/java/org/blackknights/utils/ConfigManager.java index 59a29aa..a7eb4ee 100644 --- a/src/main/java/org/blackknights/utils/ConfigManager.java +++ b/src/main/java/org/blackknights/utils/ConfigManager.java @@ -61,6 +61,7 @@ private ConfigManager(File configFile) { if (configFile.createNewFile() || configFile.length() == 0) { LOGGER.info("Created config file"); this.json = this.getDefault(); + this.saveConfig(); } } catch (IOException e) { diff --git a/src/main/java/org/blackknights/utils/Controller.java b/src/main/java/org/blackknights/utils/Controller.java deleted file mode 100644 index c6b5593..0000000 --- a/src/main/java/org/blackknights/utils/Controller.java +++ /dev/null @@ -1,76 +0,0 @@ -/* Black Knights Robotics (C) 2025 */ -package org.blackknights.utils; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; - -/** A wrapper class for {@link XboxController} to provide easier bindings */ -public class Controller extends XboxController { - - public JoystickButton leftBumper; - public JoystickButton rightBumper; - - public JoystickButton leftTrigger; - public JoystickButton rightTrigger; - - public JoystickButton yButton; - public JoystickButton aButton; - public JoystickButton bButton; - public JoystickButton xButton; - - public JoystickButton startButton; - public JoystickButton selectButton; - - public JoystickButton leftStickButton; - public JoystickButton rightStickButton; - - public POVButton dpadUp; - public POVButton dpadDown; - public POVButton dpadLeft; - public POVButton dpadRight; - - /** - * Create a new instance of controller - * - * @param port The port of the controller as specified on DriverStation - */ - public Controller(int port) { - super(port); - - leftTrigger = new JoystickButton(this, XboxController.Axis.kLeftTrigger.value); - rightTrigger = new JoystickButton(this, XboxController.Axis.kRightTrigger.value); - - leftBumper = new JoystickButton(this, XboxController.Button.kLeftBumper.value); - rightBumper = new JoystickButton(this, XboxController.Button.kRightBumper.value); - - yButton = new JoystickButton(this, XboxController.Button.kY.value); - aButton = new JoystickButton(this, XboxController.Button.kA.value); - bButton = new JoystickButton(this, XboxController.Button.kB.value); - xButton = new JoystickButton(this, XboxController.Button.kX.value); - - startButton = new JoystickButton(this, XboxController.Button.kStart.value); - selectButton = new JoystickButton(this, XboxController.Button.kBack.value); - - leftStickButton = new JoystickButton(this, XboxController.Button.kLeftStick.value); - rightStickButton = new JoystickButton(this, XboxController.Button.kRightStick.value); - - dpadUp = new POVButton(this, 0); - dpadDown = new POVButton(this, 180); - dpadLeft = new POVButton(this, 270); - dpadRight = new POVButton(this, 90); - } - - /** - * Check if the controller has input from the sticks - * - * @param deadzone Controller deadzone - * @return If the controller has input - */ - public boolean hasStickInput(double deadzone) { - return Math.abs(getLeftX()) > deadzone - || Math.abs(getLeftY()) > deadzone - || Math.abs(getRightX()) > deadzone - || Math.abs(getRightY()) > deadzone; - } -}