From 286c98ea53a9fc5f455e0577f68917bf060e491e Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Tue, 4 Mar 2025 00:02:17 -0700 Subject: [PATCH 01/13] arm tuning, intake current limits --- build.gradle | 2 +- .../java/org/blackknights/RobotContainer.java | 34 +++++++++++++++++-- .../blackknights/constants/ArmConstants.java | 2 +- .../subsystems/IntakeSubsystem.java | 3 ++ 4 files changed, 36 insertions(+), 5 deletions(-) diff --git a/build.gradle b/build.gradle index f3dc1fe..634acff 100644 --- a/build.gradle +++ b/build.gradle @@ -14,7 +14,7 @@ repositories { } -def ROBOT_MAIN_CLASS = "frc.robot.Main" +def ROBOT_MAIN_CLASS = "org.blackknights.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 98bb335..f9e71f3 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -102,13 +102,41 @@ private void configureBindings() { elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); + primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + + // SECONDARY CONTROLLER + climberSubsystem.setDefaultCommand( new ClimberCommand(climberSubsystem, secondaryController)); - primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + // secondaryController.leftBumper.onTrue(new InstantCommand(() -> + // coralQueue.stepBackwards())); + // secondaryController.rightBumper.onTrue(new InstantCommand(() -> + // coralQueue.stepForwards())); + + secondaryController.aButton.whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE)); + + secondaryController.bButton.whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L2)); + + secondaryController.xButton.whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L3)); + + secondaryController.yButton.whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); + + secondaryController.leftBumper.whileTrue( + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false)); - secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepBackwards())); - secondaryController.rightBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + secondaryController.rightBumper.whileTrue( + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE, () -> false)); } /** Runs once when the code starts */ diff --git a/src/main/java/org/blackknights/constants/ArmConstants.java b/src/main/java/org/blackknights/constants/ArmConstants.java index b02a1d5..0b1d02f 100644 --- a/src/main/java/org/blackknights/constants/ArmConstants.java +++ b/src/main/java/org/blackknights/constants/ArmConstants.java @@ -14,7 +14,7 @@ public class ArmConstants { public static final TrapezoidProfile.Constraints PIVOT_CONSTRAINTS = new TrapezoidProfile.Constraints(PIVOT_MAX_VELOCITY, PIVOT_MAX_ACCELERATION); - public static final double PIVOT_ENCODER_OFFSET = 5.167; + public static final double PIVOT_ENCODER_OFFSET = 1.581; // 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/subsystems/IntakeSubsystem.java b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java index 548479a..eab9b27 100644 --- a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java @@ -12,6 +12,9 @@ public class IntakeSubsystem extends SubsystemBase { /** Create a new intake subsystem */ public IntakeSubsystem() { motor.setInverted(false); + motor.enableCurrentLimit(true); + motor.configContinuousCurrentLimit(20); + motor.configPeakCurrentLimit(0); } /** From cf928f41a4cec63e0c704fbe73c4de8563404169 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Tue, 4 Mar 2025 19:27:38 -0700 Subject: [PATCH 02/13] all: final changes Co-authored-by: aaditsangvikar --- .../java/org/blackknights/RobotContainer.java | 35 +++++------ .../blackknights/commands/IntakeCommand.java | 15 ++--- .../blackknights/constants/ArmConstants.java | 2 +- .../blackknights/subsystems/ArmSubsystem.java | 62 ++++++++++--------- .../subsystems/ElevatorSubsystem.java | 8 --- .../subsystems/IntakeSubsystem.java | 9 ++- 6 files changed, 62 insertions(+), 69 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index f9e71f3..bd0fb54 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -88,17 +88,16 @@ private void configureBindings() { true, true)); - primaryController.rightBumper.whileTrue( + primaryController.leftBumper.whileTrue( getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - primaryController.leftBumper.whileTrue( + primaryController.rightBumper.whileTrue( new ParallelCommandGroup( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand( - intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false))); + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); @@ -116,9 +115,7 @@ private void configureBindings() { secondaryController.aButton.whileTrue( new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE)); + elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1)); secondaryController.bButton.whileTrue( new ElevatorArmCommand( @@ -132,11 +129,16 @@ private void configureBindings() { new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); - secondaryController.leftBumper.whileTrue( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE, () -> false)); + secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); secondaryController.rightBumper.whileTrue( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE, () -> false)); + new InstantCommand(() -> coralQueue.stepBackwards())); + + // secondaryController.rightTrigger.whileTrue( + // new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); + // + // secondaryController.leftTrigger.whileTrue( + // new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); } /** Runs once when the code starts */ @@ -153,7 +155,8 @@ public void robotPeriodic() { /** Runs ones when enabled in teleop */ public void teleopInit() { - CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); + if (cqProfiles.getSelected() != null) + CoralQueue.getInstance().loadProfile(cqProfiles.getSelected()); } /** @@ -206,10 +209,7 @@ private Command getPlaceCommand( () -> currentSupplier.get().getPose(), true, "fine"), - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.OUTTAKE, - () -> armSubsystem.hasGamePiece()) + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE) .withTimeout(2)), new ElevatorArmCommand( elevatorSubsystem, @@ -245,10 +245,7 @@ private Command getAutoIntakeCommand() { : ScoringConstants.INTAKE_RED, true, "rough"), - new IntakeCommand( - intakeSubsystem, - IntakeCommand.IntakeMode.INTAKE, - () -> armSubsystem.hasGamePiece()) + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE) .withTimeout(2)), new ElevatorArmCommand( elevatorSubsystem, diff --git a/src/main/java/org/blackknights/commands/IntakeCommand.java b/src/main/java/org/blackknights/commands/IntakeCommand.java index 5f03da4..d9114a5 100644 --- a/src/main/java/org/blackknights/commands/IntakeCommand.java +++ b/src/main/java/org/blackknights/commands/IntakeCommand.java @@ -2,7 +2,6 @@ package org.blackknights.commands; import edu.wpi.first.wpilibj2.command.Command; -import java.util.function.BooleanSupplier; import org.blackknights.subsystems.IntakeSubsystem; import org.blackknights.utils.ConfigManager; @@ -10,20 +9,16 @@ public class IntakeCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final IntakeMode mode; - private final BooleanSupplier hasGamePiece; /** * Create a new intake command * * @param intakeSubsystem The instance of {@link IntakeSubsystem} * @param mode The intake mode ({@link IntakeMode}) - * @param hasGamePiece A {@link BooleanSupplier} supplying if we currently have a coral */ - public IntakeCommand( - IntakeSubsystem intakeSubsystem, IntakeMode mode, BooleanSupplier hasGamePiece) { + public IntakeCommand(IntakeSubsystem intakeSubsystem, IntakeMode mode) { this.intakeSubsystem = intakeSubsystem; this.mode = mode; - this.hasGamePiece = hasGamePiece; addRequirements(intakeSubsystem); } @@ -50,10 +45,10 @@ public void end(boolean interrupted) { intakeSubsystem.setVoltage(0); } - // @Override - // public boolean isFinished() { - // return mode.equals(IntakeMode.INTAKE) && hasGamePiece.getAsBoolean(); - // } + @Override + public boolean isFinished() { + return mode.equals(IntakeMode.INTAKE) && intakeSubsystem.getLinebreak(); + } /** Enum of the different intake modes */ public enum IntakeMode { diff --git a/src/main/java/org/blackknights/constants/ArmConstants.java b/src/main/java/org/blackknights/constants/ArmConstants.java index 0b1d02f..773dc80 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; // 5.167 + public static final double PIVOT_ENCODER_OFFSET = 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/subsystems/ArmSubsystem.java b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java index a5f0c97..51db177 100644 --- a/src/main/java/org/blackknights/subsystems/ArmSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ArmSubsystem.java @@ -4,18 +4,17 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkLimitSwitch; import com.revrobotics.spark.SparkLowLevel; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkFlexConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ArmConstants; import org.blackknights.utils.ConfigManager; +import org.blackknights.utils.NetworkTablesUtils; /** Subsystem for controlling arm */ public class ArmSubsystem extends SubsystemBase { @@ -24,19 +23,7 @@ public class ArmSubsystem extends SubsystemBase { private final AbsoluteEncoder pivotAbsEncoder = pivotMotor.getAbsoluteEncoder(); - private final SparkLimitSwitch armLinebreak; - - private final DoubleEntry armEncoderPos = - NetworkTableInstance.getDefault() - .getTable("Arm") - .getDoubleTopic("EncoderPos") - .getEntry(getPivotAngle()); - - private final DoubleEntry armEncoderVelocity = - NetworkTableInstance.getDefault() - .getTable("Arm") - .getDoubleTopic("EncoderVelocity") - .getEntry(getPivotSpeed()); + private final NetworkTablesUtils NTDebug = NetworkTablesUtils.getTable("debug"); private final ProfiledPIDController pivotPID = new ProfiledPIDController( @@ -82,8 +69,6 @@ public ArmSubsystem() { pivotConfig, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kPersistParameters); - - armLinebreak = pivotMotor.getForwardLimitSwitch(); } /** @@ -95,15 +80,6 @@ public void setPivotSpeed(double speed) { pivotMotor.set(speed); } - /** - * Checks if the hand has a game piece - * - * @return returns if the hand has a game piece - */ - public boolean hasGamePiece() { - return armLinebreak.isPressed(); - } - /** * moves arm to a set angle * @@ -115,6 +91,9 @@ public void setPivotAngle(double angle) { double pidValue = pivotPID.calculate(getPivotAngle(), angle); double ffValue = pivotFF.calculate(angle, 0); + NTDebug.setEntry("Arm ff out", ffValue); + NTDebug.setEntry("Arm pid out", ffValue); + setPivotSpeed(pidValue + ffValue); } @@ -128,7 +107,10 @@ public double getPivotAngle() { // ? pivotAbsEncoder.getPosition() - 2 * Math.PI - // ArmConstants.PIVOT_ENCODER_OFFSET // : pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET; - return Math.PI * 2 - pivotAbsEncoder.getPosition() - ArmConstants.PIVOT_ENCODER_OFFSET; + return Math.PI * 2 + - pivotAbsEncoder.getPosition() + - ConfigManager.getInstance() + .get("arm_encoder_offset", ArmConstants.PIVOT_ENCODER_OFFSET); } public double getPivotSpeed() { @@ -136,8 +118,28 @@ public double getPivotSpeed() { } public void periodic() { - armEncoderPos.set(getPivotAngle()); - armEncoderVelocity.set(getPivotSpeed()); + NTDebug.setEntry("Arm Encoder Pos", getPivotAngle()); + NTDebug.setEntry("Arm Encoder Speed", getPivotSpeed()); + + NTDebug.setEntry("Arm PID Error", pivotPID.getPositionError()); + NTDebug.setEntry("Arm PID Setpoint", pivotPID.getGoal().position); + + pivotPID.setConstraints( + new TrapezoidProfile.Constraints( + Math.toRadians( + ConfigManager.getInstance() + .get( + "arm_max_vel_degs", + Math.toDegrees(ArmConstants.PIVOT_MAX_VELOCITY))), + ConfigManager.getInstance() + .get( + "arm_max_accel_degs", + Math.toDegrees(ArmConstants.PIVOT_MAX_ACCELERATION)))); + + pivotPID.setTolerance( + Math.toRadians( + ConfigManager.getInstance().get("arm_tol", ArmConstants.PIVOT_TOLERANCE))); + pivotPID.setP(ConfigManager.getInstance().get("arm_p", ArmConstants.PIVOT_P)); pivotPID.setI(ConfigManager.getInstance().get("arm_i", ArmConstants.PIVOT_I)); pivotPID.setD(ConfigManager.getInstance().get("arm_d", ArmConstants.PIVOT_D)); diff --git a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java index 9f96d79..553a444 100644 --- a/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ElevatorSubsystem.java @@ -12,7 +12,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ElevatorConstants; import org.blackknights.utils.ConfigManager; @@ -32,9 +31,6 @@ public class ElevatorSubsystem extends SubsystemBase { private final RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder(); // Linebreaks - private final DigitalInput topLineBreak = new DigitalInput(ElevatorConstants.TOP_LINEBREAK_ID); - private final DigitalInput bottomLineBreak = - new DigitalInput(ElevatorConstants.BOTTOM_LINEBREAK_ID); private final ProfiledPIDController elevatorPID = new ProfiledPIDController( @@ -210,10 +206,6 @@ public double getElevatorVelocity() { return encoderAverageVel * ElevatorConstants.ROTATIONS_TO_METERS; } - public boolean getBottomLinebreak() { - return !bottomLineBreak.get(); - } - /** * Check if elevator is at target position * diff --git a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java index eab9b27..27a10de 100644 --- a/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/IntakeSubsystem.java @@ -2,6 +2,7 @@ package org.blackknights.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.blackknights.constants.ArmConstants; @@ -9,9 +10,11 @@ public class IntakeSubsystem extends SubsystemBase { private final WPI_TalonSRX motor = new WPI_TalonSRX(ArmConstants.MOTOR_ID); + private final DigitalInput intakeLinebreak = new DigitalInput(0); + /** Create a new intake subsystem */ public IntakeSubsystem() { - motor.setInverted(false); + motor.setInverted(true); motor.enableCurrentLimit(true); motor.configContinuousCurrentLimit(20); motor.configPeakCurrentLimit(0); @@ -34,4 +37,8 @@ public void setSpeed(double speed) { public void setVoltage(double voltage) { motor.setVoltage(voltage); } + + public boolean getLinebreak() { + return !intakeLinebreak.get(); + } } From 71368c1dfdb11cd4653873d732b94275c519b79b Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Tue, 4 Mar 2025 22:35:18 -0700 Subject: [PATCH 03/13] changing to CommandXboxController --- .../java/org/blackknights/RobotContainer.java | 34 ++++++++++--------- .../blackknights/commands/ClimberCommand.java | 13 +++---- .../org/blackknights/utils/Controller.java | 3 -- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index bd0fb54..8d75fd0 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -7,6 +7,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.blackknights.commands.*; import org.blackknights.constants.ScoringConstants; import org.blackknights.constants.VisionConstants; @@ -28,8 +30,8 @@ public class RobotContainer { ButtonBoardSubsystem buttonBoardSubsystem = new ButtonBoardSubsystem(buttonBoard); // Controllers - Controller primaryController = new Controller(0); - Controller secondaryController = new Controller(1); + CommandXboxController primaryController = new CommandXboxController(0); + CommandXboxController secondaryController = new CommandXboxController(1); private final NetworkTablesUtils NTTune = NetworkTablesUtils.getTable("debug"); @@ -88,10 +90,10 @@ private void configureBindings() { true, true)); - primaryController.leftBumper.whileTrue( + primaryController.leftBumper().whileTrue( getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - primaryController.rightBumper.whileTrue( + primaryController.leftBumper().whileTrue( new ParallelCommandGroup( new ElevatorArmCommand( elevatorSubsystem, @@ -101,7 +103,7 @@ private void configureBindings() { elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); - primaryController.dpadDown.whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); + primaryController.povDown().whileTrue(new RunCommand(() -> swerveSubsystem.zeroGyro())); // SECONDARY CONTROLLER @@ -113,32 +115,32 @@ private void configureBindings() { // secondaryController.rightBumper.onTrue(new InstantCommand(() -> // coralQueue.stepForwards())); - secondaryController.aButton.whileTrue( + secondaryController.a().whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1)); - secondaryController.bButton.whileTrue( + secondaryController.b().whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L2)); - secondaryController.xButton.whileTrue( + secondaryController.x().whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L3)); - secondaryController.yButton.whileTrue( + secondaryController.y().whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); - secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + secondaryController.leftBumper().onTrue(new InstantCommand(() -> coralQueue.stepForwards())); - secondaryController.rightBumper.whileTrue( + secondaryController.rightBumper().whileTrue( new InstantCommand(() -> coralQueue.stepBackwards())); - // secondaryController.rightTrigger.whileTrue( - // new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); - // - // secondaryController.leftTrigger.whileTrue( - // new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + secondaryController.rightTrigger(0.2).whileTrue( + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); + + secondaryController.leftTrigger(0.2).whileTrue( + new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); } /** Runs once when the code starts */ diff --git a/src/main/java/org/blackknights/commands/ClimberCommand.java b/src/main/java/org/blackknights/commands/ClimberCommand.java index 06d49cf..44709f3 100644 --- a/src/main/java/org/blackknights/commands/ClimberCommand.java +++ b/src/main/java/org/blackknights/commands/ClimberCommand.java @@ -2,13 +2,14 @@ package org.blackknights.commands; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.blackknights.subsystems.ClimberSubsystem; import org.blackknights.utils.Controller; /** Climber command to control the climber */ public class ClimberCommand extends Command { public ClimberSubsystem climberSubsystem; - public Controller controller; + public CommandXboxController controller; /** * Command to controller the climber, right now over pure voltage @@ -16,7 +17,7 @@ public class ClimberCommand extends Command { * @param climberSubsystem The instance of {@link ClimberSubsystem} * @param controller A {@link Controller} to control the climber */ - public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) { + public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController controller) { this.climberSubsystem = climberSubsystem; this.controller = controller; addRequirements(climberSubsystem); @@ -24,13 +25,13 @@ public ClimberCommand(ClimberSubsystem climberSubsystem, Controller controller) @Override public void execute() { - if (controller.dpadDown.getAsBoolean()) { + if (controller.povDown().getAsBoolean()) { climberSubsystem.setClimberSpeed(1); - } else if (controller.dpadUp.getAsBoolean()) { + } else if (controller.povUp().getAsBoolean()) { climberSubsystem.setClimberSpeed(-1); - } else if (controller.dpadLeft.getAsBoolean()) { + } else if (controller.povLeft().getAsBoolean()) { climberSubsystem.setLockSpeed(0.5); - } else if (controller.dpadRight.getAsBoolean()) { + } else if (controller.povRight().getAsBoolean()) { climberSubsystem.setLockSpeed(-0.5); } else { climberSubsystem.setClimberSpeed(0); diff --git a/src/main/java/org/blackknights/utils/Controller.java b/src/main/java/org/blackknights/utils/Controller.java index c6b5593..b920039 100644 --- a/src/main/java/org/blackknights/utils/Controller.java +++ b/src/main/java/org/blackknights/utils/Controller.java @@ -38,9 +38,6 @@ public class Controller extends XboxController { public Controller(int port) { super(port); - leftTrigger = new JoystickButton(this, XboxController.Axis.kLeftTrigger.value); - rightTrigger = new JoystickButton(this, XboxController.Axis.kRightTrigger.value); - leftBumper = new JoystickButton(this, XboxController.Button.kLeftBumper.value); rightBumper = new JoystickButton(this, XboxController.Button.kRightBumper.value); From 788f7b5e23badcd4086b8d94741d873224309816 Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Tue, 4 Mar 2025 22:40:57 -0700 Subject: [PATCH 04/13] adding climber motor config, fixing robot container --- .../java/org/blackknights/RobotContainer.java | 5 ----- .../blackknights/subsystems/ClimberSubsystem.java | 15 +++++++++++++++ 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index bd0fb54..b96c002 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -108,11 +108,6 @@ private void configureBindings() { climberSubsystem.setDefaultCommand( new ClimberCommand(climberSubsystem, secondaryController)); - // secondaryController.leftBumper.onTrue(new InstantCommand(() -> - // coralQueue.stepBackwards())); - // secondaryController.rightBumper.onTrue(new InstantCommand(() -> - // coralQueue.stepForwards())); - secondaryController.aButton.whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1)); diff --git a/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java b/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java index f9a7a92..437e8e7 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,18 @@ public class ClimberSubsystem extends SubsystemBase { public SparkFlex climberMotor = new SparkFlex(15, SparkFlex.MotorType.kBrushless); public TalonSRX lockMotor = new TalonSRX(16); + public SparkFlexConfig climberMotorConfig = new SparkFlexConfig(); + + public ClimberSubsystem() { + climberMotorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); + climberMotorConfig.smartCurrentLimit(40, 40); + + climberMotor.configure( + climberMotorConfig, + SparkBase.ResetMode.kResetSafeParameters, + SparkBase.PersistMode.kPersistParameters); + } + /** * Set the speed of the climber motor * From d64a483109491a8afe208ede0c4b525e7a10949d Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Tue, 4 Mar 2025 22:41:40 -0700 Subject: [PATCH 05/13] linting --- src/main/java/org/blackknights/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index b96c002..ac5c10e 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -126,7 +126,7 @@ private void configureBindings() { secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); - secondaryController.rightBumper.whileTrue( + secondaryController.rightBumper.onTrue( new InstantCommand(() -> coralQueue.stepBackwards())); // secondaryController.rightTrigger.whileTrue( From 45734ccec13a470ab01a38725c84ee0cf2115d19 Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Tue, 4 Mar 2025 22:42:15 -0700 Subject: [PATCH 06/13] linting --- .../java/org/blackknights/RobotContainer.java | 88 ++++++++++++------- 1 file changed, 57 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index 8d75fd0..97141e7 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -6,9 +6,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.*; -import java.util.function.Supplier; - import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import java.util.function.Supplier; import org.blackknights.commands.*; import org.blackknights.constants.ScoringConstants; import org.blackknights.constants.VisionConstants; @@ -90,16 +89,22 @@ private void configureBindings() { true, true)); - primaryController.leftBumper().whileTrue( - getPlaceCommand(() -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); - - primaryController.leftBumper().whileTrue( - new ParallelCommandGroup( - new ElevatorArmCommand( - elevatorSubsystem, - armSubsystem, - () -> ScoringConstants.ScoringHeights.INTAKE), - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); + primaryController + .leftBumper() + .whileTrue( + getPlaceCommand( + () -> coralQueue.getCurrentPosition(), () -> coralQueue.getNext())); + + primaryController + .leftBumper() + .whileTrue( + new ParallelCommandGroup( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.INTAKE), + new IntakeCommand( + intakeSubsystem, IntakeCommand.IntakeMode.INTAKE))); elevatorSubsystem.setDefaultCommand(new BaseCommand(elevatorSubsystem, armSubsystem)); @@ -115,32 +120,53 @@ private void configureBindings() { // secondaryController.rightBumper.onTrue(new InstantCommand(() -> // coralQueue.stepForwards())); - secondaryController.a().whileTrue( - new ElevatorArmCommand( - elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1)); + secondaryController + .a() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L1)); - secondaryController.b().whileTrue( - new ElevatorArmCommand( - elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L2)); + secondaryController + .b() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L2)); - secondaryController.x().whileTrue( - new ElevatorArmCommand( - elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L3)); + secondaryController + .x() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L3)); - secondaryController.y().whileTrue( - new ElevatorArmCommand( - elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L4)); + secondaryController + .y() + .whileTrue( + new ElevatorArmCommand( + elevatorSubsystem, + armSubsystem, + () -> ScoringConstants.ScoringHeights.L4)); - secondaryController.leftBumper().onTrue(new InstantCommand(() -> coralQueue.stepForwards())); + secondaryController + .leftBumper() + .onTrue(new InstantCommand(() -> coralQueue.stepForwards())); - secondaryController.rightBumper().whileTrue( - new InstantCommand(() -> coralQueue.stepBackwards())); + secondaryController + .rightBumper() + .whileTrue(new InstantCommand(() -> coralQueue.stepBackwards())); - secondaryController.rightTrigger(0.2).whileTrue( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); + secondaryController + .rightTrigger(0.2) + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.OUTTAKE)); - secondaryController.leftTrigger(0.2).whileTrue( - new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); + secondaryController + .leftTrigger(0.2) + .whileTrue(new IntakeCommand(intakeSubsystem, IntakeCommand.IntakeMode.INTAKE)); } /** Runs once when the code starts */ From 27d73af579936b5b64635183adaf1f20cf935b32 Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Wed, 5 Mar 2025 22:07:55 -0600 Subject: [PATCH 07/13] Revert "linting" This reverts commit d64a483109491a8afe208ede0c4b525e7a10949d. --- src/main/java/org/blackknights/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index ac5c10e..b96c002 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -126,7 +126,7 @@ private void configureBindings() { secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); - secondaryController.rightBumper.onTrue( + secondaryController.rightBumper.whileTrue( new InstantCommand(() -> coralQueue.stepBackwards())); // secondaryController.rightTrigger.whileTrue( From 04badd32b40f1bfb03ed2b024eb96c895b8fd55d Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Wed, 5 Mar 2025 22:09:23 -0600 Subject: [PATCH 08/13] Revert "Revert "linting"" This reverts commit 27d73af579936b5b64635183adaf1f20cf935b32. --- src/main/java/org/blackknights/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index b96c002..ac5c10e 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -126,7 +126,7 @@ private void configureBindings() { secondaryController.leftBumper.onTrue(new InstantCommand(() -> coralQueue.stepForwards())); - secondaryController.rightBumper.whileTrue( + secondaryController.rightBumper.onTrue( new InstantCommand(() -> coralQueue.stepBackwards())); // secondaryController.rightTrigger.whileTrue( From b441117ecadce8e12924e28d7639598219ffa68e Mon Sep 17 00:00:00 2001 From: aaditsangvikar Date: Wed, 5 Mar 2025 22:10:21 -0600 Subject: [PATCH 09/13] Revert "adding climber motor config, fixing robot container" This reverts commit 788f7b5e23badcd4086b8d94741d873224309816. --- .../java/org/blackknights/RobotContainer.java | 5 +++++ .../blackknights/subsystems/ClimberSubsystem.java | 15 --------------- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/blackknights/RobotContainer.java b/src/main/java/org/blackknights/RobotContainer.java index ac5c10e..65b089b 100644 --- a/src/main/java/org/blackknights/RobotContainer.java +++ b/src/main/java/org/blackknights/RobotContainer.java @@ -108,6 +108,11 @@ private void configureBindings() { climberSubsystem.setDefaultCommand( new ClimberCommand(climberSubsystem, secondaryController)); + // secondaryController.leftBumper.onTrue(new InstantCommand(() -> + // coralQueue.stepBackwards())); + // secondaryController.rightBumper.onTrue(new InstantCommand(() -> + // coralQueue.stepForwards())); + secondaryController.aButton.whileTrue( new ElevatorArmCommand( elevatorSubsystem, armSubsystem, () -> ScoringConstants.ScoringHeights.L1)); diff --git a/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java b/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java index 437e8e7..f9a7a92 100644 --- a/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java +++ b/src/main/java/org/blackknights/subsystems/ClimberSubsystem.java @@ -2,10 +2,7 @@ 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 */ @@ -13,18 +10,6 @@ public class ClimberSubsystem extends SubsystemBase { public SparkFlex climberMotor = new SparkFlex(15, SparkFlex.MotorType.kBrushless); public TalonSRX lockMotor = new TalonSRX(16); - public SparkFlexConfig climberMotorConfig = new SparkFlexConfig(); - - public ClimberSubsystem() { - climberMotorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); - climberMotorConfig.smartCurrentLimit(40, 40); - - climberMotor.configure( - climberMotorConfig, - SparkBase.ResetMode.kResetSafeParameters, - SparkBase.PersistMode.kPersistParameters); - } - /** * Set the speed of the climber motor * From deed47cb51742838b5028c716718e58162c02adc Mon Sep 17 00:00:00 2001 From: TotalTaxAmount <64336456+TotalTaxAmount@users.noreply.github.com> Date: Thu, 6 Mar 2025 07:56:35 -0700 Subject: [PATCH 10/13] Delete src/main/java/org/blackknights/utils/Controller.java --- .../org/blackknights/utils/Controller.java | 73 ------------------- 1 file changed, 73 deletions(-) delete mode 100644 src/main/java/org/blackknights/utils/Controller.java diff --git a/src/main/java/org/blackknights/utils/Controller.java b/src/main/java/org/blackknights/utils/Controller.java deleted file mode 100644 index b920039..0000000 --- a/src/main/java/org/blackknights/utils/Controller.java +++ /dev/null @@ -1,73 +0,0 @@ -/* Black Knights Robotics (C) 2025 */ -package org.blackknights.utils; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; - -/** A wrapper class for {@link XboxController} to provide easier bindings */ -public class Controller extends XboxController { - - public JoystickButton leftBumper; - public JoystickButton rightBumper; - - public JoystickButton leftTrigger; - public JoystickButton rightTrigger; - - public JoystickButton yButton; - public JoystickButton aButton; - public JoystickButton bButton; - public JoystickButton xButton; - - public JoystickButton startButton; - public JoystickButton selectButton; - - public JoystickButton leftStickButton; - public JoystickButton rightStickButton; - - public POVButton dpadUp; - public POVButton dpadDown; - public POVButton dpadLeft; - public POVButton dpadRight; - - /** - * Create a new instance of controller - * - * @param port The port of the controller as specified on DriverStation - */ - public Controller(int port) { - super(port); - - leftBumper = new JoystickButton(this, XboxController.Button.kLeftBumper.value); - rightBumper = new JoystickButton(this, XboxController.Button.kRightBumper.value); - - yButton = new JoystickButton(this, XboxController.Button.kY.value); - aButton = new JoystickButton(this, XboxController.Button.kA.value); - bButton = new JoystickButton(this, XboxController.Button.kB.value); - xButton = new JoystickButton(this, XboxController.Button.kX.value); - - startButton = new JoystickButton(this, XboxController.Button.kStart.value); - selectButton = new JoystickButton(this, XboxController.Button.kBack.value); - - leftStickButton = new JoystickButton(this, XboxController.Button.kLeftStick.value); - rightStickButton = new JoystickButton(this, XboxController.Button.kRightStick.value); - - dpadUp = new POVButton(this, 0); - dpadDown = new POVButton(this, 180); - dpadLeft = new POVButton(this, 270); - dpadRight = new POVButton(this, 90); - } - - /** - * Check if the controller has input from the sticks - * - * @param deadzone Controller deadzone - * @return If the controller has input - */ - public boolean hasStickInput(double deadzone) { - return Math.abs(getLeftX()) > deadzone - || Math.abs(getLeftY()) > deadzone - || Math.abs(getRightX()) > deadzone - || Math.abs(getRightY()) > deadzone; - } -} From 1d00d865da6d66e9a485f4df8071fe22b293b3c3 Mon Sep 17 00:00:00 2001 From: TotalTaxAmount <64336456+TotalTaxAmount@users.noreply.github.com> Date: Thu, 6 Mar 2025 09:20:26 -0700 Subject: [PATCH 11/13] misc: fix ClimberCommand.java --- src/main/java/org/blackknights/commands/ClimberCommand.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/blackknights/commands/ClimberCommand.java b/src/main/java/org/blackknights/commands/ClimberCommand.java index 44709f3..644d5da 100644 --- a/src/main/java/org/blackknights/commands/ClimberCommand.java +++ b/src/main/java/org/blackknights/commands/ClimberCommand.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import org.blackknights.subsystems.ClimberSubsystem; -import org.blackknights.utils.Controller; /** Climber command to control the climber */ public class ClimberCommand extends Command { @@ -15,7 +14,7 @@ public class ClimberCommand extends Command { * Command to controller the climber, right now over pure voltage * * @param climberSubsystem The instance of {@link ClimberSubsystem} - * @param controller A {@link Controller} to control the climber + * @param controller A {@link CommandXboxController} to control the climber */ public ClimberCommand(ClimberSubsystem climberSubsystem, CommandXboxController controller) { this.climberSubsystem = climberSubsystem; From 4fe996a7e8fab8c326f447386f12fbc8084ceef0 Mon Sep 17 00:00:00 2001 From: totaltaxamount Date: Sat, 8 Mar 2025 17:08:46 -0700 Subject: [PATCH 12/13] 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 13/13] 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));