Skip to content

Swerve fixes #207

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

Draft
nirmalkozak wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Swerve fixes #207

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7c8751d
initial commit
nirmalkozak Jun 27, 2026
48059c0
builds
nirmalkozak Jul 9, 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
33 changes: 23 additions & 10 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Robot.RobotRunType;
import frc.robot.commands.WaitSupplierCommand;
import frc.robot.localization.DrivetrainState;
import frc.robot.sim.FuelSim;
import frc.robot.sim.SimulatedRobotState;
import frc.robot.subsystems.LEDs;
Expand Down Expand Up @@ -84,8 +85,9 @@ public final class RobotContainer {
private final AutoCommandFactory autoCommandFactory;

/* Subsystems */
private final LEDs leds = new LEDs();
private final DrivetrainState drivetrainState;
private final Swerve swerve;
private final LEDs leds = new LEDs();
private final Vision vision;
private final AdjustableHood adjustableHood;
private final Turret turret;
Expand All @@ -112,15 +114,19 @@ public RobotContainer(RobotRunType runtimeType) {
switch (runtimeType) {
case kReal:
sim = null;
swerve = new Swerve(SwerveReal::new, GyroNavX2::new, SwerveModuleReal::new);
Swerve.Bundle realBundle =
Swerve.create(SwerveReal::new, GyroNavX2::new, SwerveModuleReal::new);
this.drivetrainState = realBundle.drivetrainState();
this.swerve = realBundle.swerve();

vision = new Vision(swerve.state, new VisionReal());
adjustableHood = new AdjustableHood(new AdjustableHoodReal());
turret = new Turret(new TurretReal(), swerve.state);
shooter = new Shooter(new ShooterReal());
intake = new Intake(new IntakeReal());
climber = new Climber(new ClimberIOEmpty());
indexer = new Indexer(new IndexerReal());

break;
case kSimulation:
SimulatedArena.overrideInstance(new Arena2026Rebuilt(false));
Expand All @@ -138,8 +144,11 @@ public RobotContainer(RobotRunType runtimeType) {
sim.indexer.addFuel();
});
FuelSim.getInstance().start();
swerve = new Swerve(sim.swerveDrive::simProvider, sim.swerveDrive::gyroProvider,
sim.swerveDrive::moduleProvider);
Swerve.Bundle simBundle = Swerve.create(sim.swerveDrive::simProvider,
sim.swerveDrive::gyroProvider, sim.swerveDrive::moduleProvider);
this.drivetrainState = simBundle.drivetrainState();
this.swerve = simBundle.swerve();

vision = new Vision(swerve.state, sim.visionSim);
adjustableHood = new AdjustableHood(sim.adjustableHood);
turret = new Turret(sim.turret, swerve.state);
Expand All @@ -150,28 +159,32 @@ public RobotContainer(RobotRunType runtimeType) {

SmartDashboard.putNumber("VisionFudge", 0.0);


FuelSim.getInstance().spawnStartingFuel();

break;
default:
sim = null;
swerve = new Swerve(SwerveIOEmpty::new, GyroIOEmpty::new, SwerveModuleIOEmpty::new);
Swerve.Bundle defaultBundle =
Swerve.create(SwerveIOEmpty::new, GyroIOEmpty::new, SwerveModuleIOEmpty::new);
this.drivetrainState = defaultBundle.drivetrainState();
this.swerve = defaultBundle.swerve();

vision = new Vision(swerve.state, new VisionIOEmpty());
adjustableHood = new AdjustableHood(new AdjustableHoodIOEmpty());
turret = new Turret(new TurretIOEmpty(), swerve.state);
shooter = new Shooter(new ShooterIOEmpty());
intake = new Intake(new IntakeIOEmpty());
climber = new Climber(new ClimberSim());
indexer = new Indexer(new IndexerIOEmpty());

break;


}

targetingState = new TargetingState(() -> swerve.state.getGlobalPoseEstimate(),
() -> swerve.state.getFieldRelativeSpeeds(), shooter.getFlyWheelVeloRPS());
() -> swerve.state.getFieldRelativeSpeeds(), shooter.getFlyWheelVeloRPS());
// DASHBOARD STUFF
SmartDashboard.putData(Constants.DashboardValues.autoChooser, autoChooser);
SmartDashboard.putNumber(Constants.DashboardValues.shootX,
Expand Down
73 changes: 54 additions & 19 deletions src/main/java/frc/robot/subsystems/swerve/Swerve.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,13 +73,13 @@
@NullMarked
public final class Swerve extends SubsystemBase {

private final Lock odometryLock = new ReentrantLock();
private final Lock odometryLock;
private final PhoenixOdometryThread odometryThread;
public final SwerveModule[] modules;
private final GyroIO gyro;
private final GyroInputsAutoLogged gyroInputs = new GyroInputsAutoLogged();
private final GyroInputsAutoLogged gyroInputs;
private final SwerveIO io;
private final SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged();
private final SwerveInputsAutoLogged inputs;

private final SwerveRateLimiter limiter = new SwerveRateLimiter();

Expand All @@ -106,34 +106,69 @@
* @param gyroIo factory for creating the gyro IO implementation
* @param moduleIoFn factory for creating per-module IO implementations
*/
public Swerve(Function<PhoenixOdometryThread, SwerveIO> swerveIo,
public static record Bundle(Swerve swerve, DrivetrainState drivetrainState) {
}

public static Bundle create(Function<PhoenixOdometryThread, SwerveIO> swerveIo,

Check warning on line 112 in src/main/java/frc/robot/subsystems/swerve/Swerve.java

View workflow job for this annotation

GitHub Actions / checkstyle

[checkstyle] src/main/java/frc/robot/subsystems/swerve/Swerve.java#L112 <com.puppycrawl.tools.checkstyle.checks.javadoc.MissingJavadocMethodCheck> 

Missing a Javadoc comment.
Raw output 
/github/workspace/src/main/java/frc/robot/subsystems/swerve/Swerve.java:112:5: warning: Missing a Javadoc comment. (com.puppycrawl.tools.checkstyle.checks.javadoc.MissingJavadocMethodCheck)
Function<PhoenixOdometryThread, GyroIO> gyroIo,
BiFunction<Integer, PhoenixOdometryThread, SwerveModuleIO> moduleIoFn) {
super("Swerve");
this.odometryThread = new PhoenixOdometryThread(this.odometryLock);
this.gyro = gyroIo.apply(this.odometryThread);
this.modules = IntStream.range(0, Constants.Swerve.modulesConstants.length)
.mapToObj(i -> new SwerveModule(i, moduleIoFn.apply(i, this.odometryThread)))
Lock localLock = new ReentrantLock();
PhoenixOdometryThread localOdometryThread = new PhoenixOdometryThread(localLock);

GyroIO localGyro = gyroIo.apply(localOdometryThread);
GyroInputsAutoLogged localGyroInputs = new GyroInputsAutoLogged();
SwerveIO localIo = swerveIo.apply(localOdometryThread);

SwerveInputsAutoLogged localInputs = new SwerveInputsAutoLogged();

SwerveModule[] localModules = IntStream.range(0, Constants.Swerve.modulesConstants.length)
.mapToObj(i -> new SwerveModule(i, moduleIoFn.apply(i, localOdometryThread)))
.toArray(SwerveModule[]::new);
this.io = swerveIo.apply(odometryThread);
this.odometryThread.start();
this.odometryLock.lock();
SwerveModulePosition[] initPositions = new SwerveModulePosition[modules.length];

localOdometryThread.start();

localLock.lock();
SwerveModulePosition[] initPositions = new SwerveModulePosition[localModules.length];
try {
Arrays.stream(modules).map(mod -> {
Arrays.stream(localModules).map(mod -> {
mod.updateInputs();
return mod.getPosition();
}).toArray(_i -> initPositions);
this.gyro.updateInputs(this.gyroInputs);
Logger.processInputs("Swerve/Gyro", this.gyroInputs);

localGyro.updateInputs(localGyroInputs);
Logger.processInputs("Swerve/Gyro", localGyroInputs);
} finally {
this.odometryLock.unlock();
localLock.unlock();
}
this.state = new DrivetrainState(initPositions, this.gyroInputs.yaw);
autoFactory = new AutoFactory(state::getGlobalPoseEstimate, state::resetPose,

DrivetrainState instantiatedState = new DrivetrainState(initPositions, localGyroInputs.yaw);

Swerve instantiatedSwerve = new Swerve(localLock, localOdometryThread, localModules,
localGyro, localGyroInputs, localIo, localInputs, instantiatedState);

return new Bundle(instantiatedSwerve, instantiatedState);
}


private Swerve(Lock odometryLock, PhoenixOdometryThread odometryThread, SwerveModule[] modules,
GyroIO gyro, GyroInputsAutoLogged gyroInputs, SwerveIO io, SwerveInputsAutoLogged inputs,
DrivetrainState state) {
super("Swerve");

this.odometryLock = odometryLock;
this.odometryThread = odometryThread;
this.modules = modules;
this.gyro = gyro;
this.gyroInputs = gyroInputs;
this.io = io;
this.inputs = inputs; // B. Assign it here to fix the error!
this.state = state;

this.autoFactory = new AutoFactory(state::getGlobalPoseEstimate, state::resetPose,
this::followTrajectory, true, this);
}


/**
* Set this to true to flip trajectories about the y axis (left/right) for auto paths.
*/
Expand Down
Loading