Skip to content

Pre dcmp #88

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

Merged
spellingcat merged 8 commits into from
Apr 11, 2026
Merged

Pre dcmp #88

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
364afa7
disable rotation slew rate limiting + stop rezeroing turret twice (ho…
spellingcat Apr 10, 2026
b14d092
remove tuning mode
spellingcat Apr 10, 2026
46c8de8
turret drifting alert
spellingcat Apr 10, 2026
fa23f36
zero only at the start of spinup
spellingcat Apr 10, 2026
81da229
make right trench double dip go behind hub
spellingcat Apr 10, 2026
e44462f
rename and reorder autos
spellingcat Apr 10, 2026
3208034
remove intake as a way of getting into flow state
spellingcat Apr 10, 2026
f39b252
stop logging some stuff
spellingcat Apr 10, 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
173 changes: 173 additions & 0 deletions src/main/deploy/choreo/RCleanuptoRPreTrenchReversed.traj
View file
Open in desktop

Large diffs are not rendered by default.

163 changes: 163 additions & 0 deletions src/main/deploy/choreo/RPreTrenchReversedtoRCleanup.traj
View file
Open in desktop

Large diffs are not rendered by default.

9 changes: 7 additions & 2 deletions src/main/java/frc/robot/Autos.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,9 @@ public enum Path {
RPreTrenchtoRNeutral("RPreTrenchtoRNeutral", Action.INTAKE),
RPreTrenchReversedtoRNeutral("RPreTrenchReversedtoRNeutral", Action.INTAKE),

RPreTrenchReversedtoRCleanup("RPreTrenchReversedtoRCleanup", Action.INTAKE),
EndWScoreRCleanuptoRPreTrenchReversed("RCleanuptoRPreTrenchReversed", Action.SCORE_AT_END),

StartingRTrenchtoRNeutral("StartingRTrenchtoRNeutral", Action.INTAKE),
StartingLTrenchtoLNeutral("StartingLTrenchtoLNeutral", Action.INTAKE),
StartingRTrenchtoDisrupt("StartingRTrenchtoDisruptR", Action.INTAKE),
Expand Down Expand Up @@ -559,8 +562,10 @@ public Command getDoubleDipRightTrench() {
new Path[] {
Path.StartingRTrenchtoRNeutral,
Path.RNeutraltoRPreTrenchReversed,
Path.RPreTrenchReversedtoRNeutral,
Path.RNeutraltoRPreTrenchReversed,
Path.RPreTrenchReversedtoRCleanup,
Path.EndWScoreRCleanuptoRPreTrenchReversed
// Path.RPreTrenchReversedtoRNeutral,
// Path.RNeutraltoRPreTrenchReversed,
},
setRightClimb());
}
Expand Down
315 changes: 139 additions & 176 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop

Large diffs are not rendered by default.

190 changes: 75 additions & 115 deletions src/main/java/frc/robot/Superstructure.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,9 +171,7 @@ public boolean isScoringActive() {

@AutoLogOutput(key = "Superstructure/Score Request")
private Trigger scoreReq =
new Trigger(() -> shotTarget == ShotTarget.SCORE)
// .and(() -> canScore())
.or(Autos.autoScoreReq);
new Trigger(() -> shotTarget == ShotTarget.SCORE).or(Autos.autoScoreReq);

@AutoLogOutput(key = "Superstructure/Feed Request")
private Trigger feedReq = new Trigger(() -> shotTarget == ShotTarget.FEED);
Expand All @@ -193,9 +191,6 @@ public boolean isScoringActive() {
@AutoLogOutput(key = "Superstructure/Ready?")
private Trigger readyTrigger;

@AutoLogOutput(key = "Superstructure/Operator Pose Override?")
private static boolean poseOverride = false;

@AutoLogOutput(key = "Superstructure/Defense?")
private boolean defense = false;

Expand Down Expand Up @@ -290,13 +285,8 @@ private void addTriggers() {
operator.povUp().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
operator.povDown().onTrue(Commands.parallel(intake.restRetracted(), shooter.stopTurret()));
shootReq =
driver
.rightTrigger()
.and(DriverStation::isTeleop)
.and(() -> canShoot())
.or(Autos.autoScoreReq)
.and(() -> canShoot()); // Maybe should include if its our turn? //TODO fix auto
// bindings
new Trigger(() -> awayFromTrench())
.and((driver.rightTrigger().and(DriverStation::isTeleop)).or(Autos.autoScoreReq));

intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq);

Expand Down Expand Up @@ -337,9 +327,7 @@ private void addTransitions() {
// SCORE_FLOW transitions
{
bindTransition(
SuperState.IDLE,
SuperState.SPIN_UP_SCORE_FLOW,
scoreReq.and(flowReq).and(shootReq.or(intakeReq)));
SuperState.IDLE, SuperState.SPIN_UP_SCORE_FLOW, scoreReq.and(flowReq).and(shootReq));

bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.SCORE_FLOW, readyTrigger);

Expand All @@ -348,17 +336,13 @@ private void addTransitions() {
SuperState.SPIN_UP_SCORE_FLOW,
new Trigger(AutoAim::targetInTurretDeadzone));

bindTransition(
SuperState.SPIN_UP_SCORE_FLOW,
SuperState.IDLE,
intakeReq.negate().and(shootReq.negate()));
bindTransition(SuperState.SPIN_UP_SCORE_FLOW, SuperState.IDLE, shootReq.negate());

bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq);

bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate());

bindTransition(
SuperState.SCORE_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, shootReq.negate());
}

