From 2a0d5c176878ff8dafccbcb8b22daf4747344fad Mon Sep 17 00:00:00 2001 From: Patrick Wang <86264473+iapcmeimpcij2pjncr@users.noreply.github.com> Date: Sat, 22 Mar 2025 09:48:47 -0500 Subject: [PATCH 1/2] feat: add score offset to smart dashboard --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bba590b..e7776b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -291,7 +291,7 @@ public static class NarwhalConstants { public static final double UPPER_ASSEMBLY_MOI = 0.995; //Kg m^2 public static final Transform2d INTAKING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0),new Rotation2d(Math.PI)); - public static final Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.1524,0.0),new Rotation2d(0)); + public static Transform2d SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.1524,0.0),new Rotation2d(0)); public static final Transform2d ALGAE_REMOVAL_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0),new Rotation2d(0.0)); public static final Transform2d CLIMB_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(0.0,0.0),new Rotation2d(Math.PI)); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ddae85b..669c471 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,8 @@ import frc.robot.util.upper_assembly.UpperAssemblyFactory; import frc.robot.util.upper_assembly.UpperAssemblyType; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -168,6 +170,10 @@ private void setupSmartDashboard() { SmartDashboard.putNumber("L4 Angle", NarwhalConstants.NarwhalWristConstants.L4_OUTTAKE_ANGLE.getDegrees()); SmartDashboard.putNumber("L4 Height", NarwhalConstants.NarwhalElevatorConstants.L4_ELEVATOR_HEIGHT); + SmartDashboard.putNumber("Auto Score Offset X", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getX()); + SmartDashboard.putNumber("Auto Score Offset Y", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getY()); + SmartDashboard.putNumber("Auto Score Offset Rot", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getRotation().getDegrees()); + SmartDashboard.putNumber("Auto Delay", this.delayTimeSeconds); SmartDashboard.putBoolean("Tush Push Mode", this.startWithTushPush); } @@ -189,6 +195,11 @@ public void scheduleAutonomous() { this.delayTimeSeconds = SmartDashboard.getNumber("Auto Delay", this.delayTimeSeconds); this.startWithTushPush = SmartDashboard.getBoolean("Tush Push Mode", this.startWithTushPush); + double autoScoreOffsetX = SmartDashboard.getNumber("Auto Score Offset X", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getX()); + double autoScoreOffsetY = SmartDashboard.getNumber("Auto Score Offset Y", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getY()); + double autoScoreOffsetRot = SmartDashboard.getNumber("Auto Score Offset Rot", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getRotation().getDegrees()); + + NarwhalConstants.SCORING_RELATIVE_TRANSFORM = new Transform2d(new Translation2d(autoScoreOffsetX, autoScoreOffsetY), Rotation2d.fromDegrees(autoScoreOffsetRot)); // Get and display the selected autonomous mode. AutonomousRoutine autonomousRoutine = autonomousSelector.getSelected(); From 2d3f690ace6bbc09d672856e78935d7222b61041 Mon Sep 17 00:00:00 2001 From: Patrick Wang <86264473+iapcmeimpcij2pjncr@users.noreply.github.com> Date: Sun, 23 Mar 2025 13:32:10 -0500 Subject: [PATCH 2/2] feat: add delay to coral score command to prevent premature score and add climb constants to smart dashboard --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- .../robot/subsystems/narwhal/NarwhalUpperAssembly.java | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e7776b9..1ce7a50 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -367,8 +367,8 @@ public static class NarwhalClimberConstants { public static final double CLIMBER_PID_MIN_OUTPUT = -1.0; public static final double CLIMBER_PID_MAX_OUTPUT = 1.0; - public static final Rotation2d DEPLOYED_WINCH_ROTATIONS = Rotation2d.fromDegrees(1080); - public static final Rotation2d CLIMB_WINCH_ROTATIONS = Rotation2d.fromDegrees(270); + public static Rotation2d DEPLOYED_WINCH_ROTATIONS = Rotation2d.fromDegrees(1035); + public static Rotation2d CLIMB_WINCH_ROTATIONS = Rotation2d.fromDegrees(270); /** The angle tolerance for the climber to be considered at a specific state. */ public static final Rotation2d CLIMBER_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 669c471..9c340ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -158,7 +158,7 @@ private void setupSmartDashboard() { SmartDashboard.putData(this.upperAssemblySelector); SmartDashboard.putData(this.drivingMotorSelector); - // Add autonomous configuration options. + // Add narwhal upper assembly configuration options. SmartDashboard.putNumber("Intake Angle", NarwhalConstants.NarwhalWristConstants.INTAKE_ANGLE.getDegrees()); SmartDashboard.putNumber("Intake Height", NarwhalConstants.NarwhalElevatorConstants.INTAKE_ELEVATOR_HEIGHT_METERS); SmartDashboard.putNumber("L1 Angle", NarwhalConstants.NarwhalWristConstants.L1_OUTTAKE_ANGLE.getDegrees()); @@ -169,7 +169,11 @@ private void setupSmartDashboard() { SmartDashboard.putNumber("L3 Height", NarwhalConstants.NarwhalElevatorConstants.L3_ELEVATOR_HEIGHT); SmartDashboard.putNumber("L4 Angle", NarwhalConstants.NarwhalWristConstants.L4_OUTTAKE_ANGLE.getDegrees()); SmartDashboard.putNumber("L4 Height", NarwhalConstants.NarwhalElevatorConstants.L4_ELEVATOR_HEIGHT); - + + SmartDashboard.putNumber("Climb Rotations (degrees)", NarwhalConstants.NarwhalClimberConstants.CLIMB_WINCH_ROTATIONS.getDegrees()); + SmartDashboard.putNumber("Deploy Rotations (degrees)", NarwhalConstants.NarwhalClimberConstants.DEPLOYED_WINCH_ROTATIONS.getDegrees()); + + // Add autonomous configuration options. SmartDashboard.putNumber("Auto Score Offset X", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getX()); SmartDashboard.putNumber("Auto Score Offset Y", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getY()); SmartDashboard.putNumber("Auto Score Offset Rot", NarwhalConstants.SCORING_RELATIVE_TRANSFORM.getRotation().getDegrees()); @@ -294,6 +298,8 @@ public void scheduleAutonomous() { public void scheduleTeleOp() { CommandScheduler.getInstance().cancelAll(); this.setWristValuesFromSmartDashbaord(); + NarwhalConstants.NarwhalClimberConstants.CLIMB_WINCH_ROTATIONS = Rotation2d.fromDegrees(SmartDashboard.getNumber("Climb Rotations (degrees)", NarwhalConstants.NarwhalClimberConstants.CLIMB_WINCH_ROTATIONS.getDegrees())); + NarwhalConstants.NarwhalClimberConstants.DEPLOYED_WINCH_ROTATIONS = Rotation2d.fromDegrees(SmartDashboard.getNumber("Deploy Rotations (degrees)", NarwhalConstants.NarwhalClimberConstants.DEPLOYED_WINCH_ROTATIONS.getDegrees())); Alliance alliance = allianceSelector.getSelected(); SmartDashboard.putString("Selected Alliance", alliance.toString()); diff --git a/src/main/java/frc/robot/subsystems/narwhal/NarwhalUpperAssembly.java b/src/main/java/frc/robot/subsystems/narwhal/NarwhalUpperAssembly.java index 47b116d..0f10bee 100644 --- a/src/main/java/frc/robot/subsystems/narwhal/NarwhalUpperAssembly.java +++ b/src/main/java/frc/robot/subsystems/narwhal/NarwhalUpperAssembly.java @@ -90,6 +90,8 @@ public Command getCoralScoreCommand(ScoringHeight scoringHeight) { new NarwhalScorePositionCommand(elevator,wrist,scoringHeight) ).andThen( new WaitUntilCommand(robotInScoringPositionSupplier::get) + ).andThen( + new WaitCommand(1.0) ).andThen( new NarwhalOuttakeCommand(intakeOuttake) ).andThen(