diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json deleted file mode 100644 index 23e0db9..0000000 --- a/src/main/deploy/pathplanner/navgrid.json +++ /dev/null @@ -1 +0,0 @@ -{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json deleted file mode 100644 index 8e6437a..0000000 --- a/src/main/deploy/pathplanner/settings.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "robotWidth": 0.837, - "robotLength": 0.837, - "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 4.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, - "defaultNominalVoltage": 12.0, - "robotMass": 50.0, - "robotMOI": 4.839, - "robotTrackwidth": 0.546, - "driveWheelRadius": 0.038, - "driveGearing": 5.08, - "maxDriveSpeed": 5.0, - "driveMotorType": "vortex", - "driveCurrentLimit": 40.0, - "wheelCOF": 0.5, - "flModuleX": 0.32, - "flModuleY": 0.32, - "frModuleX": 0.32, - "frModuleY": -0.32, - "blModuleX": -0.32, - "blModuleY": 0.32, - "brModuleX": -0.32, - "brModuleY": -0.32, - "bumperOffsetX": 0.0, - "bumperOffsetY": 0.0, - "robotFeatures": [] -} \ No newline at end of file diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index b9957a5..2ba4d84 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -1,8 +1,12 @@ /* 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.Timer; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; @@ -44,6 +48,12 @@ public class RobotContainer { Camera.CameraType.PHOTONVISION, VisionConstants.RIGHT_CAM_TRANSFORM); + private final Camera centerCam = + new Camera( + "centerCam", + Camera.CameraType.PHOTONVISION, + VisionConstants.CENTER_CAM_TRANSFORM); + private final Odometry odometry = Odometry.getInstance(); // Auto Chooser SendableChooser superSecretMissileTech = new SendableChooser<>(); @@ -62,17 +72,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 */ @@ -83,9 +109,17 @@ 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)); @@ -96,30 +130,51 @@ private void configureBindings() { () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); primaryController - .leftBumper() + .rightBumper() .whileTrue( - new ParallelCommandGroup( - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); + new SequentialCommandGroup( + new ParallelRaceGroup( + 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)), + new RunCommand( + () -> + swerveSubsystem.drive( + ConfigManager.getInstance() + .get("back_mps", -1.0), + 0.0, + 0.0, + false, + false, + false), + swerveSubsystem) + .withTimeout( + ConfigManager.getInstance() + .get("back_time_sec", 0.2)))); 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)); - // secondaryController.leftBumper.onTrue(new InstantCommand(() -> - // coralQueue.stepBackwards())); - // secondaryController.rightBumper.onTrue(new InstantCommand(() -> - // coralQueue.stepForwards())); - secondaryController .a() .whileTrue( @@ -154,7 +209,7 @@ private void configureBindings() { secondaryController .leftBumper() - .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // secondaryController .rightBumper() @@ -173,6 +228,7 @@ private void configureBindings() { public void robotInit() { odometry.addCamera(leftCam); odometry.addCamera(rightCam); + odometry.addCamera(centerCam); } /** Runs every 20ms while the robot is on */ @@ -183,6 +239,8 @@ public void robotPeriodic() { /** Runs ones when enabled in teleop */ public void teleopInit() { + buttonBoardSubsystem.bind(); + if (cqProfiles.getSelected() != null) CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); } @@ -218,6 +276,7 @@ public Command getAutonomousCommand() { private Command getPlaceCommand( Supplier currentSupplier, Supplier nextSupplier) { + return new SequentialCommandGroup( new ParallelRaceGroup( new AlignCommand( @@ -230,19 +289,48 @@ 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) - .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)), + new ParallelRaceGroup( + new AutoEndCommand(), + new BaseCommand(elevatorSubsystem, armSubsystem), + new RunCommand( + () -> + swerveSubsystem.drive( + ConfigManager.getInstance() + .get("back_mps", -1.0), + 0.0, + 0.0, + false, + false, + false), + swerveSubsystem) + .withTimeout( + ConfigManager.getInstance().get("back_time_sec", 0.2)))); } /** @@ -260,24 +348,59 @@ private Command getLocationPlaceCommand(CoralQueue.CoralPosition position) { * * @return The command to goto the feeder */ - private Command getAutoIntakeCommand() { - return new ParallelDeadlineGroup( - 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) - .withTimeout(2)), + 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 ParallelCommandGroup( + new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.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 + } + + private class AutoEndCommand extends Command { + private double currTime = Timer.getFPGATimestamp() * 1000; + + @Override + public void initialize() { + this.currTime = Timer.getFPGATimestamp() * 1000; + } + + @Override + public boolean isFinished() { + return DriverStation.isAutonomous() + && Timer.getFPGATimestamp() * 1000 - this.currTime + > ConfigManager.getInstance().get("auto_place_backup_time_ms", 0.2); + } + } } diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 3108929..10ae692 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -20,8 +20,6 @@ public class AlignCommand extends Command { private static final Logger LOGGER = LogManager.getLogger(); private final SwerveSubsystem swerveSubsystem; - // private final Controller controller; - private final ProfiledPIDController xAxisPid = new ProfiledPIDController( AlignConstants.X_AXIS_P, @@ -43,6 +41,21 @@ public class AlignCommand extends Command { AlignConstants.ROTATION_D, AlignConstants.ROTATION_CONSTRAINTS); + /// Infinite acceleration PIDs + private final ProfiledPIDController xAxisInfPid = + new ProfiledPIDController( + AlignConstants.X_AXIS_P, + AlignConstants.X_AXIS_I, + AlignConstants.X_AXIS_D, + AlignConstants.X_AXIS_CONSTRAINTS); + + private final ProfiledPIDController yAxisInfPid = + new ProfiledPIDController( + AlignConstants.Y_AXIS_P, + AlignConstants.Y_AXIS_I, + AlignConstants.Y_AXIS_D, + AlignConstants.Y_AXIS_CONSTRAINTS); + private final Odometry odometry = Odometry.getInstance(); private final ConfigManager configManager = ConfigManager.getInstance(); @@ -58,6 +71,12 @@ public class AlignCommand extends Command { private double timeSenseFinished = -1; private boolean doUpdate = true; + private double distToTarget = Double.MAX_VALUE; + + private double finalX = 0.0; + private double finalY = 0.0; + private boolean log = true; + /** * Align to a fieldspace position with odometry * @@ -72,12 +91,10 @@ public class AlignCommand extends Command { */ public AlignCommand( SwerveSubsystem swerveSubsystem, - // Controller controller, Supplier poseSupplier, boolean stopWhenFinished, String profile) { this.swerveSubsystem = swerveSubsystem; - // this.controller = controller; this.pose2dSupplier = poseSupplier; this.stopWhenFinished = stopWhenFinished; this.profile = profile; @@ -93,8 +110,41 @@ public AlignCommand( configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); + Pose3d robotPose = Odometry.getInstance().getRobotPose(); + this.rotationPid.enableContinuousInput(-Math.PI, Math.PI); + this.xAxisPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); + this.rotationPid.reset( + robotPose.getRotation().getZ(), + swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); + + this.xAxisInfPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisInfPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); + + this.xAxisPid.setGoal(robotPose.getX()); + this.yAxisPid.setGoal(robotPose.getY()); + this.rotationPid.setGoal(robotPose.getRotation().getZ()); + + this.xAxisInfPid.setGoal(robotPose.getX()); + this.yAxisInfPid.setGoal(robotPose.getY()); + + this.xAxisPid.calculate(robotPose.getX()); + this.yAxisPid.calculate(robotPose.getY()); + this.rotationPid.calculate(robotPose.getRotation().getZ()); + + this.xAxisInfPid.calculate(robotPose.getX()); + this.yAxisInfPid.calculate(robotPose.getY()); + addRequirements(swerveSubsystem); } @@ -103,6 +153,10 @@ public void initialize() { this.targetPos = pose2dSupplier.get(); this.timeSenseFinished = -1; this.doUpdate = true; + this.distToTarget = Double.MAX_VALUE; + this.finalX = 0.0; + this.finalY = 0.0; + this.log = true; LOGGER.info("Initializing AlignCommand"); Pose3d robotPose = odometry.getRobotPose(); @@ -112,14 +166,23 @@ public void initialize() { this.yAxisPid.setP(configManager.get("align_y_axis_p", 3)); this.rotationPid.setP(configManager.get("align_rot_p", 7.3)); + this.xAxisInfPid.setP(configManager.get("align_x_axis_p", 3)); + this.yAxisInfPid.setP(configManager.get("align_y_axis_p", 3)); + this.xAxisPid.setD(configManager.get("align_x_axis_d", .25)); this.yAxisPid.setD(configManager.get("align_y_axis_d", .25)); this.rotationPid.setD(configManager.get("align_rot_d", 0.5)); + this.xAxisInfPid.setD(configManager.get("align_x_axis_d", .25)); + this.yAxisInfPid.setD(configManager.get("align_y_axis_d", .25)); + this.xAxisPid.setI(configManager.get("align_x_axis_i", 0.0)); this.yAxisPid.setI(configManager.get("align_y_axis_i", 0.0)); this.rotationPid.setI(configManager.get("align_rot_i", 0.0)); + this.xAxisInfPid.setI(configManager.get("align_x_axis_i", 0.0)); + this.yAxisInfPid.setI(configManager.get("align_y_axis_i", 0.0)); + this.xAxisPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), @@ -141,6 +204,15 @@ public void initialize() { String.format("align_%s_rot_max_accel_degps", this.profile), 360)))); + this.xAxisInfPid.setConstraints( + new TrapezoidProfile.Constraints( + configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), + 5000)); + this.yAxisInfPid.setConstraints( + new TrapezoidProfile.Constraints( + configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), + 5000)); + this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.yAxisPid.setTolerance( @@ -150,6 +222,12 @@ public void initialize() { configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); + this.xAxisPid.setTolerance( + configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); + this.yAxisPid.setTolerance( + configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); + + // Reset All pids this.xAxisPid.reset( robotPose.getX(), swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); @@ -160,33 +238,45 @@ public void initialize() { robotPose.getRotation().getZ(), swerveSubsystem.getFieldRelativeChassisSpeeds().omegaRadiansPerSecond); - this.xAxisPid.setGoal(robotPose.getX()); - this.yAxisPid.setGoal(robotPose.getY()); - this.rotationPid.setGoal(robotPose.getRotation().getZ()); - - this.xAxisPid.calculate(robotPose.getX()); - this.yAxisPid.calculate(robotPose.getY()); - this.rotationPid.calculate(robotPose.getRotation().getZ()); + this.xAxisInfPid.reset( + robotPose.getX(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); + this.yAxisInfPid.reset( + robotPose.getY(), + swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond); this.xAxisPid.setGoal(targetPos.getX()); this.yAxisPid.setGoal(targetPos.getY()); this.rotationPid.setGoal(targetPos.getRotation().getRadians()); + + this.xAxisInfPid.setGoal(targetPos.getX()); + this.yAxisInfPid.setGoal(targetPos.getY()); } @Override public void execute() { - // Set PIDs to target - Pose3d robotPose = odometry.getRobotPose(); + this.distToTarget = + Math.pow(robotPose.getX() - targetPos.getX(), 2) + + Math.pow(robotPose.getY() - targetPos.getY(), 2); + double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); double rotationPidCalc = this.rotationPid.calculate(robotPose.getRotation().getZ()); + double infX = xAxisInfPid.calculate(robotPose.getX()); + double infY = yAxisInfPid.calculate(robotPose.getY()); + + debug.setEntry("Dist to target (Error)", distToTarget); + debug.setEntry("X Pid Error", this.xAxisPid.getPositionError()); debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); + debug.setEntry("X Pid Error (inf)", this.xAxisInfPid.getPositionError()); + debug.setEntry("Y Pid Error (inf)", this.yAxisInfPid.getPositionError()); + debug.setEntry("X Pid setpoint", this.xAxisPid.atSetpoint()); debug.setEntry("X Pid goal", this.xAxisPid.atGoal()); @@ -198,17 +288,33 @@ public void execute() { debug.setEntry("Robot rotation: ", robotPose.getRotation().getZ()); debug.setEntry("Rot setpoint", this.rotationPid.getSetpoint().position); - double finalX = + debug.setEntry( + "Rot diff", + Math.abs( + Math.abs(this.targetPos.getRotation().getRadians()) + - Math.abs(odometry.getRobotPose().getRotation().getZ()))); + + this.finalX = xAxisCalc + (xAxisPid.atGoal() || xAxisPid.atSetpoint() ? 0 : Math.signum(xAxisCalc) * configManager.get("align_ff", 0.1)); - double finalY = + this.finalY = yAxisCalc + (yAxisPid.atGoal() || yAxisPid.atSetpoint() ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); + if (!stopWhenFinished) { + this.finalX = infX; + this.finalY = infY; + } + + if (log) { + log = false; + LOGGER.info("First commanded speeds: {} {}", xAxisCalc, xAxisCalc); + } + debug.setEntry("Xms", finalX); debug.setEntry("Yms", finalY); debug.setEntry("Rrads", rotationPidCalc); @@ -221,20 +327,10 @@ public void execute() { this.targetPos.getRotation().getRadians() }); - // if (controller.hasStickInput(0.03) || !odometry.hasSeenTarget()) { - // swerveSubsystem.drive( - // controller.getLeftX(), - // controller.getLeftY(), - // controller.getRightX(), - // true, - // true, - // false); - // } else { swerveSubsystem.drive(finalX, finalY, rotationPidCalc, true, true, true); - // } - if (xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal() && this.doUpdate) { - LOGGER.debug("Hit goal, waiting for time to expire"); + if (checkAtGoal() && doUpdate) { + LOGGER.info("Hit goal, waiting for time to expire"); this.timeSenseFinished = Timer.getFPGATimestamp() * 1000; this.doUpdate = false; } @@ -242,9 +338,7 @@ public void execute() { @Override public boolean isFinished() { - return xAxisPid.atGoal() - && yAxisPid.atGoal() - && rotationPid.atGoal() + return checkAtGoal() && Timer.getFPGATimestamp() * 1000 - this.timeSenseFinished > configManager.get( String.format("align_%s_finish_time", this.profile), 200.0); @@ -252,6 +346,21 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - if (stopWhenFinished) swerveSubsystem.drive(0, 0, 0, false, false, false); + if (stopWhenFinished) swerveSubsystem.zeroVoltage(); + else swerveSubsystem.drive(this.finalX, this.finalY, 0.0, true, true, true); + LOGGER.info("Final commanded speeds: {} {}", this.finalX, this.finalY); + } + + private boolean checkAtGoal() { + return (stopWhenFinished && xAxisPid.atGoal() && yAxisPid.atGoal() && rotationPid.atGoal()) + || (!stopWhenFinished + && this.distToTarget + <= Math.pow( + configManager.get( + String.format( + "align_%s_pos_dist_tol", this.profile), + 0.05), + 2) + && rotationPid.atGoal()); } } diff --git a/src/main/java/org/blackknights/commands/BaseCommand.java b/src/main/java/org/blackknights/commands/BaseCommand.java index 1053b0e..b87985e 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,8 +31,10 @@ public void initialize() { @Override public void execute() { - armSubsystem.setPivotAngle(-0.1); - if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) { + armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1)); + if (armSubsystem.getPivotAngle() <= -Math.PI / 4 + || armSubsystem.getPivotAngle() + >= ConfigManager.getInstance().get("arm_movement_max", 0.2)) { elevatorSubsystem.holdPosition(); } else { elevatorSubsystem.zeroElevator(); diff --git a/src/main/java/org/blackknights/commands/ClimberCommand.java b/src/main/java/org/blackknights/commands/ClimberCommand.java index 644d5da..2bf39f5 100644 --- a/src/main/java/org/blackknights/commands/ClimberCommand.java +++ b/src/main/java/org/blackknights/commands/ClimberCommand.java @@ -1,6 +1,7 @@ /* 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; @@ -24,16 +25,17 @@ public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController c @Override public void execute() { - if (controller.povDown().getAsBoolean()) { - climberSubsystem.setClimberSpeed(1); - } else if (controller.povUp().getAsBoolean()) { - climberSubsystem.setClimberSpeed(-1); - } else if (controller.povLeft().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.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 d9114a5..d509ad4 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -1,6 +1,7 @@ /* Black Knights Robotics (C) 2025 */ package org.blackknights.commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; @@ -10,6 +11,8 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; + private double currentTime; + /** * Create a new intake command * @@ -22,6 +25,11 @@ public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { addRequirements(intakeSubsystem); } + @Override + public void initialize() { + this.currentTime = Timer.getFPGATimestamp() * 1000; + } + @Override public void execute() { switch (mode) { @@ -47,7 +55,11 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak(); + return (mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak()) + || (mode.equals(IntakeMode.OUTTAKE) + && !intakeSubsystem.getLinebreak() + && Timer.getFPGATimestamp() * 1000 - this.currentTime + > ConfigManager.getInstance().get("outtake_wait_time_ms", 40)); } /** Enum of the different intake modes */ diff --git a/src/main/java/org/blackknights/constants/ArmConstants.java b/src/main/java/org/blackknights/constants/ArmConstants.java index 773dc80..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 = 1.581 - 0.092; // 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..828ca8e 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import org.blackknights.utils.ConfigManager; /** Vision related constants */ public class VisionConstants { @@ -10,8 +11,35 @@ public class VisionConstants { public static double VISION_TRUST = 0.5; public static final Transform3d LEFT_CAM_TRANSFORM = - new Transform3d(0.235, 0.36, 0.16, new Rotation3d(0.0, 0.0, Math.toRadians(-13))); + new Transform3d( + 0.253, + 0.336, + 0.229, + new Rotation3d( + 0.0, + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("left_cam_angle", -10.0)))); 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.253, + -0.3995, + 0.229, + new Rotation3d( + 0.0, + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("right_cam_angle", 10.0)))); + + public static final Transform3d CENTER_CAM_TRANSFORM = + new Transform3d( + 0.1, // 0.341122 0.3832 + 0.0, + 0.2040382, + new Rotation3d( + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("center_cam_pitch", 45.0)), + 0.0)); } 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/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/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/Camera.java b/src/main/java/org/blackknights/utils/Camera.java index 4ff5c7c..05212c4 100644 --- a/src/main/java/org/blackknights/utils/Camera.java +++ b/src/main/java/org/blackknights/utils/Camera.java @@ -22,10 +22,10 @@ public class Camera { private Optional photonCamera; private Optional photonPoseEstimator; - private Transform3d camOffset; + private final Transform3d camOffset; private double photonTimestamp; private Pose3d targetPose; - private String name; + private final String name; private final CameraType camType; 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/tuning/backups/tuning_2025_03_Fri:21:16:57.json b/tuning/backups/tuning_2025_03_Fri:21:16:57.json new file mode 100755 index 0000000..7faffa1 --- /dev/null +++ b/tuning/backups/tuning_2025_03_Fri:21:16:57.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.1,"driver_max_speed":3.5,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.3,"inf_switch_dist":2.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"left_cam_angle":-12.0,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":4.5,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":5.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":4.0,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.03,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.25,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":4.0,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"driver_max_speed_rot":360,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":65.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.95,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.5,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":200.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.5,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.16,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"right_cam_angle":7.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":1.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":520,"align_rough_x_max_vel_m":5.0} diff --git a/tuning/backups/tuning_2025_03_Mon:20:05:39.json b/tuning/backups/tuning_2025_03_Mon:20:05:39.json new file mode 100755 index 0000000..65d058f --- /dev/null +++ b/tuning/backups/tuning_2025_03_Mon:20:05:39.json @@ -0,0 +1 @@ +{"align_enable_slow_mode":false,"shooter_high_pass":0.0,"align_close_speed_scaler":0.5,"align_close__y_i":0.0,"align_close_distance":1.0,"odom_wheel_trust":0.1,"shooter_amp":0.0,"align_rough_pos_tolerance":0.3,"align_rough_rotation_tolerance":10,"align_fine_x_max_vel_m":3.0,"align_close_y_p":3.1,"arm_tol":1.5,"vision_min_distance":0.5,"Coral_Queue":"10L2,11L1,9L3,1L3,8L3","shooter_low_pass":0.0,"align_close_y_d":0.1,"swerve_turning_p":1.0,"drive_magnitude_slew_rate":22.0,"swerve_turning_i":0.0,"outtake_speed":10,"swerve_turning_d":0.0,"align_fine_y_max_vel_m":3.0,"max_roation_diff":3.0,"arm_base_angle":0.1,"align_rot_max_accel_degps":360.0,"elevator_pid_tolerance":0.05,"align_ff":0.1,"algin_step_one_time":200.0,"align_fine_rot_max_accel_degps":360,"elevator_max_accel":3.0,"align_auto_rot_max_accel_degps":360,"drive_rotational_slew_rate":30.0,"align_auto_x_max_vel_m":2.5,"test_drive_rot":0.0,"align_close_x_i":0.0,"align_rough_y_max_vel_m":4.0,"align_x_max_accel_mps":2.0,"odom_vision_trust":0.5,"vision_cutoff_distance":3.0,"elevator_current_max":20.0,"align_close_x_p":3.1,"align_rough_y_max_accel_mps":3.5,"arm_intake":0.64,"swerve_drive_i":0.0,"swerve_drive_d":0.0,"align_close_x_d":0.1,"align_dist_back":0.72,"swerve_drive_p":0.04,"align_rot_max_vel_deg":360.0,"autointake_first_back":1.2,"align_x_i":0.0,"align_x_p":3.0,"scoring_left_x":0.5,"arm_max_vel_degs":90.0,"scoring_left_y":0.21,"align_fine_pos_tolerance":0.05,"l4_elevator_position":0.0,"align_x_d":0.25,"controller_deadband":0.2,"reef_align_pos_tolerance":0.005,"intake_slow_voltage":-0.6,"align_pos_rough_tol":0.2,"align_x_axis_p":3,"align_y_axis_p":3,"align_target_y":2.922,"align_target_x":12.499,"intake_notein_speed":0.0,"align_rough_x_max_accel_mps":3.5,"align_rot_d":0.5,"align_x_axis_d":0.25,"align_y_axis_d":0.25,"align_x_max_vel_m":3.0,"align_rot_i":0.0,"align_x_axis_i":0.0,"align_slow_more":false,"align_auto_finish_time":200.0,"intake_speed":-8,"align_rot_p":7.3,"odom_wheel_trust_theta":0.08726646259971647,"align_y_axis_i":0.0,"odom_vision_trust_theta":0.08726646259971647,"shooter_speaker":0.0,"align_rough_back":0.5,"align_ff_ks":1.0,"align_y_p":3.0,"align_target_rads":1.02,"intake_shoot_speed":0.0,"align_auto_rotation_tolerance":1,"align_y_i":0.0,"photon_camera_offset":0.4,"elevator_i":0.0,"align_auto_rot_max_vel_deg":360,"elevator_p":25.0,"align_ff_kv":1.0,"elevator_l1":0.15,"elevator_l2":0.34,"arm_max_accel_degs":90.0,"elevator_d":0.0,"elevator_l3":0.875,"elevator_l4":1.64,"l1_elevator_position":1.0,"align_close_rot_d":0.0,"align_fine_y_max_accel_mps":3.0,"align_y_max_accel_mps":2.0,"align_y_d":0.25,"reef_align_rotation_tolerance":1.0,"align_rough_rot_max_vel_deg":360,"align_close_rot_p":3.0,"arm_p":0.82,"align_fine_rot_max_vel_deg":360,"align_auto_pos_tolerance":0.05,"align_close_rot_i":0.0,"arm_i":0.0,"l3_elevator_position":0.0,"elevator_kg":0.4,"arm_d":0.0,"arm_kv":0.0,"align_auto_y_max_accel_mps":2.0,"arm_ks":0.0,"align_fine_finish_time":400.0,"elevator_max_vel":5.0,"elevator_ks":0.3,"align_fine_x_max_accel_mps":3.0,"placeholder":0.0,"arm_kg":0.1,"scoring_right_x":0.5,"scoring_right_y":-0.13,"arm_encoder_offset":1.679,"elevator_intake":0.14,"align_rot_rough_tol":1.0,"arm_ka":0.0,"align_slow_mode":false,"align_fine_max_time":3.0,"test_drive_y":0.0,"test_drive_x":0.1,"align_fine_rotation_tolerance":1,"elevator_zero_voltage":-3.0,"align_y_max_vel_m":3.0,"autointake_dist_back":0.6,"align_auto_y_max_vel_m":2.5,"arm_l4":-0.636,"arm_l3":-0.6,"arm_l2":-0.5,"align_auto_x_max_accel_mps":2.0,"arm_l1":-0.5,"align_rough_finish_time":200.0,"elevator_zeroing_voltage":0.0,"align_rough_rot_max_accel_degps":360,"align_rough_x_max_vel_m":4.0} diff --git a/tuning/fetch_tuning.sh b/tuning/fetch_tuning.sh new file mode 100755 index 0000000..514820f --- /dev/null +++ b/tuning/fetch_tuning.sh @@ -0,0 +1,3 @@ +#!/usr/bin/env bash + +scp admin@10.20.36.2:/home/lvuser/deploy/tuning.json backups/tuning_$(date +%Y_%m_%a:%T).json