Skip to content

3 21 shot retuning #70

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
Mar 22, 2026
Merged

3 21 shot retuning #70

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
43d20ed
retune shots yay
spellingcat Mar 21, 2026
9db8a85
turn off test shots
spellingcat Mar 21, 2026
149b9d7
fmt?
spellingcat Mar 21, 2026
f523e97
retune shots again
spellingcat Mar 22, 2026
65803b9
Merge branch 'main' into 3-21-shot-retuning
spellingcat Mar 22, 2026
8ad1e66
fmt i think
spellingcat Mar 22, 2026
0b7a21c
update vendordeps
spellingcat Mar 22, 2026
efc0748
fix gear ratios and redo ff
spellingcat Mar 22, 2026
e59f3c0
speed up flywheels a smidge
spellingcat Mar 22, 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
13 changes: 12 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@
import frc.robot.subsystems.swerve.SwerveSubsystem;
import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread;
import frc.robot.utils.CommandXboxControllerSubsystem;
import frc.robot.utils.FieldUtils;
import frc.robot.utils.FieldUtils.ClimbTargets;
import frc.robot.utils.FieldUtils.FeedTargets;
import frc.robot.utils.FieldUtils.TrenchPoses;
Expand Down Expand Up @@ -149,7 +150,7 @@ public enum RobotEdition {
* This is for when we're testing shot and extension numbers and should be FALSE once bring up is
* complete
*/
public static final boolean TUNING_MODE = false;
public static final boolean TUNING_MODE = true;

public boolean hasZeroedSinceStartup = false;

Expand Down Expand Up @@ -762,6 +763,9 @@ private void addAutos() {
autoChooser.addOption("Right Bump Outpost Climb", autos.getRightBumpOutpostClimbAuto());
autoChooser.addOption("Right Bump Outpost Center", autos.getRightBumpOutpostCenterAuto());

autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
autoChooser.addOption("Hood Sysid", shooter.runHoodSysid());

haveAutosGenerated = true;
System.out.println("Done generating autos");
}
Expand Down Expand Up @@ -849,6 +853,13 @@ public void robotPeriodic() {
Logger.recordOutput("Turret/out of range", AutoAim.targetInTurretDeadzone());

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

Logger.recordOutput(
"Distance to hub",
shooter
.getTurretPose(swerve.getPose())
.getTranslation()
.getDistance(FieldUtils.getCurrentHubTranslation()));
}

public void updateAlerts() {
Expand Down
2 changes: 2 additions & 0 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 @@ -531,6 +531,7 @@ private void addCommands() {
intake.restExtended(),
indexer.rest(),
shooter.score(
// shooter.testShot(
swerve::getPose,
() ->
AutoAim.getCompensatedSOTMShotData(
Expand Down Expand Up @@ -558,6 +559,7 @@ private void addCommands() {
: AutoAim.COMP_HUB_SHOT_TREE)
.flywheelVelocityRotPerSec()),
shooter.score(
// shooter.testShot(
swerve::getPose,
() ->
AutoAim.getCompensatedSOTMShotData(
Expand Down
13 changes: 13 additions & 0 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 @@ -96,4 +96,17 @@ public default Command currentZeroTurretAgainstForwardHardstop() {
public default Command stopTurret() {
return Commands.none();
}

public default Command testShot(
Supplier<Pose2d> robotPoseSupplier, Supplier<ChassisSpeeds> chassisSpeedsSupplier) {
return Commands.none();
}

public default Command runFlywheelSysid() {
return Commands.none();
}

public default Command runHoodSysid() {
return Commands.none();
}
}
97 changes: 89 additions & 8 deletions src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@

package frc.robot.subsystems.shooter;

import static edu.wpi.first.units.Units.Volts;

import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.signals.GravityTypeValue;
Expand All @@ -23,9 +25,14 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
import frc.robot.components.cancoder.CANcoderIO;
import frc.robot.components.cancoder.CANcoderIOInputsAutoLogged;
import frc.robot.utils.FieldUtils;
import frc.robot.utils.LoggedTunableNumber;
import frc.robot.utils.autoaim.AutoAim;
import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData;
import java.util.function.BooleanSupplier;
Expand All @@ -39,10 +46,10 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {
/** Creates a new TurretSubsystem. */
public static final double HOOD_GEAR_RATIO = 33.8671875; // 58.96875;

public static final double FLYWHEEL_GEAR_RATIO = 0.84615384615;
public static final double FLYWHEEL_GEAR_RATIO = 20.0 / 18.0; // 0.84615384615;

public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(73);
public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(23.16);
public static final Rotation2d HOOD_MAX_ANGLE = Rotation2d.fromDegrees(56);
public static final Rotation2d HOOD_MIN_ANGLE = Rotation2d.fromDegrees(11.33);
public static final double HOOD_CURRENT_ZERO_THRESHOLD = 30.0;
public static final double TURRET_CURRENT_ZERO_THRESHOLD = 30.0; // TODO find

Expand Down Expand Up @@ -96,6 +103,24 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {

private LinearFilter currentFilter = LinearFilter.movingAverage(10);

private SysIdRoutine hoodSysid =
new SysIdRoutine(
new Config(
null,
Volts.of(5),
null,
(state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())),
new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this));

private SysIdRoutine flywheelSysid =
new SysIdRoutine(
new Config(
null,
null,
null,
(state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())),
new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this));

private static final Alert cancoder24tDisconnectedAlert =
new Alert("24T Cancoder disconnected!", AlertType.kError);
private static final Alert cancoder26tDisconnectedAlert =
Expand All @@ -113,6 +138,10 @@ public class TurretSubsystem extends SubsystemBase implements Shooter {
"Turret may have gone past hardstop!! Reoffset cancoders + min/max position",
AlertType.kError);

private LoggedTunableNumber testDegrees =
new LoggedTunableNumber("Shooter/Test Hood Degrees", 50.0);
private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 50.0);

public TurretSubsystem(
FlywheelIO flywheelIO,
HoodIO hoodIO,
Expand Down Expand Up @@ -198,6 +227,20 @@ public Command feed(
});
}

public Command testShot(
Supplier<Pose2d> robotPoseSupplier, Supplier<ChassisSpeeds> chassisSpeedsSupplier) {
return this.run(
() -> {
hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get()));
flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get());
turretIO.setTurretPosition(
AutoAim.getTurretHubTargetRotation(
FieldUtils.getCurrentHubTranslation(),
robotPoseSupplier.get(),
chassisSpeedsSupplier.get()));
});
}

@Override
public Command rest(
Supplier<Pose2d> robotPoseSupplier,
Expand Down Expand Up @@ -446,10 +489,10 @@ public static TalonFXConfiguration getFlywheelConfig() {
config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO;

// slot 0 is for motion profiled velocity
config.Slot0.kS = 0.79522; // 0.63933;
config.Slot0.kV = 0.11087; // 0.11582;
config.Slot0.kA = 0.026101; // 0.020809;
config.Slot0.kP = 0.6;
config.Slot0.kS = 0.33706; // 0.63933;
config.Slot0.kV = 0.13893; // 0.11582;
config.Slot0.kA = 0.030026; // 0.020809;
config.Slot0.kP = 0.4;
config.Slot0.kD = 0;

// slot 1 is for torque current
Expand Down Expand Up @@ -480,7 +523,7 @@ public static TalonFXConfiguration getHoodConfig() {
config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

config.Slot0.kS = 0.57613;
config.Slot0.kG = 0.35748;
config.Slot0.kG = 0.55748;
config.Slot0.kV = 5.4081;
config.Slot0.kA = 0.14829;
config.Slot0.kP = 260.0;
Expand Down Expand Up @@ -538,4 +581,42 @@ public static CANcoderConfiguration getCancoder26tConfigs() {

return config;
}

@Override
public Command runHoodSysid() {
return Commands.sequence(
hoodSysid
.quasistatic(Direction.kForward)
.until(
() ->
hoodInputs.hoodPositionRotations.getDegrees()
> (HOOD_MAX_ANGLE.getDegrees() - 5)), // Stop before endstop
hoodSysid
.quasistatic(Direction.kReverse)
.until(
() ->
hoodInputs.hoodPositionRotations.getDegrees()
< (HOOD_MIN_ANGLE.getDegrees() + 5)),
hoodSysid
.dynamic(Direction.kForward)
.until(
() ->
hoodInputs.hoodPositionRotations.getDegrees()
> (HOOD_MAX_ANGLE.getDegrees() - 5)),
hoodSysid
.dynamic(Direction.kReverse)
.until(
() ->
hoodInputs.hoodPositionRotations.getDegrees()
< (HOOD_MIN_ANGLE.getDegrees() + 5)));
}

@Override
public Command runFlywheelSysid() {
return Commands.sequence(
flywheelSysid.quasistatic(Direction.kForward),
flywheelSysid.quasistatic(Direction.kReverse),
flywheelSysid.dynamic(Direction.kForward),
flywheelSysid.dynamic(Direction.kReverse));
}
}
128 changes: 74 additions & 54 deletions src/main/java/frc/robot/utils/autoaim/AutoAim.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,77 +58,97 @@ public class AutoAim {

public static final InterpolatingShotTree COMP_HUB_SHOT_TREE = new InterpolatingShotTree();

// TODO update tof
static { // For hub shot tree
// TODO min shot
static {
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 + 17),
new ShotData(TurretSubsystem.HOOD_MIN_ANGLE, 40 - 6 + 3, 1.04));

1.716849,
new ShotData(
Rotation2d.fromDegrees(23 - 13.16),
30 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
0.8));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12),
new ShotData(Rotation2d.fromDegrees(25), 35 - 6 + 3, 1.14));

2.017596,
new ShotData(
Rotation2d.fromDegrees(23 - 13.16),
33 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
0.9));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12),
2.423868,
new ShotData(
Rotation2d.fromDegrees(26),
37 - 6 + 3
// - 6
,
1.10));