// --------------------------------------------------------------------------
Expand All @@ -377,9 +361,7 @@ private void addTransitions() {
// FEED_FLOW transitions
{
bindTransition(
SuperState.IDLE,
SuperState.SPIN_UP_FEED_FLOW,
feedReq.and(flowReq).and(shootReq.or(intakeReq)));
SuperState.IDLE, SuperState.SPIN_UP_FEED_FLOW, feedReq.and(flowReq).and(shootReq));

bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.FEED_FLOW, readyTrigger);

Expand All @@ -388,15 +370,13 @@ private void addTransitions() {
SuperState.SPIN_UP_FEED_FLOW,
new Trigger(AutoAim::targetInTurretDeadzone));

bindTransition(
SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
bindTransition(SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, shootReq.negate());

bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq);

bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate());

bindTransition(
SuperState.FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, shootReq.negate());
}

bindTransition(SuperState.SCORE, SuperState.FEED, feedReq);
Expand Down Expand Up @@ -487,13 +467,16 @@ private void addCommands() {
SuperState.SPIN_UP_FEED,
intake.restExtended(),
indexer.rest(),
shooter.feed(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
ShotTrees.FEED_SHOT_TREE)),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.feed(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
ShotTrees.FEED_SHOT_TREE))),
climber.retract());

bindCommands(
Expand Down Expand Up @@ -521,13 +504,16 @@ private void addCommands() {
SuperState.SPIN_UP_FEED_FLOW,
intake.intake(),
indexer.index(),
shooter.feed(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
ShotTrees.FEED_SHOT_TREE)),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.feed(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
ShotTrees.FEED_SHOT_TREE))),
climber.retract());

bindCommands(
Expand Down Expand Up @@ -587,18 +573,15 @@ private void addCommands() {
.flywheelVelocityRotPerSec()
// shooter.getTestVel()
),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE))),
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
// shooter.testShot(() -> swerve.getPose(), () -> swerve.getVelocityFieldRelative())),
climber.retract());

Expand Down Expand Up @@ -634,18 +617,15 @@ private void addCommands() {
: ShotTrees.COMP_HUB_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE))),
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.retract());

bindCommands(
Expand Down Expand Up @@ -685,18 +665,15 @@ private void addCommands() {
SuperState.SPIN_UP_SCORE_PRE_CLIMB,
intake.restRetracted(),
indexer.rest(),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE))),
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.extend());

