247 lines
11 KiB
Java
247 lines
11 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import static edu.wpi.first.units.Units.*;
|
|
|
|
import java.util.Optional;
|
|
import java.util.function.Supplier;
|
|
|
|
import com.ctre.phoenix6.SignalLogger;
|
|
import com.ctre.phoenix6.Utils;
|
|
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
|
|
import com.ctre.phoenix6.swerve.SwerveModuleConstants;
|
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.config.PIDConstants;
|
|
import com.pathplanner.lib.config.RobotConfig;
|
|
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
|
|
|
import edu.wpi.first.math.Matrix;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
|
import edu.wpi.first.math.numbers.N1;
|
|
import edu.wpi.first.math.numbers.N3;
|
|
import edu.wpi.first.wpilibj.DriverStation;
|
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
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;
|
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
|
|
|
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
|
|
|
|
/**
|
|
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
|
* Subsystem so it can easily be used in command-based projects.
|
|
*
|
|
* Generated by the 2026 Tuner X Swerve Project Generator
|
|
* https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
|
*/
|
|
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
|
private static final double kSimLoopPeriod = 0.004; // 4 ms
|
|
private Notifier m_simNotifier = null;
|
|
private double m_lastSimTime;
|
|
|
|
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
|
|
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
|
|
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
|
|
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
|
/* Keep track if we've ever applied the operator perspective before or not */
|
|
private boolean m_hasAppliedOperatorPerspective = false;
|
|
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
|
|
private void configureAutoBuilder() {
|
|
try {
|
|
RobotConfig config = RobotConfig.fromGUISettings();
|
|
AutoBuilder.configure(
|
|
() -> getState().Pose,
|
|
this::resetPose,
|
|
() -> getState().Speeds,
|
|
(speeds, feedforwards) -> setControl(
|
|
m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
|
|
.withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
|
|
.withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
|
|
),
|
|
new PPHolonomicDriveController(
|
|
new PIDConstants(10, 0, 0),
|
|
new PIDConstants(7, 0, 0)
|
|
),
|
|
config,
|
|
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
|
|
this
|
|
);
|
|
} catch (Exception ex) {
|
|
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
|
|
}
|
|
}
|
|
/**
|
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
* <p>
|
|
* This constructs the underlying hardware devices, so users should not construct
|
|
* the devices themselves. If they need the devices, they can access them through
|
|
* getters in the classes.
|
|
*
|
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
* @param modules Constants for each specific module
|
|
*/
|
|
public CommandSwerveDrivetrain(
|
|
SwerveDrivetrainConstants drivetrainConstants,
|
|
SwerveModuleConstants<?, ?, ?>... modules
|
|
) {
|
|
super(drivetrainConstants, modules);
|
|
if (Utils.isSimulation()) {
|
|
startSimThread();
|
|
}
|
|
configureAutoBuilder();
|
|
}
|
|
|
|
/**
|
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
* <p>
|
|
* This constructs the underlying hardware devices, so users should not construct
|
|
* the devices themselves. If they need the devices, they can access them through
|
|
* getters in the classes.
|
|
*
|
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
* CAN FD, and 100 Hz on CAN 2.0.
|
|
* @param modules Constants for each specific module
|
|
*/
|
|
public CommandSwerveDrivetrain(
|
|
SwerveDrivetrainConstants drivetrainConstants,
|
|
double odometryUpdateFrequency,
|
|
SwerveModuleConstants<?, ?, ?>... modules
|
|
) {
|
|
super(drivetrainConstants, odometryUpdateFrequency, modules);
|
|
if (Utils.isSimulation()) {
|
|
startSimThread();
|
|
}
|
|
configureAutoBuilder();
|
|
}
|
|
|
|
/**
|
|
* Constructs a CTRE SwerveDrivetrain using the specified constants.
|
|
* <p>
|
|
* This constructs the underlying hardware devices, so users should not construct
|
|
* the devices themselves. If they need the devices, they can access them through
|
|
* getters in the classes.
|
|
*
|
|
* @param drivetrainConstants Drivetrain-wide constants for the swerve drive
|
|
* @param odometryUpdateFrequency The frequency to run the odometry loop. If
|
|
* unspecified or set to 0 Hz, this is 250 Hz on
|
|
* CAN FD, and 100 Hz on CAN 2.0.
|
|
* @param odometryStandardDeviation The standard deviation for odometry calculation
|
|
* in the form [x, y, theta]ᵀ, with units in meters
|
|
* and radians
|
|
* @param visionStandardDeviation The standard deviation for vision calculation
|
|
* in the form [x, y, theta]ᵀ, with units in meters
|
|
* and radians
|
|
* @param modules Constants for each specific module
|
|
*/
|
|
public CommandSwerveDrivetrain(
|
|
SwerveDrivetrainConstants drivetrainConstants,
|
|
double odometryUpdateFrequency,
|
|
Matrix<N3, N1> odometryStandardDeviation,
|
|
Matrix<N3, N1> visionStandardDeviation,
|
|
SwerveModuleConstants<?, ?, ?>... modules
|
|
) {
|
|
super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
|
|
if (Utils.isSimulation()) {
|
|
startSimThread();
|
|
}
|
|
configureAutoBuilder();
|
|
}
|
|
|
|
/**
|
|
* Returns a command that applies the specified control request to this swerve drivetrain.
|
|
*
|
|
* @param request Function returning the request to apply
|
|
* @return Command to run
|
|
*/
|
|
public Command applyRequest(Supplier<SwerveRequest> request) {
|
|
return run(() -> this.setControl(request.get()));
|
|
}
|
|
|
|
@Override
|
|
public void periodic() {
|
|
/*
|
|
* Periodically try to apply the operator perspective.
|
|
* If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
|
|
* This allows us to correct the perspective in case the robot code restarts mid-match.
|
|
* Otherwise, only check and apply the operator perspective if the DS is disabled.
|
|
* This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
|
|
*/
|
|
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
|
|
DriverStation.getAlliance().ifPresent(allianceColor -> {
|
|
setOperatorPerspectiveForward(
|
|
allianceColor == Alliance.Red
|
|
? kRedAlliancePerspectiveRotation
|
|
: kBlueAlliancePerspectiveRotation
|
|
);
|
|
m_hasAppliedOperatorPerspective = true;
|
|
});
|
|
}
|
|
}
|
|
|
|
private void startSimThread() {
|
|
m_lastSimTime = Utils.getCurrentTimeSeconds();
|
|
|
|
/* Run simulation at a faster rate so PID gains behave more reasonably */
|
|
m_simNotifier = new Notifier(() -> {
|
|
final double currentTime = Utils.getCurrentTimeSeconds();
|
|
double deltaTime = currentTime - m_lastSimTime;
|
|
m_lastSimTime = currentTime;
|
|
|
|
/* use the measured time delta, get battery voltage from WPILib */
|
|
updateSimState(deltaTime, RobotController.getBatteryVoltage());
|
|
});
|
|
m_simNotifier.startPeriodic(kSimLoopPeriod);
|
|
}
|
|
|
|
/**
|
|
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
|
* while still accounting for measurement noise.
|
|
*
|
|
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
|
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
|
*/
|
|
@Override
|
|
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
|
|
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
|
|
}
|
|
|
|
/**
|
|
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
|
* while still accounting for measurement noise.
|
|
* <p>
|
|
* Note that the vision measurement standard deviations passed into this method
|
|
* will continue to apply to future measurements until a subsequent call to
|
|
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
|
|
*
|
|
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
|
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
|
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
|
|
* in the form [x, y, theta]ᵀ, with units in meters and radians.
|
|
*/
|
|
@Override
|
|
public void addVisionMeasurement(
|
|
Pose2d visionRobotPoseMeters,
|
|
double timestampSeconds,
|
|
Matrix<N3, N1> visionMeasurementStdDevs
|
|
) {
|
|
super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
|
|
}
|
|
|
|
/**
|
|
* Return the pose at a given timestamp, if the buffer is not empty.
|
|
*
|
|
* @param timestampSeconds The timestamp of the pose in seconds.
|
|
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
|
|
*/
|
|
@Override
|
|
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
|
|
return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds));
|
|
}
|
|
}
|