This commit is contained in:
samuel desharnais
2024-12-17 18:32:47 -05:00
parent 81aefe3d56
commit 5bc17643ec
4 changed files with 81 additions and 36 deletions

View File

@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
/**
* Class that extends the Phoenix SwerveDrivetrain class and implements
* subsystem so it can be used in command-based projects easily.
@ -31,20 +30,20 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
private final Rotation2d RedAlliancePerspectiveRotation = Rotation2d.fromDegrees(180);
/* Keep track if we've ever applied the operator perspective before or not */
private boolean hasAppliedOperatorPerspective = false;
public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, SwerveModuleConstants... modules) {
super(driveTrainConstants, OdometryUpdateFrequency, modules);
if (Utils.isSimulation()) {
startSimThread();
}
}
public CommandSwerveDrivetrain(SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) {
super(driveTrainConstants, modules);
if (Utils.isSimulation()) {
startSimThread();
}
}
public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
return run(() -> this.setControl(requestSupplier.get()));
}
@ -63,7 +62,7 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
});
m_simNotifier.startPeriodic(kSimLoopPeriod);
}
@Override
public void periodic() {
/* Periodically try to apply the operator perspective */