Skip to content

Autoaim refactor #80

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 9 commits into from
Apr 6, 2026
Merged

Autoaim refactor #80

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
0e3778e
Rearrange all the autoaim files
SCool62 Apr 3, 2026
db79c28
Spotless
SCool62 Apr 3, 2026
f1a02cb
Fix log statements
SCool62 Apr 3, 2026
0f664cd
Rename getSOTMShotParameters
SCool62 Apr 3, 2026
1b95b42
Add doc comments
SCool62 Apr 3, 2026
099bf45
Spotless
SCool62 Apr 3, 2026
4f6d460
Merge branch 'main' into refactor/autoaim
spellingcat Apr 6, 2026
f51811c
uncook merge
spellingcat Apr 6, 2026
df8e695
i think this is ok for now
spellingcat Apr 6, 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
31 changes: 16 additions & 15 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@
import frc.robot.utils.FieldUtils.TrenchPoses;
import frc.robot.utils.FuelSim;
import frc.robot.utils.autoaim.AutoAim;
import frc.robot.utils.autoaim.NewAutoAim;
import java.io.File;
import java.util.Arrays;
import java.util.Optional;
Expand Down Expand Up @@ -186,7 +185,7 @@ public enum RobotEdition {

private final SlewRateLimiter xAccelLimiter = new SlewRateLimiter(1);
private final SlewRateLimiter yAccelLimiter = new SlewRateLimiter(1);
private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(3.0);
private final SlewRateLimiter rAccelLimiter = new SlewRateLimiter(0.5);

private static int lowBatteryCycleCount = 0;
private static final double lowBatteryVoltage =
Expand Down Expand Up @@ -651,6 +650,13 @@ public Robot() {
fuelSim.setSubticks(5);

// fuelSim.start();

// Log climb poses
Logger.recordOutput(
"AutoAlign/Climb Targets",
Arrays.stream(ClimbTargets.values())
.map(target -> target.getPose())
.toArray(Pose2d[]::new));
}

/** Scales a joystick value for teleop driving */
Expand Down Expand Up @@ -713,7 +719,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
Commands.parallel(
shooter.runHoodCurrentZeroing(), intake.runCurrentZeroing())));

new Trigger(() -> NewAutoAim.targetInTurretDeadzone())
new Trigger(() -> AutoAim.targetInTurretDeadzone())
.onTrue(driver.rumbleCmd(1, 1).withTimeout(0.25));
// .alongWith(operator.rumbleCmd(1, 1).withTimeout(0.25)));
// ---zeroing stuff---
Expand Down Expand Up @@ -765,7 +771,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
// driver
// .leftBumper()
// .and(
new Trigger(NewAutoAim::targetInTurretDeadzone)
new Trigger(AutoAim::targetInTurretDeadzone)
.and(() -> Superstructure.getState().isAScoreState())
.and(() -> !Superstructure.getState().isAFlowState())
.and(() -> !Superstructure.getPoseOverride())
Expand All @@ -782,7 +788,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
* SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
shooter::getTurretPosition));

new Trigger(NewAutoAim::targetInTurretDeadzone)
new Trigger(AutoAim::targetInTurretDeadzone)
.and(() -> Superstructure.getState().isAFeedState())
.and(() -> !Superstructure.getState().isAFlowState())
.and(() -> !Superstructure.getPoseOverride())
Expand Down Expand Up @@ -922,29 +928,24 @@ public void robotPeriodic() {
});

updateAlerts();
Logger.recordOutput("Flywheel Fudge Factor", AutoAim.getFudgeFactor());

// Log climb poses
Logger.recordOutput(
"AutoAlign/Climb Targets",
Arrays.stream(ClimbTargets.values())
.map(target -> target.getPose())
.toArray(Pose2d[]::new));
Logger.recordOutput("AutoAim/Flywheel Fudge Factor", AutoAim.getFudgeFactor());

Logger.recordOutput(
"trench poses",
Arrays.stream(TrenchPoses.values()).map(target -> target.getPose()).toArray(Pose2d[]::new));

Logger.recordOutput("Turret/out of range", NewAutoAim.targetInTurretDeadzone());
Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone());

noLogStickAlert.set(!directory.exists());

Logger.recordOutput(
"Distance to hub",
"AutoAim/Distance to hub",
shooter
.getTurretPose(swerve.getPose())
.getTranslation()
.getDistance(FieldUtils.getCurrentHubTranslation()));
Logger.recordOutput(
"AutoAim/Feed Target", FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose());
}