bindCommands(
Expand All @@ -713,18 +690,15 @@ private void addCommands() {
: ShotTrees.COMP_HUB_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter
.resetTurretToCalculatedPosition()
.andThen(
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE))),
shooter.score(
() ->
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.extend());

bindCommands(
Expand All @@ -740,14 +714,14 @@ public void periodic() {
Logger.recordOutput("Superstructure/State Timer", stateTimer.get());

// this really should be in robot.java but i cooked myself with the robot selecting thing
Logger.recordOutput(
"shooter sotm viz",
new Pose3d(swerve.getPose())
.transformBy(
new Transform3d(
new Translation3d(0, 0, 0.5),
new Rotation3d(
0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0))));
// Logger.recordOutput(
// "shooter sotm viz",
// new Pose3d(swerve.getPose())
// .transformBy(
// new Transform3d(
// new Translation3d(0, 0, 0.5),
// new Rotation3d(
// 0, ((Math.PI / 2) - shooter.getHoodSetpoint().getRadians()) * -1, 0))));
}

/**
Expand Down Expand Up @@ -918,16 +892,10 @@ public boolean inScoringArea() {
}

public boolean canScore() {
return
// (isOurShift() || !DriverStation.isFMSAttached())
// &&
(inScoringArea() || poseOverride)
&& (!swerve.isNearTrench() || poseOverride
// || fixedShotTarget != FixedShotTarget.NONE
);
return inScoringArea() && !swerve.isNearTrench();
}

public boolean canShoot() {
public boolean awayFromTrench() {
return !swerve.isNearTrenchForHood();
}

Expand All @@ -938,12 +906,4 @@ public static ShotTarget getShotTarget() {
public static FeedTarget getFeedTarget() {
return feedTarget;
}

// public static FixedShotTarget getFixedShotTarget() {
// return fixedShotTarget;
// }

public static boolean getPoseOverride() {
return poseOverride;
}
}
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/components/camera/Camera.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,10 +106,10 @@ public Optional<EstimatedRobotPose> update(PhotonPipelineResult result) {
if (result.getTargets().size() < 1) {
return Optional.empty();
}
if (Robot.ROBOT_MODE != RobotMode.REAL)
Logger.recordOutput(
"Vision/" + io.getName() + "/Best Distance",
result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm());
// if (Robot.ROBOT_MODE != RobotMode.REAL)
// Logger.recordOutput(
// "Vision/" + io.getName() + "/Best Distance",
// result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm());
Optional<EstimatedRobotPose> estPose = estimator.update(result);
return estPose;
}
Expand Down Expand Up @@ -163,12 +163,12 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {
setSimPose(estPose, !inputs.stale);

// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose);
// Logger.recordOutput("Vision/" + getName() + "/Pose3d", visionPose);
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d());
// Logger.recordOutput("Vision/" + getName() + "/Pose2d", visionPose.toPose2d());
final Matrix<N3, N1> deviations = findVisionMeasurementStdDevs(estPose.get());
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData());
// Logger.recordOutput("Vision/" + getName() + "/Deviations", deviations.getData());

Tracer.trace(
"Add Measurement From " + getName(),
Expand All @@ -182,7 +182,7 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {

hasFutureData |= inputs.result.metadata.captureTimestampMicros > RobotController.getTime();
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update");
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Good Update");

Tracer.trace(
"Log Tag Poses",
Expand All @@ -196,16 +196,16 @@ public void updateCamera(SwerveDrivePoseEstimator swerveEstimator) {
.get();
}
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds);
// Logger.recordOutput("Vision/" + getName() + "/Target Poses", targetPose3ds);
});

} else {
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale");
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Stale");
}
} catch (NoSuchElementException e) {
// if (Robot.ROBOT_TYPE != RobotType.REAL)
Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate");
// Logger.recordOutput("Vision/" + getName() + "/Invalid Pose Result", "Bad Estimate");
}
futureVisionData.set(hasFutureData);
}
Expand Down
Loading
Loading