From c5c6dd2eededa3b44d9ea7d3275d7217359f57b5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:28:28 -0700 Subject: [PATCH] Add codes --- src/main/java/frc4388/robot/Constants.java | 9 +- .../java/frc4388/robot/RobotContainer.java | 86 ++++++++++++++++--- .../frc4388/robot/commands/GotoLastApril.java | 2 + .../frc4388/robot/commands/LidarAlign.java | 4 +- .../frc4388/robot/subsystems/Elevator.java | 34 +++++--- 5 files changed, 108 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a00433..ff68716 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -242,7 +242,7 @@ public final class Constants { public static final int SECONDS_TO_MICROS = 1000000; public static final double XY_TOLERANCE = 0.07; // Meters - public static final double ROT_TOLERANCE = 1; // Degrees + public static final double ROT_TOLERANCE = 5; // Degrees // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); // public static final Pose2d targetpos = @@ -365,9 +365,11 @@ public final class Constants { ;;;;;;;;;;;;;;;;;;;;;;;;;;;; // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right - public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(17); public static final double L4_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); public static final double L4_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); + public static final double L3_DISTANCE_1 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(6); + public static final double L3_DISTANCE_2 = VERTICAL_SCORING_POSITION_OFFSET + Units.inchesToMeters(12); @@ -413,6 +415,7 @@ public final class Constants { public static final double WAITING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe public static final double SCORING_THREE_ELEVATOR = -9.25; + public static final double DEALGAE_L2_ELEVATOR = -23.5; public static final double MAX_POSITION_ELEVATOR = 4.5 * GEAR_RATIO_ELEVATOR; // TODO: find MAX position public static final double GEAR_RATIO_ENDEFECTOR = -100.0; @@ -421,7 +424,7 @@ public final class Constants { public static final double COMPLETLY_DOWN_ENDEFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; - public static final double SCORING_THREE_ENDEFECTOR = 0.375 * GEAR_RATIO_ENDEFECTOR; + public static final double PRIMED_THREE_ENDEFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; public static final double SCORING_FOUR_ENDEFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value public static final double COMPLETLY_TOP_ENDEFECTOR = 0.5 * GEAR_RATIO_ENDEFECTOR; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ecb7157..d957885 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ import java.util.List; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -115,19 +116,74 @@ public class RobotContainer { // private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); private Command AprilLidarAlign = new SequentialCommandGroup( new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.RIGHT), // new InstantCommand(() -> System.out.println("Soup")), // new WaitCommand(1), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT), new LidarAlign(m_robotSwerveDrive, m_lidar), - new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1, Side.LEFT), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), new waitEndefectorRefrence(m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,1), new Translation2d(), 500, true), new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) ); + + private Command AprilLidarAlignLeft = new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_2, Side.LEFT), + // new InstantCommand(() -> System.out.println("Soup")), + // new WaitCommand(1), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L4_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + ); + + private Command AprilLidarAlignL3Left = new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.LEFT), + // new InstantCommand(() -> System.out.println("Soup")), + // new WaitCommand(1), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.LEFT), + new LidarAlign(m_robotSwerveDrive, m_lidar), + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + ); + + private Command AprilLidarAlignL3 = new SequentialCommandGroup( + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_2, Side.RIGHT), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + + new GotoLastApril(m_robotSwerveDrive, m_vision, FieldConstants.L3_DISTANCE_1 - Units.inchesToMeters(0), Side.RIGHT), + + new LidarAlign(m_robotSwerveDrive, m_lidar), + + // new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator), + + new waitEndefectorRefrence(m_robotElevator), + new waitElevatorRefrence(m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), 500, true), + new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator) + ); + private Command AprilLidarLeft = new SequentialCommandGroup( AprilLidarAlign.asProxy(), new LidarAlign(m_robotSwerveDrive, m_lidar) @@ -265,12 +321,22 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(AprilLidarAlignLeft); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(AprilLidarAlign); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(AprilLidarAlign); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(AprilLidarAlign); + // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -281,17 +347,17 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON) + DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator)); - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator)); - - DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) + .onTrue(AprilLidarAlignL3Left); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) + .onTrue(AprilLidarAlignL3); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON) .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator)); diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 45e8bff..b3c6507 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -78,6 +78,8 @@ public class GotoLastApril extends Command { youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1; rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1; + // System.out.println(xerr + ", " + yerr + ", " + roterr + ", " + rotoutput); + Translation2d leftStick = new Translation2d( Math.max(Math.min(-youtput, 1), -1), Math.max(Math.min(-xoutput, 1), -1) diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index a40a878..20aac9d 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -39,7 +39,7 @@ public class LidarAlign extends Command { this.currentFinderTick = 0; this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; - this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + this.headedRight = (GotoLastApril.tagRelativeXError < 0); } @@ -52,7 +52,7 @@ public class LidarAlign extends Command { return; } - if (currentFinderTick > 100) { //arbutrary threshhold for now. + if (currentFinderTick > 40) { //arbutrary threshhold for now. headedRight = !headedRight; currentFinderTick *= -1; } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 7747bfe..260739b 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -106,12 +106,6 @@ public class Elevator extends SubsystemBase { break; } - case ScoringThree: { - PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR); - PIDPosition(endefectorMotor, ElevatorConstants.SCORING_THREE_ENDEFECTOR); - break; - } - case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR); PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFECTOR); @@ -123,6 +117,22 @@ public class Elevator extends SubsystemBase { PIDPosition(endefectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFECTOR); break; } + + case PrimedThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFECTOR); + break; + } + + case ScoringThree: { + PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR); + PIDPosition(endefectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFECTOR); + break; + } + + default: { + assert false; + } } } @@ -148,7 +158,7 @@ public class Elevator extends SubsystemBase { private void periodicWaiting() { if (!basinBeamBreak.get()) - transitionState(CoordinationState.WatingBeamTriped); + transitionState(CoordinationState.Ready); } private void periodicWaitingTripped() { @@ -157,8 +167,8 @@ public class Elevator extends SubsystemBase { } private void periodicReady() { - if (elevatorAtRefrence()) - transitionState(CoordinationState.Hovering); + // if (elevatorAtRefrence()) + // transitionState(CoordinationState.Hovering); } private void periodicScoring() { @@ -173,11 +183,11 @@ public class Elevator extends SubsystemBase { SmartDashboard.putNumber("endefector", endefectorLimitSwitch.get() ? 1 : 0); if (currentState == CoordinationState.Waiting) { - periodicWaiting(); + // periodicWaiting(); } else if (currentState == CoordinationState.WatingBeamTriped) { - periodicWaitingTripped(); + // periodicWaitingTripped(); } else if (currentState == CoordinationState.Ready) { - periodicReady(); + // periodicReady(); } // } else if (currentState == CoordinationState.ScoringThree || currentState == CoordinationState.ScoringFour) { // periodicScoring();