From 9939ad7717c232a5a1e5bdbb9c3728d3d4f88af3 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 13:35:40 -0800 Subject: [PATCH 01/19] intake bringup --- .../robot/subsystems/intake/IntakeSubsystem.java | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 89ab34d..d4311d9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -36,11 +36,11 @@ public void periodic() { // TODO get actual values public Command intake() { - return this.run(() -> io.setRollerVoltage(5)); + return this.run(() -> io.setRollerVoltage(8)); } public Command outake() { - return this.run(() -> io.setRollerVoltage(-2)); + return this.run(() -> io.setRollerVoltage(-5)); } public Command rest() { @@ -62,14 +62,13 @@ public static TalonFXConfiguration getIntakeConfig() { config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kS = 0.24; - config.Slot0.kV = 0.6; - config.Slot0.kP = 110.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.42; + config.Slot0.kV = 0.21; + config.Slot0.kA = 0.00347; - config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; return config; From 8165e4782ac041fe82da4629fdc56d338ced7ed8 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 13:37:34 -0800 Subject: [PATCH 02/19] add to string in order for sysid to recognize the routine names --- .../java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 4 ++-- .../java/frc/robot/subsystems/intake/IntakeSubsystem.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index af761a4..c2dce3d 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -39,7 +39,7 @@ public class IndexerSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state)), + (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())), new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this)); public static final double MAX_ACCELERATION = 10.0; @@ -166,7 +166,7 @@ public void periodic() { indexRollerIO.updateInputs(rollerInputs); Logger.processInputs("Indexer/Roller", rollerInputs); kickerIO.updateInputs(kickerInputs); - Logger.processInputs("Intake/Kicker", kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); } public Command runRollerSysId() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index d4311d9..d859644 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -22,7 +22,7 @@ public class IntakeSubsystem extends SubsystemBase { private SysIdRoutine intakeRollerSysid = new SysIdRoutine( - new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state)), + new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this)); public IntakeSubsystem(RollerIO io) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9cfde0a..af0da12 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -39,7 +39,7 @@ public class ShooterSubsystem extends SubsystemBase { private SysIdRoutine hoodSysid = new SysIdRoutine( new Config( - null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state)), + null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); private SysIdRoutine flywheelSysid = @@ -48,7 +48,7 @@ public class ShooterSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state)), + (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); /** Creates a new HoodSubsystem. */ From 461e8b7e1ea8a0a7fec7071d0331c2479a8f6a99 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 14:23:32 -0800 Subject: [PATCH 03/19] log rotations for sysid --- .../java/frc/robot/components/rollers/RollerIO.java | 11 +++++++++-- .../frc/robot/subsystems/intake/IntakeSubsystem.java | 6 +++++- .../robot/subsystems/shooter/ShooterSubsystem.java | 5 ++++- 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/components/rollers/RollerIO.java b/src/main/java/frc/robot/components/rollers/RollerIO.java index 1575259..46cea56 100644 --- a/src/main/java/frc/robot/components/rollers/RollerIO.java +++ b/src/main/java/frc/robot/components/rollers/RollerIO.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; @@ -23,6 +24,7 @@ public static class RollerIOInputs { public double appliedVoltage = 0.0; public double statorCurrentAmps = 0.0; public double motorTemperatureCelsius = 0.0; + public double motorPositionRotations = 0.0; } protected final TalonFX motor; @@ -32,6 +34,7 @@ public static class RollerIOInputs { private final StatusSignal appliedVoltage; private final StatusSignal statorCurrentAmps; private final StatusSignal motorTemperatureCelsius; + private final StatusSignal motorPosition; private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage velocityVoltage = @@ -47,6 +50,7 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { appliedVoltage = motor.getMotorVoltage(); statorCurrentAmps = motor.getStatorCurrent(); motorTemperatureCelsius = motor.getDeviceTemp(); + motorPosition = motor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, @@ -54,7 +58,8 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { supplyCurrentAmps, statorCurrentAmps, appliedVoltage, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); motor.getConfigurator().apply(config); motor.optimizeBusUtilization(); @@ -66,13 +71,15 @@ public void updateInputs(RollerIOInputs inputs) { supplyCurrentAmps, appliedVoltage, statorCurrentAmps, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); inputs.velocityRotsPerSec = angularVelocityRotsPerSec.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.motorTemperatureCelsius = motorTemperatureCelsius.getValueAsDouble(); + inputs.motorPositionRotations = motorPosition.getValueAsDouble(); } public void setRollerVoltage(double volts) { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index d859644..7b8c5ff 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -22,7 +22,11 @@ public class IntakeSubsystem extends SubsystemBase { private SysIdRoutine intakeRollerSysid = new SysIdRoutine( - new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this)); public IntakeSubsystem(RollerIO io) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index af0da12..b215f52 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -39,7 +39,10 @@ public class ShooterSubsystem extends SubsystemBase { private SysIdRoutine hoodSysid = new SysIdRoutine( new Config( - null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), + null, + null, + null, + (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); private SysIdRoutine flywheelSysid = From 459d8cc8027510a0e382b620619ec9a3c0a30f1c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 14:50:04 -0800 Subject: [PATCH 04/19] tune hood --- .../frc/robot/subsystems/shooter/HoodIO.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index e852e44..b4a5f23 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -14,9 +14,11 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; @@ -32,11 +34,13 @@ public static class HoodIOInputs { public double hoodSupplyCurrentAmp = 0.0; public double hoodVoltage = 0.0; public double hoodTempC = 0.0; + // For sysid + public double hoodRotations = 0.0; } protected TalonFX hoodMotor; - private final BaseStatusSignal hoodPositionRotations; + private final StatusSignal hoodPositionRotations; private final BaseStatusSignal hoodAngularVelocity; private final StatusSignal hoodVoltage; private final StatusSignal hoodStatorCurrent; @@ -81,13 +85,13 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; - // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.kS = 0.0; - config.Slot0.kG = 0.0; - config.Slot0.kV = 1.1; - config.Slot0.kP = 5.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.055; + config.Slot0.kG = 0.445; + config.Slot0.kV = 1.45; + config.Slot0.kP = 35; + config.Slot0.kD = 0.25; config.CurrentLimits.StatorCurrentLimit = 80.0; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -123,5 +127,6 @@ public void updateInputs(HoodIOInputs inputs) { inputs.hoodStatorCurrentAmps = hoodStatorCurrent.getValueAsDouble(); inputs.hoodSupplyCurrentAmp = hoodSupplyCurrent.getValueAsDouble(); inputs.hoodTempC = hoodTemp.getValueAsDouble(); + inputs.hoodRotations = hoodPositionRotations.getValueAsDouble(); } } From ad924dfbf66044c6615283524dc412bb31bfb107 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 15:02:44 -0800 Subject: [PATCH 05/19] tune flywheel --- .../robot/subsystems/shooter/FlywheelIO.java | 19 ++++++++++++++----- .../frc/robot/subsystems/shooter/HoodIO.java | 4 ++-- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index ac355ed..acc16c2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; @@ -31,6 +32,7 @@ public static class FlywheelIOInputs { public double flywheelLeaderSupplyCurrentAmp = 0.0; public double flywheelLeaderVoltage = 0.0; public double flywheelLeaderTempC = 0.0; + public double flywheelLeaderPosition = 0.0; public double flywheelFollowerVelocityRotationsPerSecond = 0.0; public double flywheelFollowerStatorCurrentAmps = 0.0; @@ -47,6 +49,7 @@ public static class FlywheelIOInputs { private final StatusSignal flywheelLeaderStatorCurrent; private final StatusSignal flywheelLeaderSupplyCurrent; private final StatusSignal flywheelLeaderTemp; + private final StatusSignal flywheelLeaderPosition; private final BaseStatusSignal flywheelFollowerVelocity; private final StatusSignal flywheelFollowerVoltage; @@ -79,6 +82,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelLeaderStatorCurrent = flywheelLeader.getStatorCurrent(); flywheelLeaderSupplyCurrent = flywheelLeader.getSupplyCurrent(); flywheelLeaderTemp = flywheelLeader.getDeviceTemp(); + flywheelLeaderPosition = flywheelLeader.getPosition(); flywheelFollowerVelocity = flywheelFollower.getVelocity(); flywheelFollowerVoltage = flywheelFollower.getMotorVoltage(); @@ -97,7 +101,8 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.getMotorVoltage(), flywheelFollower.getStatorCurrent(), flywheelFollower.getSupplyCurrent(), - flywheelFollower.getDeviceTemp()); + flywheelFollower.getDeviceTemp(), + flywheelLeaderPosition); flywheelLeader.optimizeBusUtilization(); flywheelFollower.optimizeBusUtilization(); @@ -111,9 +116,10 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; - config.Slot0.kS = 0.0; - config.Slot0.kV = 0.15; // From sim - config.Slot0.kP = 10.0; + config.Slot0.kS = 0.43477; + config.Slot0.kV = 0.144; // From sim + config.Slot0.kA = 0.016433; + config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 120.0; @@ -148,7 +154,8 @@ public void updateInputs(FlywheelIOInputs inputs) { flywheelFollowerVoltage, flywheelFollowerStatorCurrent, flywheelFollowerSupplyCurrent, - flywheelFollowerTemp); + flywheelFollowerTemp, + flywheelLeaderPosition); inputs.flywheelLeaderVelocityRotationsPerSecond = flywheelLeaderVelocity.getValueAsDouble(); inputs.flywheelLeaderVoltage = flywheelLeaderVoltage.getValueAsDouble(); @@ -156,6 +163,8 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelLeaderSupplyCurrentAmp = flywheelLeaderSupplyCurrent.getValueAsDouble(); inputs.flywheelLeaderTempC = flywheelLeaderTemp.getValueAsDouble(); + inputs.flywheelLeaderPosition = flywheelLeaderPosition.getValueAsDouble(); + inputs.flywheelFollowerVelocityRotationsPerSecond = flywheelFollowerVelocity.getValueAsDouble(); inputs.flywheelFollowerVoltage = flywheelFollowerVoltage.getValueAsDouble(); inputs.flywheelFollowerStatorCurrentAmps = flywheelFollowerStatorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index b4a5f23..a983f1f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -85,11 +85,11 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; - config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; config.Slot0.kS = 0.055; config.Slot0.kG = 0.445; - config.Slot0.kV = 1.45; + config.Slot0.kV = 1.45; config.Slot0.kP = 35; config.Slot0.kD = 0.25; From 423e7be63037472512969c46dd1e81c32d5d1bc9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 15:39:57 -0800 Subject: [PATCH 06/19] tune canranges and stuff --- .vscode/settings.json | 3 ++- src/main/java/frc/robot/Robot.java | 1 + .../frc/robot/components/canrange/CANrangeIOReal.java | 5 +++-- .../frc/robot/subsystems/indexer/IndexerSubsystem.java | 10 +++++----- .../frc/robot/subsystems/intake/IntakeSubsystem.java | 2 +- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 4 ++++ src/main/java/frc/robot/utils/autoaim/AutoAim.java | 2 +- 7 files changed, 17 insertions(+), 10 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 7399d64..50e3d8c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,5 +59,6 @@ ], "java.dependency.enableDependencyCheckup": false, "wpilib.selectDefaultSimulateExtension": false, - "wpilib.skipSelectSimulateExtension": true + "wpilib.skipSelectSimulateExtension": true, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 036f351..376334c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -316,6 +316,7 @@ public Robot() { .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); + SmartDashboard.putData("Zero hood", shooter.zeroHood()); // Reset alert timers canInitialErrorTimer.restart(); diff --git a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java index 0548179..343ed4f 100644 --- a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java +++ b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java @@ -17,14 +17,15 @@ public class CANrangeIOReal implements CANrangeIO { private final StatusSignal distance; private final StatusSignal isDetected; - public CANrangeIOReal(int CANrangeID, CANBus canbus) { + public CANrangeIOReal(int CANrangeID, CANBus canbus, double xFovDegrees) { canrange = new CANrange(CANrangeID, canbus); distance = canrange.getDistance(); isDetected = canrange.getIsDetected(); final CANrangeConfiguration config = new CANrangeConfiguration(); config.ToFParams.UpdateFrequency = 50; // update frequency in Hz - config.ProximityParams.ProximityThreshold = 0.05; + config.ProximityParams.ProximityThreshold = 0.254; + config.FovParams.FOVRangeX = xFovDegrees; BaseStatusSignal.setUpdateFrequencyForAll(50.0, distance, isDetected); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index c2dce3d..bb0fea1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -48,8 +48,8 @@ public class IndexerSubsystem extends SubsystemBase { public IndexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; - firstCANRangeIO = new CANrangeIOReal(0, canbus); - secondCANRangeIO = new CANrangeIOReal(1, canbus); + firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); + secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); this.indexRollerIO = indexRollerIO; } @@ -77,8 +77,8 @@ public boolean isPartiallyFull() { public Command index() { return this.run( () -> { - indexRollerIO.setRollerVoltage(5); - kickerIO.setRollerVoltage(-5); + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(-7); }); } @@ -111,7 +111,7 @@ public static TalonFXConfiguration getIndexerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 7b8c5ff..e159e95 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -40,7 +40,7 @@ public void periodic() { // TODO get actual values public Command intake() { - return this.run(() -> io.setRollerVoltage(8)); + return this.run(() -> io.setRollerVoltage(10)); } public Command outake() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b215f52..45ff3c9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -155,4 +155,8 @@ public boolean atFlywheelVelocitySetpoint() { flywheelIO.getSetpointRotPerSec(), FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } + + public Command zeroHood() { + return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(2))); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 1426b50..3bd1c20 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -14,7 +14,7 @@ public class AutoAim { static { // For hub shot tree // TODO: ADD SHOTS TO HUB SHOT HERE HUB_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + 1.0, new ShotData(Rotation2d.fromDegrees(20), 10, 0.5)); // Placeholder to prevent crashes } // Ig we'll see if we need more than 1 feed shot tree From 42acd6233f6002eb3f46e5c559062c4269fe3453 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Mon, 19 Jan 2026 16:17:16 -0800 Subject: [PATCH 07/19] logged tunable numbrs and motion profile --- src/main/java/frc/robot/Superstructure.java | 4 ++-- .../subsystems/indexer/IndexerSubsystem.java | 18 ++++++++++-------- .../subsystems/intake/IntakeSubsystem.java | 6 ++++-- .../robot/subsystems/shooter/FlywheelIO.java | 12 ++++++++++-- .../subsystems/shooter/ShooterSubsystem.java | 8 ++++++++ 5 files changed, 34 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9be6815..b3629d0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -220,7 +220,7 @@ private void addCommands() { swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC bindCommands( - SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); + SuperState.SCORE, intake.rest(), indexer.kick(), shooter.shoot(swerve::getPose)); bindCommands( SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); @@ -239,7 +239,7 @@ private void addCommands() { indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); - bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); + bindCommands(SuperState.SPIT, intake.outake(), indexer.spit(), shooter.spit()); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index bb0fea1..b05fdea 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -15,6 +15,8 @@ import frc.robot.components.canrange.CANrangeIOReal; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import frc.robot.utils.LoggedTunableNumber; + import org.littletonrobotics.junction.Logger; public class IndexerSubsystem extends SubsystemBase { @@ -77,24 +79,24 @@ public boolean isPartiallyFull() { public Command index() { return this.run( () -> { - indexRollerIO.setRollerVoltage(7); - kickerIO.setRollerVoltage(-7); + indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Index", 7).get()); + kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Index", -7).get()); }); } - public Command indexToShoot() { + public Command kick() { return this.run( () -> { - indexRollerIO.setRollerVoltage(10); - kickerIO.setRollerVoltage(5); + indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Kick", 10).get()); + kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Kick", 5).get()); }); } - public Command outtake() { + public Command spit() { return this.run( () -> { - indexRollerIO.setRollerVoltage(-5); - kickerIO.setRollerVoltage(-5); + indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Spit", -5).get()); + kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Spit", -5).get()); }); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index e159e95..dd02c22 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.*; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import frc.robot.utils.LoggedTunableNumber; + import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends SubsystemBase { @@ -40,11 +42,11 @@ public void periodic() { // TODO get actual values public Command intake() { - return this.run(() -> io.setRollerVoltage(10)); + return this.run(() -> io.setRollerVoltage(new LoggedTunableNumber("Intake/Intake", 10).get())); } public Command outake() { - return this.run(() -> io.setRollerVoltage(-5)); + return this.run(() -> io.setRollerVoltage(new LoggedTunableNumber("Intake/Spit", -5).get())); } public Command rest() { diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index acc16c2..7ae9c02 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -59,8 +60,7 @@ public static class FlywheelIOInputs { private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - private MotionMagicVelocityVoltage motionMagicVelocityVoltage = - new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withAcceleration(100); + private MotionMagicVelocityVoltage motionMagicVelocityVoltage; private double velocitySetpointRotPerSec = 0.0; @@ -90,6 +90,12 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollowerSupplyCurrent = flywheelFollower.getSupplyCurrent(); flywheelFollowerTemp = flywheelFollower.getDeviceTemp(); + motionMagicVelocityVoltage = + new MotionMagicVelocityVoltage( + 0.0).withAcceleration( + config.MotionMagic.MotionMagicAcceleration) + .withEnableFOC(true); + BaseStatusSignal.setUpdateFrequencyForAll( 50.0, flywheelLeader.getVelocity(), @@ -125,6 +131,8 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.CurrentLimits.StatorCurrentLimit = 120.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 80.0; + + config.MotionMagic.MotionMagicAcceleration = 100.0; return config; } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 45ff3c9..380f8a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -16,6 +16,7 @@ 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.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; @@ -60,6 +61,13 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; } + public Command testShoot() { + return this.run(() -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(new LoggedTunableNumber("Shooter/Test Degrees").get())); + flywheelIO.setMotionProfiledFlywheelVelocity(new LoggedTunableNumber("Shooter/Test Velocity").get()); + }); + } + public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { From a508a5ffcfd60c1a936ad26e2b7ef0f21aeb9adf Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 20 Jan 2026 16:39:45 -0800 Subject: [PATCH 08/19] Made the logged tunable numbers work for the shooter --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/Superstructure.java | 3 +-- .../robot/subsystems/indexer/IndexerSubsystem.java | 1 - .../frc/robot/subsystems/intake/IntakeSubsystem.java | 1 - .../frc/robot/subsystems/shooter/FlywheelIO.java | 8 +++----- .../robot/subsystems/shooter/ShooterSubsystem.java | 12 ++++++++---- 6 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 376334c..7c4e1e5 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -317,6 +317,7 @@ public Robot() { .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); SmartDashboard.putData("Zero hood", shooter.zeroHood()); + SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers canInitialErrorTimer.restart(); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index b3629d0..25494b6 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -219,8 +219,7 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC - bindCommands( - SuperState.SCORE, intake.rest(), indexer.kick(), shooter.shoot(swerve::getPose)); + bindCommands(SuperState.SCORE, intake.rest(), indexer.kick(), shooter.shoot(swerve::getPose)); bindCommands( SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index b05fdea..8246170 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -16,7 +16,6 @@ import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; import frc.robot.utils.LoggedTunableNumber; - import org.littletonrobotics.junction.Logger; public class IndexerSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index dd02c22..1f11162 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -13,7 +13,6 @@ import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; import frc.robot.utils.LoggedTunableNumber; - import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 7ae9c02..e910a1e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -8,7 +8,6 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; @@ -91,9 +90,8 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollowerTemp = flywheelFollower.getDeviceTemp(); motionMagicVelocityVoltage = - new MotionMagicVelocityVoltage( - 0.0).withAcceleration( - config.MotionMagic.MotionMagicAcceleration) + new MotionMagicVelocityVoltage(0.0) + .withAcceleration(config.MotionMagic.MotionMagicAcceleration) .withEnableFOC(true); BaseStatusSignal.setUpdateFrequencyForAll( @@ -131,7 +129,7 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.CurrentLimits.StatorCurrentLimit = 120.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 80.0; - + config.MotionMagic.MotionMagicAcceleration = 100.0; return config; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 380f8a8..070c065 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -55,6 +55,9 @@ public class ShooterSubsystem extends SubsystemBase { (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 0.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 0.0); + /** Creates a new HoodSubsystem. */ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.hoodIO = hoodIO; @@ -62,10 +65,11 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { } public Command testShoot() { - return this.run(() -> { - hoodIO.setHoodPosition(Rotation2d.fromDegrees(new LoggedTunableNumber("Shooter/Test Degrees").get())); - flywheelIO.setMotionProfiledFlywheelVelocity(new LoggedTunableNumber("Shooter/Test Velocity").get()); - }); + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + }); } public Command shoot(Supplier robotPoseSupplier) { From 1009e379b9d5271a92f8accff52052906f476cbd Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 20 Jan 2026 18:25:04 -0800 Subject: [PATCH 09/19] Testing at 10th st. Doesn't work :( --- src/main/java/frc/robot/Superstructure.java | 15 ++++++++++++--- .../subsystems/indexer/IndexerSubsystem.java | 2 +- .../frc/robot/subsystems/shooter/FlywheelIO.java | 2 ++ .../java/frc/robot/subsystems/shooter/HoodIO.java | 9 +++++++++ .../subsystems/shooter/ShooterSubsystem.java | 10 +++++++++- .../java/frc/robot/utils/autoaim/AutoAim.java | 6 +++--- 6 files changed, 36 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 25494b6..da7d6d2 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -153,7 +153,9 @@ private void addTransitions() { bindTransition( SuperState.SPIN_UP_SCORE, SuperState.SCORE, - new Trigger(shooter::atFlywheelVelocitySetpoint)); + new Trigger(shooter::atFlywheelVelocitySetpoint) + .and(() -> stateTimer.hasElapsed(0.2)) + .and(shooter::atHoodSetpoint)); bindTransition( SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint)); @@ -210,7 +212,10 @@ private void addCommands() { shooter.rest()); // Maybe index at slower speed? bindCommands( - SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); + SuperState.SPIN_UP_SCORE, + intake.rest(), + indexer.rest(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); bindCommands( SuperState.SPIN_UP_FEED, @@ -219,7 +224,11 @@ private void addCommands() { shooter.feed( swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC - bindCommands(SuperState.SCORE, intake.rest(), indexer.kick(), shooter.shoot(swerve::getPose)); + bindCommands( + SuperState.SCORE, + intake.rest(), + indexer.kick(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); bindCommands( SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 8246170..763f30b 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -79,7 +79,7 @@ public Command index() { return this.run( () -> { indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Index", 7).get()); - kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Index", -7).get()); + kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Index", 7).get()); }); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index e910a1e..9bb0e74 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -21,6 +21,7 @@ import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; /** Add your docs here. */ public class FlywheelIO { @@ -178,6 +179,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelFollowerTempC = flywheelFollowerTemp.getValueAsDouble(); } + @AutoLogOutput(key = "Shooter/Setpoint") public double getSetpointRotPerSec() { return velocitySetpointRotPerSec; } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index a983f1f..90075ec 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -23,6 +23,7 @@ import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; public class HoodIO { /** Creates a new HoodIOReal. */ @@ -50,6 +51,8 @@ public static class HoodIOInputs { private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + private Rotation2d hoodSetpoint = Rotation2d.kZero; + public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); @@ -105,6 +108,7 @@ public void setHoodVoltage(double hoodVoltage) { } public void setHoodPosition(Rotation2d hoodPosition) { + hoodSetpoint = hoodPosition; hoodMotor.setControl(positionVoltage.withPosition(hoodPosition.getRotations())); } @@ -129,4 +133,9 @@ public void updateInputs(HoodIOInputs inputs) { inputs.hoodTempC = hoodTemp.getValueAsDouble(); inputs.hoodRotations = hoodPositionRotations.getValueAsDouble(); } + + @AutoLogOutput(key = "Shooter/Hood/Setpoint") + public Rotation2d getHoodSetpoint() { + return hoodSetpoint; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 070c065..16dd18e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -20,10 +20,11 @@ import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class ShooterSubsystem extends SubsystemBase { - public static double HOOD_GEAR_RATIO = 147.0 / 13.0; + public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0); @@ -161,6 +162,7 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kReverse)); } + @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { return MathUtil.isNear( flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, @@ -168,6 +170,12 @@ public boolean atFlywheelVelocitySetpoint() { FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } + @AutoLogOutput(key = "Shooter/Hood/At Setpoint") + public boolean atHoodSetpoint() { + return MathUtil.isNear( + hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); + } + public Command zeroHood() { return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(2))); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 3bd1c20..05bf5cf 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; @@ -12,9 +13,8 @@ public class AutoAim { // If we need other shot trees (i.e. for feeding) we can put them here static { // For hub shot tree - // TODO: ADD SHOTS TO HUB SHOT HERE - HUB_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.fromDegrees(20), 10, 0.5)); // Placeholder to prevent crashes + // 6 feet from edge of hub + HUB_SHOT_TREE.put(Units.inchesToMeters(95), new ShotData(Rotation2d.fromDegrees(0), 0, 0)); } // Ig we'll see if we need more than 1 feed shot tree From fcab4f7dc9984365bf6f9470d8612d56bf818ac5 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 20 Jan 2026 22:39:27 -0800 Subject: [PATCH 10/19] Add new transition condition for SPIN_UP_FEED -> FEED as well --- src/main/java/frc/robot/Superstructure.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index da7d6d2..97f5297 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -158,7 +158,11 @@ private void addTransitions() { .and(shooter::atHoodSetpoint)); bindTransition( - SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint)); + SuperState.SPIN_UP_FEED, + SuperState.FEED, + new Trigger(shooter::atFlywheelVelocitySetpoint) + .and(() -> stateTimer.hasElapsed(0.2)) + .and(shooter::atHoodSetpoint)); bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); From fda189abcca8d14d5b7f811de5124b47eb50f7f6 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 13:41:34 -0800 Subject: [PATCH 11/19] fix hood unit conversion --- src/main/java/frc/robot/subsystems/shooter/HoodIO.java | 3 ++- .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 6 +++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 90075ec..b835889 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -125,7 +125,8 @@ public void updateInputs(HoodIOInputs inputs) { hoodSupplyCurrent, hoodTemp); - inputs.hoodPositionRotations = Rotation2d.fromRadians(hoodPositionRotations.getValueAsDouble()); + inputs.hoodPositionRotations = + Rotation2d.fromRotations(hoodPositionRotations.getValueAsDouble()); inputs.hoodAngularVelocity = hoodAngularVelocity.getValueAsDouble(); inputs.hoodVoltage = hoodVoltage.getValueAsDouble(); inputs.hoodStatorCurrentAmps = hoodStatorCurrent.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 16dd18e..327c32d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -56,7 +56,7 @@ public class ShooterSubsystem extends SubsystemBase { (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); - private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 0.0); + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 0.0); /** Creates a new HoodSubsystem. */ @@ -179,4 +179,8 @@ public boolean atHoodSetpoint() { public Command zeroHood() { return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(2))); } + + public Command ninety() { + return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(90))); + } } From 1540a37c0402862a1c32f78cc78e0cd589b92ac4 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 15:10:18 -0800 Subject: [PATCH 12/19] EYYYYY --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Superstructure.java | 9 ++++--- .../subsystems/indexer/IndexerSubsystem.java | 13 +++++---- .../subsystems/intake/IntakeSubsystem.java | 5 ++-- .../frc/robot/subsystems/shooter/HoodIO.java | 4 +++ .../subsystems/shooter/ShooterSubsystem.java | 10 +++---- .../java/frc/robot/utils/autoaim/AutoAim.java | 27 +++++++++++++++++-- .../utils/autoaim/InterpolatingShotTree.java | 6 ++--- 8 files changed, 48 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7c4e1e5..10928ec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -316,7 +316,7 @@ public Robot() { .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); - SmartDashboard.putData("Zero hood", shooter.zeroHood()); + SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 97f5297..2f93c14 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -154,8 +154,9 @@ private void addTransitions() { SuperState.SPIN_UP_SCORE, SuperState.SCORE, new Trigger(shooter::atFlywheelVelocitySetpoint) - .and(() -> stateTimer.hasElapsed(0.2)) - .and(shooter::atHoodSetpoint)); + .debounce(0.5) + .and(new Trigger(shooter::atHoodSetpoint).debounce(0.5)) + .and(() -> stateTimer.hasElapsed(0.5))); bindTransition( SuperState.SPIN_UP_FEED, @@ -166,7 +167,7 @@ private void addTransitions() { bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); - bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5)); // FEED_FLOW transitions { @@ -212,7 +213,7 @@ private void addCommands() { bindCommands( SuperState.READY, intake.rest(), - indexer.index(), + indexer.rest(), shooter.rest()); // Maybe index at slower speed? bindCommands( diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 763f30b..80d534a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -15,7 +15,6 @@ import frc.robot.components.canrange.CANrangeIOReal; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; -import frc.robot.utils.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class IndexerSubsystem extends SubsystemBase { @@ -78,24 +77,24 @@ public boolean isPartiallyFull() { public Command index() { return this.run( () -> { - indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Index", 7).get()); - kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Index", 7).get()); + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(7); }); } public Command kick() { return this.run( () -> { - indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Kick", 10).get()); - kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Kick", 5).get()); + indexRollerIO.setRollerVoltage(10); + kickerIO.setRollerVoltage(5); }); } public Command spit() { return this.run( () -> { - indexRollerIO.setRollerVoltage(new LoggedTunableNumber("Indexer Roller/Spit", -5).get()); - kickerIO.setRollerVoltage(new LoggedTunableNumber("Kicker/Spit", -5).get()); + indexRollerIO.setRollerVoltage(-5); + kickerIO.setRollerVoltage(-5); }); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 1f11162..e159e95 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.*; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; -import frc.robot.utils.LoggedTunableNumber; import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends SubsystemBase { @@ -41,11 +40,11 @@ public void periodic() { // TODO get actual values public Command intake() { - return this.run(() -> io.setRollerVoltage(new LoggedTunableNumber("Intake/Intake", 10).get())); + return this.run(() -> io.setRollerVoltage(10)); } public Command outake() { - return this.run(() -> io.setRollerVoltage(new LoggedTunableNumber("Intake/Spit", -5).get())); + return this.run(() -> io.setRollerVoltage(-5)); } public Command rest() { diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index b835889..3c38920 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -139,4 +139,8 @@ public void updateInputs(HoodIOInputs inputs) { public Rotation2d getHoodSetpoint() { return hoodSetpoint; } + + public void resetEncoder(Rotation2d rotations) { + hoodMotor.setPosition(rotations.getRotations()); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 327c32d..6d1cab7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -26,7 +26,7 @@ public class ShooterSubsystem extends SubsystemBase { public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); - public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0); + public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; @@ -100,7 +100,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar public Command rest() { return this.run( () -> { - hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); }); } @@ -177,10 +177,6 @@ public boolean atHoodSetpoint() { } public Command zeroHood() { - return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(2))); - } - - public Command ninety() { - return this.runOnce(() -> hoodIO.setHoodPosition(Rotation2d.fromDegrees(90))); + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 05bf5cf..9aa7c5f 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -14,7 +14,30 @@ public class AutoAim { static { // For hub shot tree // 6 feet from edge of hub - HUB_SHOT_TREE.put(Units.inchesToMeters(95), new ShotData(Rotation2d.fromDegrees(0), 0, 0)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 12), + new ShotData(Rotation2d.fromDegrees(6), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36), + new ShotData(Rotation2d.fromDegrees(8), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24), + new ShotData(Rotation2d.fromDegrees(12), 31)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24), + new ShotData(Rotation2d.fromDegrees(17), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24), + new ShotData(Rotation2d.fromDegrees(21), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24), + new ShotData(Rotation2d.fromDegrees(24), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24 + 24), + new ShotData(Rotation2d.fromDegrees(28), 30)); + + // 2sqrt2 ft + + // 6 deg 30 rps } // Ig we'll see if we need more than 1 feed shot tree @@ -23,7 +46,7 @@ public class AutoAim { static { // For feed shot tree // TODO: POPULATE FEED_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + 1.0, new ShotData(Rotation2d.kCW_90deg, 10)); // Placeholder to prevent crashes } // TODO: SOTM diff --git a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java index deaebd9..a9e90c0 100644 --- a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java +++ b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java @@ -5,8 +5,7 @@ import java.util.TreeMap; public class InterpolatingShotTree { - public record ShotData( - Rotation2d hoodAngle, double flywheelVelocityRotPerSec, double flightTimeSec) {} + public record ShotData(Rotation2d hoodAngle, double flywheelVelocityRotPerSec) {} private final TreeMap map = new TreeMap<>(); @@ -90,8 +89,7 @@ private ShotData interpolate(ShotData startValue, ShotData endValue, double t) { MathUtil.interpolate( startValue.hoodAngle().getRadians(), endValue.hoodAngle().getRadians(), t)), MathUtil.interpolate( - startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t), - MathUtil.interpolate(startValue.flightTimeSec(), endValue.flightTimeSec(), t)); + startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t)); } /** From 72346cf75500b94065637dad119678fb39ac2c48 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 15:36:26 -0800 Subject: [PATCH 13/19] make things go in the right direction --- src/main/java/frc/robot/Superstructure.java | 57 +++++++++++-------- .../subsystems/indexer/IndexerSubsystem.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 2 +- 3 files changed, 35 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2f93c14..54e9223 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -67,8 +67,8 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Feed Request") private Trigger feedReq; - @AutoLogOutput(key = "Superstructure/Flowstate Request") - private Trigger flowReq; + // @AutoLogOutput(key = "Superstructure/Flowstate Request") + // private Trigger flowReq; @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; @@ -127,7 +127,7 @@ private void addTriggers() { .and(() -> shouldFeed == true) .or(Autos.autoFeedReq); - flowReq = driver.leftTrigger().and(driver.rightTrigger()); + // flowReq = driver.leftTrigger().and(driver.rightTrigger()); antiJamReq = driver.a().or(operator.a()); @@ -137,14 +137,16 @@ private void addTriggers() { } private void addTransitions() { - bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); + bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq.and(scoreReq.negate())); bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); bindTransition( - SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); + SuperState.INTAKE, + SuperState.READY, + (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())).or(isFull)); - bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); + // bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); @@ -158,41 +160,46 @@ private void addTransitions() { .and(new Trigger(shooter::atHoodSetpoint).debounce(0.5)) .and(() -> stateTimer.hasElapsed(0.5))); - bindTransition( - SuperState.SPIN_UP_FEED, - SuperState.FEED, - new Trigger(shooter::atFlywheelVelocitySetpoint) - .and(() -> stateTimer.hasElapsed(0.2)) - .and(shooter::atHoodSetpoint)); + // bindTransition( + // SuperState.SPIN_UP_FEED, + // SuperState.FEED, + // new Trigger(shooter::atFlywheelVelocitySetpoint) + // .and(() -> stateTimer.hasElapsed(0.2)) + // .and(shooter::atHoodSetpoint)); - bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + // bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5)); // FEED_FLOW transitions - { - bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); + // { + // bindTransition(SuperState.FEED, SuperState.FEED_FLOW, intakeReq.and(feedReq)); - bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); + // bindTransition(SuperState.FEED_FLOW, SuperState.FEED, intakeReq.negate().and(feedReq)); - bindTransition( - SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + // bindTransition( + // SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); - // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); - } + // // No so sure about the end condition here. + // bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + // } // SCORE_FLOW transitions { - bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, scoreReq.and(intakeReq)); - bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); + bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, intakeReq.negate().and(scoreReq)); bindTransition( - SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + SuperState.SCORE_FLOW, + SuperState.READY, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.IDLE, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty)); } // Transition from any state to SPIT for anti jamming diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 80d534a..610b5dc 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -86,7 +86,7 @@ public Command kick() { return this.run( () -> { indexRollerIO.setRollerVoltage(10); - kickerIO.setRollerVoltage(5); + kickerIO.setRollerVoltage(-7); }); } @@ -136,7 +136,7 @@ public static TalonFXConfiguration getKickerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Converts angular motion to linear motion config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 6d1cab7..bd8ff9c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -57,7 +57,7 @@ public class ShooterSubsystem extends SubsystemBase { new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); - private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 0.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); /** Creates a new HoodSubsystem. */ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { From 52c2247d4936792d39d66e508997e3c8f70c5c53 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 15:50:30 -0800 Subject: [PATCH 14/19] i think we've hit flow state hell yeah --- src/main/java/frc/robot/Superstructure.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 54e9223..dd77aa8 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -169,7 +169,7 @@ private void addTransitions() { // bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); - bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5)); + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5).and(scoreReq.negate())); // FEED_FLOW transitions // { @@ -242,8 +242,7 @@ private void addCommands() { indexer.kick(), /*shooter.shoot(swerve::getPose)*/ shooter.testShoot()); - bindCommands( - SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); + bindCommands(SuperState.SCORE_FLOW, intake.intake(), indexer.kick(), shooter.testShoot()); bindCommands( SuperState.FEED, From 2221b9c4b1488e715a185fd449e62f812b167636 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 16:14:35 -0800 Subject: [PATCH 15/19] add debounce for indexer being full --- src/main/java/frc/robot/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index dd77aa8..03daaab 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -131,7 +131,7 @@ private void addTriggers() { antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(indexer::isFull); + isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune isEmpty = new Trigger(indexer::isEmpty); } From 8be44cbca866496fc33b499d818ef371fe8578bb Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 17:16:27 -0800 Subject: [PATCH 16/19] fix heading snap --- src/main/java/frc/robot/Superstructure.java | 4 ++-- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 03daaab..2a7a52a 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -227,7 +227,7 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), /*shooter.shoot(swerve::getPose)*/ - shooter.testShoot()); + shooter.shoot(swerve::getPose)); bindCommands( SuperState.SPIN_UP_FEED, @@ -240,7 +240,7 @@ private void addCommands() { SuperState.SCORE, intake.rest(), indexer.kick(), /*shooter.shoot(swerve::getPose)*/ - shooter.testShoot()); + shooter.shoot(swerve::getPose)); bindCommands(SuperState.SCORE_FLOW, intake.intake(), indexer.kick(), shooter.testShoot()); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 220fa62..424a206 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -263,6 +263,8 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update vision", this::updateVision); + + Logger.recordOutput("Current Hub Pose", FieldUtils.getCurrentHubPose()); }); } @@ -586,8 +588,11 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { () -> { Translation2d robotHubVec = FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); + // return FieldUtils.getCurrentHubPose().minus(getPose()).getRotation(); + // Logger.recordOutput("robot hub vec", robotHubVec); // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) - return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())); + return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())) + .plus(Rotation2d.kCW_90deg); }, xVel, yVel); From e658830bc36791ec1f551a56928e936f473a6e9c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 17:30:07 -0800 Subject: [PATCH 17/19] get rid of is full --- src/main/java/frc/robot/Superstructure.java | 6 ++++-- .../java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 2 +- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 5 ++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2a7a52a..9c6913e 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -144,11 +144,13 @@ private void addTransitions() { bindTransition( SuperState.INTAKE, SuperState.READY, - (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())).or(isFull)); + (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate()))); + // .or(isFull)); // bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); - bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); + bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq); + // .and(isFull.negate())); bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 610b5dc..1e8eebf 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -85,7 +85,7 @@ public Command index() { public Command kick() { return this.run( () -> { - indexRollerIO.setRollerVoltage(10); + indexRollerIO.setRollerVoltage(12); kickerIO.setRollerVoltage(-7); }); } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 9aa7c5f..384fb6f 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import org.littletonrobotics.junction.Logger; public class AutoAim { @@ -52,6 +53,8 @@ public class AutoAim { // TODO: SOTM public static double distanceToHub(Pose2d pose) { - return pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + Logger.recordOutput("Autoaim/Distance To Hub", distance); + return distance; } } From d9f32ae33258bc41259f6f2386dee9d611b1c7e4 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Wed, 21 Jan 2026 17:45:27 -0800 Subject: [PATCH 18/19] update shot map --- src/main/java/frc/robot/Superstructure.java | 4 ++-- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9c6913e..f4e1895 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -229,7 +229,7 @@ private void addCommands() { SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), /*shooter.shoot(swerve::getPose)*/ - shooter.shoot(swerve::getPose)); + shooter.testShoot()); bindCommands( SuperState.SPIN_UP_FEED, @@ -242,7 +242,7 @@ private void addCommands() { SuperState.SCORE, intake.rest(), indexer.kick(), /*shooter.shoot(swerve::getPose)*/ - shooter.shoot(swerve::getPose)); + shooter.testShoot()); bindCommands(SuperState.SCORE_FLOW, intake.intake(), indexer.kick(), shooter.testShoot()); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 384fb6f..f4cf4d7 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -20,19 +20,19 @@ public class AutoAim { new ShotData(Rotation2d.fromDegrees(6), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36), - new ShotData(Rotation2d.fromDegrees(8), 30)); + new ShotData(Rotation2d.fromDegrees(10.5), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24), - new ShotData(Rotation2d.fromDegrees(12), 31)); + new ShotData(Rotation2d.fromDegrees(14.5), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24), - new ShotData(Rotation2d.fromDegrees(17), 30)); + new ShotData(Rotation2d.fromDegrees(18), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24), - new ShotData(Rotation2d.fromDegrees(21), 30)); + new ShotData(Rotation2d.fromDegrees(21.5), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24), - new ShotData(Rotation2d.fromDegrees(24), 30)); + new ShotData(Rotation2d.fromDegrees(24.5), 30)); HUB_SHOT_TREE.put( Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24 + 24), new ShotData(Rotation2d.fromDegrees(28), 30)); From abf792a185291c9b98b03c0a87f97c55def18966 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:32:03 -0800 Subject: [PATCH 19/19] remove outdated comments and make shot tree numbers look more normal --- .../subsystems/intake/IntakeSubsystem.java | 1 - .../robot/subsystems/shooter/FlywheelIO.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 2 +- .../constants/AlphaSwerveConstants.java | 7 ------- .../java/frc/robot/utils/autoaim/AutoAim.java | 20 +++++++------------ 5 files changed, 9 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index e159e95..f2e00f4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -38,7 +38,6 @@ public void periodic() { Logger.processInputs("Intake", inputs); } - // TODO get actual values public Command intake() { return this.run(() -> io.setRollerVoltage(10)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 9bb0e74..304e65b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -122,7 +122,7 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; config.Slot0.kS = 0.43477; - config.Slot0.kV = 0.144; // From sim + config.Slot0.kV = 0.144; config.Slot0.kA = 0.016433; config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index bd8ff9c..7a11881 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -30,7 +30,7 @@ public class ShooterSubsystem extends SubsystemBase { public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; - public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; // TODO: TUNE + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index ce122de..78470e3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -181,27 +181,23 @@ public Mass getMass() { @Override public ModuleConstants getFrontLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(-0.29).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getFrontRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.012)); } @Override public ModuleConstants getBackLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.229).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getBackRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.205)); } @@ -212,7 +208,6 @@ public int getGyroID() { @Override public Pigeon2Configuration getGyroConfig() { - // TODO getGyroConfig Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = 0.18661323189735413; config.MountPose.MountPoseRoll = -0.706454336643219; @@ -222,7 +217,6 @@ public Pigeon2Configuration getGyroConfig() { @Override public TalonFXConfiguration getDriveConfig() { - // TODO getDriveConfig var driveConfig = new TalonFXConfiguration(); // Current limits driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; @@ -255,7 +249,6 @@ public TalonFXConfiguration getDriveConfig() { @Override public TalonFXConfiguration getTurnConfig(int cancoderID) { - // TODO getTurnConfig var turnConfig = new TalonFXConfiguration(); // Current limits turnConfig.CurrentLimits.SupplyCurrentLimit = 20.0; diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index f4cf4d7..d1f27a5 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -11,34 +11,28 @@ public class AutoAim { public static final InterpolatingShotTree HUB_SHOT_TREE = new InterpolatingShotTree(); - // If we need other shot trees (i.e. for feeding) we can put them here - static { // For hub shot tree - // 6 feet from edge of hub HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 12), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), new ShotData(Rotation2d.fromDegrees(6), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), new ShotData(Rotation2d.fromDegrees(10.5), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), new ShotData(Rotation2d.fromDegrees(14.5), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), new ShotData(Rotation2d.fromDegrees(18), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), new ShotData(Rotation2d.fromDegrees(21.5), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), new ShotData(Rotation2d.fromDegrees(24.5), 30)); HUB_SHOT_TREE.put( - Units.inchesToMeters(12 * 2 * Math.sqrt(2) + 6 + 36 + 24 + 24 + 24 + 24 + 24), + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), new ShotData(Rotation2d.fromDegrees(28), 30)); - - // 2sqrt2 ft + - // 6 deg 30 rps } // Ig we'll see if we need more than 1 feed shot tree