Rotation2d.fromDegrees(25 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.1));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12),
2.664198,
new ShotData(
Rotation2d.fromDegrees(30),
37 - 6 + 3
// - 6
,
1.09));

Rotation2d.fromDegrees(26 - 13.16),
36 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.2));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12),
2.903207,
new ShotData(
Rotation2d.fromDegrees(33),
37 - 6 + 3
// - 6
,
1.15));

Rotation2d.fromDegrees(30 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.2));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12),
3.156802,
new ShotData(
Rotation2d.fromDegrees(36),
38 - 6 + 3
// - 6
,
Rotation2d.fromDegrees(32 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.23));

COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12),
3.437033,
new ShotData(
Rotation2d.fromDegrees(38),
39 - 6 + 3
// - 6
,
1.33));
Rotation2d.fromDegrees(34 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.25));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12),
3.611052,
new ShotData(
Rotation2d.fromDegrees(38 - 13.16),
34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.24));
COMP_HUB_SHOT_TREE.put(
3.773999,
new ShotData(
Rotation2d.fromDegrees(39 - 13.16),
34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.21));
COMP_HUB_SHOT_TREE.put(
3.899275,
new ShotData(
Rotation2d.fromDegrees(40 - 13.16),
34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.2));
COMP_HUB_SHOT_TREE.put(
4.138058,
new ShotData(
Rotation2d.fromDegrees(41 - 13.16),
34 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.13));
COMP_HUB_SHOT_TREE.put(
4.602258,
new ShotData(
Rotation2d.fromDegrees(43 - 13.16),
34.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.15));
COMP_HUB_SHOT_TREE.put(
4.893493,
new ShotData(
Rotation2d.fromDegrees(45 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.2));
COMP_HUB_SHOT_TREE.put(
5.225402,
new ShotData(
Rotation2d.fromDegrees(39),
40.5 - 6 + 3
// - 6
,
1.35));
Rotation2d.fromDegrees(47 - 13.16),
35 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.2));
COMP_HUB_SHOT_TREE.put(
Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12 + 6),
5.584793,
new ShotData(
Rotation2d.fromDegrees(39),
40.5 - 6 + 3 + 2
// - 6
,
1.35));
Rotation2d.fromDegrees(49 - 13.16),
35.5 * 0.84615384615 / TurretSubsystem.FLYWHEEL_GEAR_RATIO + 1,
1.17));
}

// Ig we'll see if we need more than 1 feed shot tree
Expand Down
Loading
Loading