From f8f4ed7c520ae9933c8c3c35317fc30baabe6675 Mon Sep 17 00:00:00 2001 From: David Polcen Date: Fri, 10 Apr 2026 19:15:13 -0500 Subject: [PATCH 1/2] fixed the stow so that it works with the new dev --- src/main/java/frc/robot/Constants.java | 7 +++ src/main/java/frc/robot/RobotContainer.java | 14 +++--- .../frc/robot/subsystems/TurretSubsystem.java | 45 ++++++++++++------- 3 files changed, 45 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f868c91..f3a9e44 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -420,6 +420,11 @@ public static class Turret { public static final Rotation2d HOOD_START_POSITION = Rotation2d.fromDegrees(5.00); public static final Rotation2d HOOD_STOW_POSITION = Rotation2d.fromDegrees(3.00); + public static final double HOOD_FINISH_VELOCITY = 0.01; + public static final double HOOD_STABLE = 5; + + public static final double HOOD_STABLE_TIME = 0.1; + public static final double OUTPUT_RANGE_MAX = 1; public static final double OUTPUT_RANGE_MIN = -1; @@ -519,6 +524,8 @@ public static class Transfer { public static final boolean MOTOR_INVERTED = true; public static final double LOAD_POWER = 0.25; + + public static final double LOAD_DELAY = 0.1; } public static class VisionOdometryConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d425963..99a449e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -381,12 +381,17 @@ public void configureBindings() { driveSubsystem::getPose, driveSubsystem::getVelocity)); - + shootTrigger.whileTrue( + turretSubsystem.getTargetCommand( + targetingSupplier, + driveSubsystem::getPose, + driveSubsystem::getVelocity)); /* Transfer runs ONLY while button AND solver valid */ shootTrigger .whileTrue( - transferSubsystem.getLoadCommand()); + new WaitCommand(Constants.Transfer.LOAD_DELAY) + .andThen(transferSubsystem.getLoadCommand())); /* Intake pivot runs while button held */ intakeTrigger.whileTrue( @@ -550,10 +555,7 @@ public void scheduleTeleOp() { manualRotationVelocityDirective, null, null); driveSubsystem.setDefaultCommand(manualDriveCommand); - turretSubsystem.setDefaultCommand(turretSubsystem.getTargetCommand( - targetingSupplier, - driveSubsystem::getPose, - driveSubsystem::getVelocity)); + turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand()); } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 1c1a2b7..cf3f1ae 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -2,18 +2,10 @@ import java.util.function.Supplier; -import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.FeedForwardConfig; -import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -22,6 +14,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PWM; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -194,6 +187,10 @@ public Rotation2d getHoodAngle() { - hoodOffsetSupplier.get().getRadians()); } + public double getHoodVelocity() { + return pitchEncoder.getVelocity().getValueAsDouble() * Constants.Turret.PITCH_ENCODER_FACTOR; + } + /** * @return the current turret yaw in the field frame */ @@ -353,16 +350,34 @@ public Command getStowCommand() { this); - Command settleDown = new RunCommand( - () -> pitchServo.setSpeed( - (Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * Constants.Turret.STOW_PUSH_DOWN_SPEED), // small - // constant - // downward - // speed - this).withTimeout(Constants.Turret.STOW_PUSH_DOWN_TIME); // enough to seat the gear + final Timer settleTimer = new Timer(); +double stopThreshold = Math.max(1e-3, Math.abs(Constants.Turret.HOOD_FINISH_VELOCITY)); + Command settleDown = new RunCommand( + + + () -> pitchServo.setSpeed((Constants.Turret.PITCH_INVERTED ? -1.0 : 1.0) * -Constants.Turret.STOW_PUSH_DOWN_SPEED), + this) + + .until(() -> { + boolean within = Math.abs(getHoodVelocity()) <= stopThreshold; + if (within) { + if (!settleTimer.isRunning()) { + settleTimer.start(); + } + + return settleTimer.hasElapsed(Constants.Turret.HOOD_STABLE); + } else { + settleTimer.stop(); + settleTimer.reset(); + return false; + } + } + ); + Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0); + resetHoodAngle(Constants.Turret.HOOD_STOW_POSITION); }); return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle()) From 7a0d986f5866f67905531d73b7ec6f0fb3388f10 Mon Sep 17 00:00:00 2001 From: "Jason M. Dudley" Date: Fri, 10 Apr 2026 19:43:33 -0500 Subject: [PATCH 2/2] Fix settle down time --- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index cf3f1ae..7e1d6c3 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -366,14 +366,14 @@ public Command getStowCommand() { settleTimer.start(); } - return settleTimer.hasElapsed(Constants.Turret.HOOD_STABLE); + return settleTimer.hasElapsed(Constants.Turret.HOOD_STABLE_TIME); } else { settleTimer.stop(); settleTimer.reset(); return false; } } - ); + ).withTimeout(Constants.Turret.STOW_PUSH_DOWN_TIME); Command finish = new InstantCommand(() -> { pitchServo.setSpeed(0.0);