Skip to content

Alpha Bring up #27

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 19 commits into from
Jan 23, 2026
Merged

Alpha Bring up #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
9939ad7
intake bringup
spellingcat Jan 19, 2026
8165e47
add to string in order for sysid to recognize the routine names
spellingcat Jan 19, 2026
461e8b7
log rotations for sysid
spellingcat Jan 19, 2026
459d8cc
tune hood
spellingcat Jan 19, 2026
ad924df
tune flywheel
spellingcat Jan 19, 2026
423e7be
tune canranges and stuff
spellingcat Jan 19, 2026
42acd62
logged tunable numbrs and motion profile
spellingcat Jan 20, 2026
a508a5f
Made the logged tunable numbers work for the shooter
SCool62 Jan 21, 2026
1009e37
Testing at 10th st. Doesn't work :(
SCool62 Jan 21, 2026
fcab4f7
Add new transition condition for SPIN_UP_FEED -> FEED as well
SCool62 Jan 21, 2026
fda189a
fix hood unit conversion
spellingcat Jan 21, 2026
1540a37
EYYYYY
spellingcat Jan 21, 2026
72346cf
make things go in the right direction
spellingcat Jan 21, 2026
52c2247
i think we've hit flow state hell yeah
spellingcat Jan 21, 2026
2221b9c
add debounce for indexer being full
spellingcat Jan 22, 2026
8be44cb
fix heading snap
spellingcat Jan 22, 2026
e658830
get rid of is full
spellingcat Jan 22, 2026
d9f32ae
update shot map
spellingcat Jan 22, 2026
abf792a
remove outdated comments and make shot tree numbers look more normal
spellingcat Jan 23, 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
3 changes: 2 additions & 1 deletion .vscode/settings.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,5 +59,6 @@
],
"java.dependency.enableDependencyCheckup": false,
"wpilib.selectDefaultSimulateExtension": false,
"wpilib.skipSelectSimulateExtension": true
"wpilib.skipSelectSimulateExtension": true,
"wpilib.autoStartRioLog": false
}
2 changes: 2 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -316,6 +316,8 @@ 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().ignoringDisable(true));
SmartDashboard.putData("Test Shot", shooter.testShoot());

// Reset alert timers
canInitialErrorTimer.restart();
Expand Down
83 changes: 52 additions & 31 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 @@ -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;
Expand Down Expand Up @@ -127,65 +127,81 @@ 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());

isFull = new Trigger(indexer::isFull);
isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune

isEmpty = new Trigger(indexer::isEmpty);
}

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()));
bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq);
// .and(isFull.negate()));

bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq);

bindTransition(
SuperState.SPIN_UP_SCORE,
SuperState.SCORE,
new Trigger(shooter::atFlywheelVelocitySetpoint));
new Trigger(shooter::atFlywheelVelocitySetpoint)
.debounce(0.5)
.and(new Trigger(shooter::atHoodSetpoint).debounce(0.5))
.and(() -> stateTimer.hasElapsed(0.5)));

bindTransition(
SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint));
// 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);
bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5).and(scoreReq.negate()));

// 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
Expand All @@ -206,11 +222,14 @@ private void addCommands() {
bindCommands(
SuperState.READY,
intake.rest(),
indexer.index(),
indexer.rest(),
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,
Expand All @@ -220,10 +239,12 @@ 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)*/
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,
Expand All @@ -239,7 +260,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() {
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/components/canrange/CANrangeIOReal.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,15 @@ public class CANrangeIOReal implements CANrangeIO {
private final StatusSignal<Distance> distance;
private final StatusSignal<Boolean> 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);

Expand Down
11 changes: 9 additions & 2 deletions src/main/java/frc/robot/components/rollers/RollerIO.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -32,6 +34,7 @@ public static class RollerIOInputs {
private final StatusSignal<Voltage> appliedVoltage;
private final StatusSignal<Current> statorCurrentAmps;
private final StatusSignal<Temperature> motorTemperatureCelsius;
private final StatusSignal<Angle> motorPosition;

private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
private final VelocityVoltage velocityVoltage =
Expand All @@ -47,14 +50,16 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) {
appliedVoltage = motor.getMotorVoltage();
statorCurrentAmps = motor.getStatorCurrent();
motorTemperatureCelsius = motor.getDeviceTemp();
motorPosition = motor.getPosition();

BaseStatusSignal.setUpdateFrequencyForAll(
50.0,
angularVelocityRotsPerSec,
supplyCurrentAmps,
statorCurrentAmps,
appliedVoltage,
motorTemperatureCelsius);
motorTemperatureCelsius,
motorPosition);

motor.getConfigurator().apply(config);
motor.optimizeBusUtilization();
Expand All @@ -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) {
Expand Down
24 changes: 12 additions & 12 deletions src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}

Expand Down Expand Up @@ -77,20 +77,20 @@ public boolean isPartiallyFull() {
public Command index() {
return this.run(
() -> {
indexRollerIO.setRollerVoltage(5);
kickerIO.setRollerVoltage(-5);
indexRollerIO.setRollerVoltage(7);
kickerIO.setRollerVoltage(7);
});
}

public Command indexToShoot() {
public Command kick() {
return this.run(
() -> {
indexRollerIO.setRollerVoltage(10);
kickerIO.setRollerVoltage(5);
indexRollerIO.setRollerVoltage(12);
kickerIO.setRollerVoltage(-7);
});
}

public Command outtake() {
public Command spit() {
return this.run(
() -> {
indexRollerIO.setRollerVoltage(-5);
Expand All @@ -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;

Expand All @@ -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;
Expand Down Expand Up @@ -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() {
Expand Down
22 changes: 12 additions & 10 deletions src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)),
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) {
Expand All @@ -34,13 +38,12 @@ public void periodic() {
Logger.processInputs("Intake", inputs);
}

// TODO get actual values
public Command intake() {
return this.run(() -> io.setRollerVoltage(5));
return this.run(() -> io.setRollerVoltage(10));
}

public Command outake() {
return this.run(() -> io.setRollerVoltage(-2));
return this.run(() -> io.setRollerVoltage(-5));
}

public Command rest() {
Expand All @@ -62,14 +65,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;
Expand Down
Loading