Skip to content

fixed the stow so that it works with the new dev #63

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
davidpolcen-sudo wants to merge 3 commits into from
Closed

fixed the stow so that it works with the new dev #63

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f8f4ed7
fixed the stow so that it works with the new dev
davidpolcen-sudo Apr 11, 2026
6115511
Merge branch 'dev' into feature/stow_update2
quipp Apr 11, 2026
7a0d986
Fix settle down time
quipp Apr 11, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Comment on lines +424 to +427
Copy link

Copilot AI Apr 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

HOOD_STABLE_TIME is introduced but is unused anywhere, while HOOD_STABLE (value 5) is the constant actually used as the Timer elapsed threshold. Either remove the unused constant or use a single clearly-named constant (with units in the name/comment) for the stow stability time to avoid confusion/misconfiguration.

Suggested change
public static final double HOOD_STABLE = 5;
public static final double HOOD_STABLE_TIME = 0.1;
public static final double HOOD_STABLE = 5; // seconds

Copilot uses AI. Check for mistakes.
public static final double OUTPUT_RANGE_MAX = 1;
public static final double OUTPUT_RANGE_MIN = -1;

Expand Down Expand Up @@ -523,6 +528,8 @@ public static class Transfer {

public static final double KICKER_LOAD_POWER = 0.25;
public static final double FEEDER_LOAD_POWER = 0.5;

public static final double LOAD_DELAY = 0.1;
}

public static class VisionOdometryConstants {
Expand Down
17 changes: 11 additions & 6 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -378,10 +378,18 @@ public void configureBindings() {
targetingSupplier,
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());
shootTrigger
.whileTrue(
new WaitCommand(Constants.Transfer.LOAD_DELAY)
.andThen(transferSubsystem.getLoadCommand()));

/* Intake pivot runs while button held */
intakeTrigger.whileTrue(
Expand Down Expand Up @@ -539,10 +547,7 @@ public void scheduleTeleOp() {
manualRotationVelocityDirective, null, null);
driveSubsystem.setDefaultCommand(manualDriveCommand);

turretSubsystem.setDefaultCommand(turretSubsystem.getTargetCommand(
targetingSupplier,
driveSubsystem::getPose,
driveSubsystem::getVelocity));
turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand());
Copy link

Copilot AI Apr 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TurretSubsystem's getStowCommand() sets SmartDashboard key Turret/IsStowing to true on initialize and never clears it. With the turret default command now set to getStowCommand(), this flag will likely stay true for the entire teleop session, which makes the indicator misleading. Consider changing the default to a non-stow idle/hold command, or update getStowCommand() to set the flag false when the stow sequence completes/ends.

Suggested change
turretSubsystem.setDefaultCommand(turretSubsystem.getStowCommand());
turretSubsystem.setDefaultCommand(new RunCommand(() -> {
}, turretSubsystem));

Copilot uses AI. Check for mistakes.

}

Expand Down
45 changes: 30 additions & 15 deletions src/main/java/frc/robot/subsystems/TurretSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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
*/
Expand Down Expand Up @@ -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),
Comment thread
quipp marked this conversation as resolved.
this)

.until(() -> {
boolean within = Math.abs(getHoodVelocity()) <= stopThreshold;
if (within) {
if (!settleTimer.isRunning()) {
settleTimer.start();
}

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);
resetHoodAngle(Constants.Turret.HOOD_STOW_POSITION);
});

return Commands.sequence(moveToTarget, settleDown, finish, Commands.idle())
Expand Down
Loading