From 4fe996a7e8fab8c326f447386f12fbc8084ceef0 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 8 Mar 2025 17:08:46 -0700 Subject: [PATCH 1/8] misc: All code after Arkansas --- .../java/org/blackknights/RobotContainer.java | 134 +++++++++++++----- .../blackknights/commands/AlignCommand.java | 2 +- .../blackknights/commands/BaseCommand.java | 3 +- .../blackknights/commands/ClimberCommand.java | 14 +- .../blackknights/constants/ArmConstants.java | 2 +- .../constants/ScoringConstants.java | 96 +++++++++---- .../constants/VisionConstants.java | 2 +- .../controllers/MAXSwerveModule.java | 18 +++ .../org/blackknights/framework/Odometry.java | 2 +- .../subsystems/ButtonBoardSubsystem.java | 37 +++-- .../subsystems/ClimberSubsystem.java | 14 ++ .../subsystems/SwerveSubsystem.java | 15 ++ .../org/blackknights/utils/ConfigManager.java | 1 + 13 files changed, 250 insertions(+), 90 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index b9957a5..f797470 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -1,6 +1,9 @@ /* 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; @@ -62,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 */ @@ -83,9 +102,9 @@ 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,9 +115,16 @@ private void configureBindings() { () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); primaryController - .leftBumper() + .rightBumper() .whileTrue( new ParallelCommandGroup( + new DriveCommands( + swerveSubsystem, + primaryController::getLeftY, + primaryController::getLeftX, + () -> -primaryController.getRightX() * Math.PI, + true, + true), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, @@ -110,16 +136,15 @@ private void configureBindings() { 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 +179,7 @@ private void configureBindings() { secondaryController .leftBumper() - .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); // secondaryController .rightBumper() @@ -183,6 +208,8 @@ public void robotPeriodic() { /** Runs ones when enabled in teleop */ public void teleopInit() { + buttonBoardSubsystem.bind(); + if (cqProfiles.getSelected() != null) CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); } @@ -230,19 +257,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) - .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, + () -> currentSupplier.get().getHeight())), + new ParallelRaceGroup( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> nextSupplier.get().getHeight()))); + () -> nextSupplier.get().getHeight()), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) + .withTimeout(1))); } /** @@ -260,24 +300,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) - .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 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/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..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/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/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) { From 6b1e93a7169469898d68016814b3836d7312e79c Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 8 Mar 2025 17:10:52 -0700 Subject: [PATCH 2/8] misc: lint --- src/main/java/org/blackknights/RobotContainer.java | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index f797470..c9c5d57 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -102,9 +102,17 @@ private void configureBindings() { swerveSubsystem.setDefaultCommand( new DriveCommands( swerveSubsystem, - () -> 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)), + () -> + 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)); From b7a2b8a3a565447694f3d2aa597a1e8d5ca82b61 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Mon, 10 Mar 2025 20:01:01 -0600 Subject: [PATCH 3/8] align: Add dual PIDs --- .../blackknights/commands/AlignCommand.java | 83 +++++++++++++++---- 1 file changed, 67 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 08eb67b..55070d2 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(); @@ -72,12 +85,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; @@ -112,14 +123,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 +161,16 @@ 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), + Double.MAX_VALUE)); + this.yAxisInfPid.setConstraints( + new TrapezoidProfile.Constraints( + configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), + Double.MAX_VALUE)); + + this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.yAxisPid.setTolerance( @@ -150,6 +180,10 @@ public void initialize() { configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); + this.xAxisInfPid.setTolerance(0.0); + this.yAxisInfPid.setTolerance(0.0); + + // Reset All pids this.xAxisPid.reset( robotPose.getX(), swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond); @@ -160,29 +194,51 @@ public void initialize() { 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()); + 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(); + double distToTarget = Math.sqrt( + Math.pow(xAxisPid.getPositionError(), 2) + Math.pow(yAxisPid.getPositionError(), 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("X Pid Error", this.xAxisPid.getPositionError()); debug.setEntry("Y Pid Error", this.yAxisPid.getPositionError()); debug.setEntry("Rot Pid Error", this.rotationPid.getPositionError()); @@ -209,6 +265,11 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); + if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) && !stopWhenFinished) { + finalX = infX; + finalY = infY; + } + debug.setEntry("Xms", finalX); debug.setEntry("Yms", finalY); debug.setEntry("Rrads", rotationPidCalc); @@ -221,17 +282,7 @@ 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"); From 0ad042cd66adb8a72e292e0800c2129f126433b6 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Mon, 10 Mar 2025 20:08:18 -0600 Subject: [PATCH 4/8] misc: Add tuning backup --- tuning/backups/tuning_2025_03_Mon:20:05:39.json | 1 + tuning/fetch_tuning.sh | 3 +++ 2 files changed, 4 insertions(+) create mode 100755 tuning/backups/tuning_2025_03_Mon:20:05:39.json create mode 100755 tuning/fetch_tuning.sh 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 From e02087a27971fe644a444d206c0f8ef80ca09d1b Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Thu, 13 Mar 2025 17:30:52 -0600 Subject: [PATCH 5/8] vision: tune offsets --- .../blackknights/commands/AlignCommand.java | 17 ++++++++------- .../constants/VisionConstants.java | 21 +++++++++++++++++-- .../java/org/blackknights/utils/Camera.java | 4 ++-- 3 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 55070d2..89cda98 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -170,7 +170,6 @@ public void initialize() { configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), Double.MAX_VALUE)); - this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); this.yAxisPid.setTolerance( @@ -227,9 +226,10 @@ public void initialize() { public void execute() { Pose3d robotPose = odometry.getRobotPose(); - double distToTarget = Math.sqrt( - Math.pow(xAxisPid.getPositionError(), 2) + Math.pow(yAxisPid.getPositionError(), 2) - ); + double distToTarget = + Math.sqrt( + Math.pow(xAxisPid.getPositionError(), 2) + + Math.pow(yAxisPid.getPositionError(), 2)); double xAxisCalc = this.xAxisPid.calculate(robotPose.getX()); double yAxisCalc = this.yAxisPid.calculate(robotPose.getY()); @@ -238,6 +238,7 @@ public void execute() { 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()); @@ -265,9 +266,11 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) && !stopWhenFinished) { - finalX = infX; - finalY = infY; + if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) + && !stopWhenFinished) { + LOGGER.info("Using inf pid"); + finalX = infX; + finalY = infY; } debug.setEntry("Xms", finalX); diff --git a/src/main/java/org/blackknights/constants/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index 4e94d5c..204e7ef 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,24 @@ 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(10))); + new Transform3d( + 0.253, + -0.3995, + 0.229, + new Rotation3d( + 0.0, + 0.0, + Math.toRadians( + ConfigManager.getInstance().get("right_cam_angle", 10.0)))); } 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; From b905e814f950d3fdeb6e05939c54778065b21908 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 15 Mar 2025 12:44:56 -0600 Subject: [PATCH 6/8] misc: random amp stuff idk --- src/main/deploy/pathplanner/navgrid.json | 1 - src/main/deploy/pathplanner/settings.json | 32 ------------------- .../blackknights/commands/AlignCommand.java | 11 ++++--- .../backups/tuning_2025_03_Fri:21:16:57.json | 1 + 4 files changed, 7 insertions(+), 38 deletions(-) delete mode 100644 src/main/deploy/pathplanner/navgrid.json delete mode 100644 src/main/deploy/pathplanner/settings.json create mode 100755 tuning/backups/tuning_2025_03_Fri:21:16:57.json 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/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index 89cda98..eda61b5 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -227,9 +227,8 @@ public void execute() { Pose3d robotPose = odometry.getRobotPose(); double distToTarget = - Math.sqrt( - Math.pow(xAxisPid.getPositionError(), 2) - + Math.pow(yAxisPid.getPositionError(), 2)); + 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()); @@ -244,6 +243,9 @@ public void execute() { 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()); @@ -266,9 +268,8 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < ConfigManager.getInstance().get("inf_switch_dist", 1.0) + if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) && !stopWhenFinished) { - LOGGER.info("Using inf pid"); finalX = infX; finalY = infY; } 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} From a7a9edecb7a018a685a26557f5f489fe1bc719dc Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 15 Mar 2025 17:52:34 -0600 Subject: [PATCH 7/8] align: Alignment tuning --- .../blackknights/commands/AlignCommand.java | 64 +++++++++++++++---- 1 file changed, 50 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/blackknights/commands/AlignCommand.java b/src/main/java/org/blackknights/commands/AlignCommand.java index eda61b5..7b44847 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -71,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 * @@ -114,6 +120,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(); @@ -164,11 +174,11 @@ public void initialize() { this.xAxisInfPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_x_max_vel_m", this.profile), 3.0), - Double.MAX_VALUE)); + 5000)); this.yAxisInfPid.setConstraints( new TrapezoidProfile.Constraints( configManager.get(String.format("align_%s_y_max_vel_m", this.profile), 3.0), - Double.MAX_VALUE)); + 5000)); this.xAxisPid.setTolerance( configManager.get(String.format("align_%s_pos_tolerance", this.profile), 0.05)); @@ -179,8 +189,10 @@ public void initialize() { configManager.get( String.format("align_%s_rotation_tolerance", this.profile), 1))); - this.xAxisInfPid.setTolerance(0.0); - this.yAxisInfPid.setTolerance(0.0); + 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( @@ -226,7 +238,7 @@ public void initialize() { public void execute() { Pose3d robotPose = odometry.getRobotPose(); - double distToTarget = + this.distToTarget = Math.pow(robotPose.getX() - targetPos.getX(), 2) + Math.pow(robotPose.getY() - targetPos.getY(), 2); @@ -257,12 +269,18 @@ 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 @@ -270,8 +288,13 @@ public void execute() { if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) && !stopWhenFinished) { - finalX = infX; - finalY = infY; + this.finalX = infX; + this.finalY = infY; + } + + if (log) { + log = false; + LOGGER.info("First commanded speeds: {} {}", xAxisCalc, xAxisCalc); } debug.setEntry("Xms", finalX); @@ -288,8 +311,8 @@ public void execute() { 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; } @@ -297,9 +320,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); @@ -308,5 +329,20 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { 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()); } } From 04578648c19b72bdc753234b5fc06f4952ffd37f Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sun, 16 Mar 2025 16:31:46 -0600 Subject: [PATCH 8/8] misc: All code after 1619 --- .../java/org/blackknights/RobotContainer.java | 103 +++++++++++++----- .../blackknights/commands/AlignCommand.java | 50 ++++++--- .../blackknights/commands/BaseCommand.java | 4 +- .../blackknights/commands/IntakeCommand.java | 14 ++- .../constants/VisionConstants.java | 11 ++ 5 files changed, 136 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index c9c5d57..2ba4d84 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -6,6 +6,7 @@ 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.*; @@ -47,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<>(); @@ -125,20 +132,35 @@ private void configureBindings() { 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))); + 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)); @@ -206,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 */ @@ -253,6 +276,7 @@ public Command getAutonomousCommand() { private Command getPlaceCommand( Supplier currentSupplier, Supplier nextSupplier) { + return new SequentialCommandGroup( new ParallelRaceGroup( new AlignCommand( @@ -290,7 +314,23 @@ private Command getPlaceCommand( armSubsystem, () -> nextSupplier.get().getHeight()), new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) - .withTimeout(1))); + .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)))); } /** @@ -315,22 +355,13 @@ private Command getAutoIntakeCommand(IntakeSides side) { intakePose.plus(new Transform2d(0, 0, Rotation2d.fromRadians(Math.PI))); return new ParallelRaceGroup( - new SequentialCommandGroup( - new AlignCommand( - swerveSubsystem, - () -> - AlignUtils.getXDistBack( - intakePoseFinal, - ConfigManager.getInstance() - .get("autointake_first_back", 1.0)), - false, - "auto"), - new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "fine")), + new ParallelCommandGroup( + new AlignCommand(swerveSubsystem, () -> intakePoseFinal, true, "rough"), + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)), new ElevatorArmCommand( elevatorSubsystem, armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + () -> ScoringConstants.ScoringHeights.INTAKE)); } private static Pose2d getPose2d(IntakeSides side) { @@ -356,4 +387,20 @@ 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 7b44847..10ae692 100644 --- a/src/main/java/org/blackknights/commands/AlignCommand.java +++ b/src/main/java/org/blackknights/commands/AlignCommand.java @@ -110,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); } @@ -212,20 +245,6 @@ public void initialize() { 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()); - this.xAxisPid.setGoal(targetPos.getX()); this.yAxisPid.setGoal(targetPos.getY()); this.rotationPid.setGoal(targetPos.getRotation().getRadians()); @@ -286,8 +305,7 @@ public void execute() { ? 0 : Math.signum(yAxisCalc) * configManager.get("align_ff", 0.1)); - if (distToTarget < Math.pow(ConfigManager.getInstance().get("inf_switch_dist", 1.0), 2) - && !stopWhenFinished) { + if (!stopWhenFinished) { this.finalX = infX; this.finalY = infY; } diff --git a/src/main/java/org/blackknights/commands/BaseCommand.java b/src/main/java/org/blackknights/commands/BaseCommand.java index 6c6c191..b87985e 100644 --- a/src/main/java/org/blackknights/commands/BaseCommand.java +++ b/src/main/java/org/blackknights/commands/BaseCommand.java @@ -32,7 +32,9 @@ public void initialize() { @Override public void execute() { armSubsystem.setPivotAngle(ConfigManager.getInstance().get("arm_base_angle", 0.1)); - if (armSubsystem.getPivotAngle() <= -Math.PI / 4 || armSubsystem.getPivotAngle() >= 0.2) { + 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/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/VisionConstants.java b/src/main/java/org/blackknights/constants/VisionConstants.java index 204e7ef..828ca8e 100644 --- a/src/main/java/org/blackknights/constants/VisionConstants.java +++ b/src/main/java/org/blackknights/constants/VisionConstants.java @@ -31,4 +31,15 @@ public class VisionConstants { 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)); }