From c58a2406ac44d3f1a81bc3a2842e829ed873a107 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 8 Mar 2025 13:22:14 -0700 Subject: [PATCH] Calibration --- src/main/java/frc4388/robot/Constants.java | 9 +-- .../java/frc4388/robot/RobotContainer.java | 67 ++++++++++++++----- .../frc4388/robot/subsystems/Elevator.java | 24 +++---- .../frc4388/robot/subsystems/SwerveDrive.java | 11 ++- .../java/frc4388/robot/subsystems/Vision.java | 29 +++++++- .../frc4388/utility/ReefPositionHelper.java | 7 +- 6 files changed, 110 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a392b2f..f562627 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -382,6 +382,7 @@ public final class Constants { public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; + public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; @@ -418,8 +419,8 @@ public final class Constants { public static final double HOVERING_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 DEALGAE_L3_ELEVATOR = -33.75; + public static final double DEALGAE_L2_ELEVATOR = -4; + public static final double DEALGAE_L3_ELEVATOR = -26.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; @@ -429,8 +430,8 @@ public final class Constants { public static final double L2_SCORE_ENDEFFECTOR = -19; - public static final double COMPLETLY_DOWN_ENDEFFECTOR = 0 * GEAR_RATIO_ENDEFECTOR; - public static final double DEALGAE_L2_ENDEFFECTOR = 0.18 * GEAR_RATIO_ENDEFECTOR; + public static final double COMPLETLY_DOWN_ENDEFFECTOR = 9001; + public static final double DEALGAE_L2_ENDEFFECTOR = 0.22 * GEAR_RATIO_ENDEFECTOR; public static final double COMPLETLY_MIDDLE_ENDEFFECTOR = 0.25 * GEAR_RATIO_ENDEFECTOR; public static final double PRIMED_THREE_ENDEFFECTOR = 0.4 * GEAR_RATIO_ENDEFECTOR; public static final double SCORING_FOUR_ENDEFFECTOR = 0.3 * GEAR_RATIO_ENDEFECTOR; // TODO: find this value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fc17912..5d3c31b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -299,31 +299,62 @@ public class RobotContainer { new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command leftAlgaeRemove = new SequentialCommandGroup( + // private Command leftAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + + private Command lowerAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new Translation2d(1,0), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME / 2, true), + new MoveForTimeCommand(m_robotSwerveDrive, + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME * 2, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); - private Command rightAlgaeRemove = new SequentialCommandGroup( + // private Command rightAlgaeRemove = new SequentialCommandGroup( + // new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + // new waitEndefectorRefrence(m_robotElevator), + // new waitElevatorRefrence(m_robotElevator), + // new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), + // new LidarAlign(m_robotSwerveDrive, m_reefLidar), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(1,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + // new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) + // ); + private Command upperAlgaeRemove = new SequentialCommandGroup( new InstantCommand(() -> {m_robotSwerveDrive.startSlowPeriod();}), new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE + Units.inchesToMeters(8), Side.FAR_LEFT, true), new waitEndefectorRefrence(m_robotElevator), new waitElevatorRefrence(m_robotElevator), - new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.LEFT, false), - new LidarAlign(m_robotSwerveDrive, m_reefLidar), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go);}, m_robotElevator), + new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.ALGAE_REMOVAL_DISTANCE, Side.FAR_LEFT, true), + waitDebuger.asProxy(), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go);}, m_robotElevator), new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0,1), new Translation2d(), AutoConstants.ALGAE_DRIVE_TIME, true), - new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), + new Translation2d(1,1), new Translation2d(0, 0), AutoConstants.ALGAE_DRIVE_TIME, true), + // new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.Waiting);}, m_robotElevator), new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();}) ); @@ -536,11 +567,11 @@ public class RobotContainer { // Lower algae removal new JoystickButton(getButtonBox(), ButtonBox.Eight) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); // Upper algae removal new JoystickButton(getButtonBox(), ButtonBox.Four) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); // Cancel button @@ -574,8 +605,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;})); - new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking())); // Auto Scoring new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) @@ -598,11 +629,11 @@ public class RobotContainer { //Controller Lower Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode) - .onTrue(leftAlgaeRemove); + .onTrue(lowerAlgaeRemove); //Controller Upper Algae Removal new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode) - .onTrue(rightAlgaeRemove); + .onTrue(upperAlgaeRemove); // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 17b6727..4b0ccdd 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -145,62 +145,62 @@ public class Elevator extends Subsystem { case L2Score: { PIDPosition(elevatorMotor, ElevatorConstants.L2_SCORE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.L2_SCORE_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_TOP_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringFour: { PIDPosition(elevatorMotor, ElevatorConstants.MAX_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.SCORING_FOUR_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case PrimedThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.PRIMED_THREE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case ScoringThree: { PIDPosition(elevatorMotor, ElevatorConstants.SCORING_THREE_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_DOWN_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Primed: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL2Go: { - PIDPosition(elevatorMotor, ElevatorConstants.WAITING_POSITION_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); + PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L2_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } case BallRemoverL3Primed: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); - PIDPosition(endeffectorMotor, ElevatorConstants.COMPLETLY_MIDDLE_ENDEFFECTOR); + PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR); break; } case BallRemoverL3Go: { PIDPosition(elevatorMotor, ElevatorConstants.DEALGAE_L3_ELEVATOR + AutoConstants.ELEVATOR_OFFSET_TRIM.get()); PIDPosition(endeffectorMotor, ElevatorConstants.DEALGAE_L2_ENDEFFECTOR + AutoConstants.ARM_OFFSET_TRIM.get()); - led.setMode(TimesNegativeOne.isRed ? LEDConstants.RED_PATTERN : LEDConstants.BLUE_PATTERN); + led.setMode(LEDConstants.SCORING_PATTERN); break; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 1c259e4..8ba2b72 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -307,6 +307,7 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.tareEverything(); robotKnowsWhereItIs = false; rotTarget = 0; + // vision.resetRotations(); } @@ -431,7 +432,15 @@ public class SwerveDrive extends Subsystem { public void setLastOdomSpeed(Optional curPose, Optional lastPose, double freq){ if(curPose.isPresent() && lastPose.isPresent()){ this.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq; - + + + // double rotDiff = curPose.get().getRotation().getDegrees() - lastPose.get().getRotation().getDegrees(); + + // if(rotDiff >= 180){ + // vision.subtractRotation(); + // }else if(rotDiff <= -180){ + // vision.addRotation(); + // } SmartDashboard.putNumber("Speed", lastOdomSpeed); } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index c0e4305..4998f03 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -110,6 +110,20 @@ public class Vision extends Subsystem { // cameras[0]. } + public double rotations = 0; + + public void resetRotations(){ + rotations = 0; + } + + public void addRotation(){ + rotations += 180; + } + + public void subtractRotation(){ + rotations -= 180; + } + private void update() { isTagProcessed = false; @@ -162,7 +176,20 @@ public class Vision extends Subsystem { lastLatency = latency / cams; if(isTagProcessed){ - lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); + + // double lastAngle = lastVisionPose.getRotation().getDegrees(); + double curAngle = Yaw/cams; + + // if(lastAngle - curAngle >= 90){ + // subtractRotation(); + // }else if(lastAngle - curAngle <= -90){ + // addRotation(); + // } + + // SmartDashboard.putNumber("curAngle", curAngle); + // SmartDashboard.putNumber("Rotations", rotations); + + lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle)); } } diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 7c020b7..75c4ad0 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/src/main/java/frc4388/utility/ReefPositionHelper.java @@ -5,6 +5,7 @@ import java.util.Optional; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc4388.robot.Constants.AutoConstants; @@ -14,7 +15,8 @@ public class ReefPositionHelper { public enum Side { LEFT, RIGHT, - CENTER + CENTER, + FAR_LEFT } public static final Pose2d[] RED_TAGS = { @@ -83,11 +85,14 @@ public class ReefPositionHelper { switch(side) { case LEFT: return -(AutoConstants.X_SCORING_POSITION_OFFSET); + case FAR_LEFT: + return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8)); case RIGHT: return (AutoConstants.X_SCORING_POSITION_OFFSET); case CENTER: return 0; } + assert false; return 0; }