public void updateAlerts() {
Expand Down
94 changes: 43 additions & 51 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 @@ -24,7 +24,7 @@
import frc.robot.utils.FieldUtils;
import frc.robot.utils.FieldUtils.FeedTargets;
import frc.robot.utils.autoaim.AutoAim;
import frc.robot.utils.autoaim.NewAutoAim;
import frc.robot.utils.autoaim.ShotTrees;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

Expand Down Expand Up @@ -310,7 +310,7 @@ private void addTriggers() {
// .debounce(0.05)
.and(new Trigger(shooter::atHoodSetpoint).debounce(0.05))
.and(new Trigger(shooter::atTurretSetpoint).debounce(0.05))
.and(new Trigger(NewAutoAim::targetInTurretDeadzone).negate());
.and(new Trigger(AutoAim::targetInTurretDeadzone).negate());
}

private void addTransitions() {
Expand Down Expand Up @@ -345,7 +345,7 @@ private void addTransitions() {
bindTransition(
SuperState.SCORE_FLOW,
SuperState.SPIN_UP_SCORE_FLOW,
new Trigger(NewAutoAim::targetInTurretDeadzone));
new Trigger(AutoAim::targetInTurretDeadzone));

bindTransition(
SuperState.SPIN_UP_SCORE_FLOW,
Expand Down Expand Up @@ -385,7 +385,7 @@ private void addTransitions() {
bindTransition(
SuperState.FEED_FLOW,
SuperState.SPIN_UP_FEED_FLOW,
new Trigger(NewAutoAim::targetInTurretDeadzone));
new Trigger(AutoAim::targetInTurretDeadzone));

bindTransition(
SuperState.SPIN_UP_FEED_FLOW, SuperState.IDLE, intakeReq.negate().and(shootReq.negate()));
Expand Down Expand Up @@ -488,36 +488,32 @@ private void addCommands() {
indexer.rest(),
shooter.feed(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE),
() -> FeedTargets.getFeedTarget(feedTarget).getPose(),
swerve::getPose),
ShotTrees.FEED_SHOT_TREE)),
climber.retract());

bindCommands(
SuperState.FEED,
intake.agitate(),
indexer.kick(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE)
ShotTrees.FEED_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter.feed(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE),
() -> FeedTargets.getFeedTarget(feedTarget).getPose(),
swerve::getPose),
ShotTrees.FEED_SHOT_TREE)),
climber.retract());

bindCommands(
Expand All @@ -526,36 +522,32 @@ private void addCommands() {
indexer.index(),
shooter.feed(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE),
() -> FeedTargets.getFeedTarget(feedTarget).getPose(),
swerve::getPose),
ShotTrees.FEED_SHOT_TREE)),
climber.retract());

bindCommands(
SuperState.FEED_FLOW,
intake.intake(),
indexer.kick(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE)
ShotTrees.FEED_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter.feed(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FeedTargets.getFeedTarget(feedTarget).getTranslation(),
AutoAim.FEED_SHOT_TREE),
() -> FeedTargets.getFeedTarget(feedTarget).getPose(),
swerve::getPose),
ShotTrees.FEED_SHOT_TREE)),
climber.retract());

bindCommands(
Expand All @@ -564,13 +556,13 @@ private void addCommands() {
indexer.rest(),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.retract());

bindCommands(
Expand All @@ -579,24 +571,24 @@ private void addCommands() {
// intake.restExtended(),
indexer.kick(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.retract());

bindCommands(
Expand All @@ -605,38 +597,38 @@ private void addCommands() {
indexer.rest(),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.retract());

bindCommands(
SuperState.SCORE_FLOW,
intake.intake(),
indexer.kick(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.retract());

bindCommands(
Expand Down Expand Up @@ -678,38 +670,38 @@ private void addCommands() {
indexer.rest(),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.extend());

bindCommands(
SuperState.SCORE_PRE_CLIMB,
intake.restRetracted(),
indexer.kick(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)
.shotData()
.flywheelVelocityRotPerSec()),
shooter.score(
() ->
NewAutoAim.getParametersMechA(
AutoAim.getShotParameters(
swerve.getPose(),
swerve.getVelocityRobotRelative(),
FieldUtils.getCurrentHubTranslation(),
Robot.ROBOT_EDITION == RobotEdition.ALPHA
? AutoAim.ALPHA_HUB_SHOT_TREE
: AutoAim.COMP_HUB_SHOT_TREE)),
? ShotTrees.ALPHA_HUB_SHOT_TREE
: ShotTrees.COMP_HUB_SHOT_TREE)),
climber.extend());

bindCommands(
Expand Down
7 changes: 2 additions & 5 deletions src/main/java/frc/robot/subsystems/shooter/Shooter.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.utils.autoaim.NewAutoAim.ShotParams;
import frc.robot.utils.autoaim.AutoAim.ShotParams;
import java.util.function.BooleanSupplier;
import java.util.function.Supplier;

Expand All @@ -27,10 +27,7 @@ public interface Shooter extends Subsystem {
* Sets hood angle and flywheel velocity based on distance from hub from the feed map + current
* pose + feed target
*/
public Command feed(
Supplier<ShotParams> shotParamsSupplier,
Supplier<Pose2d> feedTarget,
Supplier<Pose2d> robotPoseSupplier);
public Command feed(Supplier<ShotParams> shotParamsSupplier);

/** Not running (set to 0) */
public Command rest(
Expand Down
Loading
Loading