diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java index 65e9e03..522892f 100644 --- a/src/main/java/frc/robot/localization/LocalizationSubsystem.java +++ b/src/main/java/frc/robot/localization/LocalizationSubsystem.java @@ -3,12 +3,9 @@ import dev.doglog.DogLog; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -35,8 +32,6 @@ public class LocalizationSubsystem extends StateMachine { private final ImuSubsystem imu; private final VisionSubsystem vision; private final SwerveSubsystem swerve; - private final SwerveDrivePoseEstimator poseEstimator; - private final SwerveDriveOdometry odometry; private final TimeInterpolatableBuffer poseHistory = TimeInterpolatableBuffer.createBuffer(1.5); private double lastAddedVisionTimestamp = 0; @@ -47,17 +42,6 @@ public LocalizationSubsystem(ImuSubsystem imu, VisionSubsystem vision, SwerveSub this.swerve = swerve; this.imu = imu; this.vision = vision; - poseEstimator = - new SwerveDrivePoseEstimator( - SwerveSubsystem.KINEMATICS, - Rotation2d.fromDegrees(imu.getRobotHeading()), - swerve.getModulePositions().toArray(new SwerveModulePosition[4]), - new Pose2d()); - odometry = - new SwerveDriveOdometry( - SwerveSubsystem.KINEMATICS, - Rotation2d.fromDegrees(imu.getRobotHeading()), - swerve.getModulePositions().toArray(new SwerveModulePosition[4])); } @Override @@ -66,24 +50,12 @@ protected void collectInputs() { } public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - public Pose2d getOdometryPose() { - return odometry.getPoseMeters(); - } - - public void resetPose(Pose2d pose) { - resetPose(pose, pose); + return swerve.drivetrain.getState().Pose; } @Override public void robotPeriodic() { super.robotPeriodic(); - SwerveModulePosition[] modulePositions = - swerve.getModulePositions().toArray(new SwerveModulePosition[4]); - odometry.update(Rotation2d.fromDegrees(imu.getRobotHeading()), modulePositions); - poseEstimator.update(Rotation2d.fromDegrees(imu.getRobotHeading()), modulePositions); for (var results : latestResult) { Pose2d visionPose = results.pose(); @@ -93,35 +65,25 @@ public void robotPeriodic() { if (visionTimestamp == lastAddedVisionTimestamp) { // Don't add the same vision pose over and over } else { - poseEstimator.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS); + swerve.drivetrain.addVisionMeasurement(visionPose, visionTimestamp, VISION_STD_DEVS); lastAddedVisionTimestamp = visionTimestamp; } - poseHistory.addSample(Timer.getFPGATimestamp(), poseEstimator.getEstimatedPosition()); + poseHistory.addSample(Timer.getFPGATimestamp(), getPose()); } DogLog.log("Localization/EstimatedPose", getPose()); - DogLog.log("Localization/OdometryPose", getOdometryPose()); } private void resetGyro(double gyroAngle) { Pose2d estimatedPose = new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(gyroAngle)); - Pose2d odometryPose = - new Pose2d(getOdometryPose().getTranslation(), Rotation2d.fromDegrees(gyroAngle)); - resetPose(estimatedPose, odometryPose); + resetPose(estimatedPose); } - public void resetPose(Pose2d estimatedPose, Pose2d odometryPose) { + public void resetPose(Pose2d estimatedPose) { imu.setAngle(estimatedPose.getRotation().getDegrees()); - poseEstimator.resetPosition( - estimatedPose.getRotation(), - swerve.getModulePositions().toArray(new SwerveModulePosition[4]), - estimatedPose); - odometry.resetPosition( - odometryPose.getRotation(), - swerve.getModulePositions().toArray(new SwerveModulePosition[4]), - odometryPose); + swerve.drivetrain.seedFieldRelative(estimatedPose); } public Command getZeroCommand() { @@ -133,7 +95,6 @@ public Command getZeroCommand() { * target pose. */ public DistanceAngle getFieldRelativeDistanceAngleToPose(Pose2d target) { - DistanceAngle distanceAngleToSpeaker = distanceAngleToTarget(target, getPose()); return distanceAngleToSpeaker; diff --git a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java deleted file mode 100644 index 8bbe6b6..0000000 --- a/src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.robot.swerve; - -import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -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 frc.robot.generated.TunerConstants; -import java.util.function.Supplier; - -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used - * in command-based projects easily. - */ -public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - public static final SwerveModuleConstants BackRight = TunerConstants.BackRight; - public static final SwerveModuleConstants BackLeft = TunerConstants.BackLeft; - public static final SwerveModuleConstants FrontRight = TunerConstants.FrontRight; - public static final SwerveModuleConstants FrontLeft = TunerConstants.FrontLeft; - public static final SwerveDrivetrainConstants DrivetrainConstants = - TunerConstants.DrivetrainConstants; - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; - - 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 CommandSwerveDrivetrain() { - this(DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } - - 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); - } - - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } -} diff --git a/src/main/java/frc/robot/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/swerve/SwerveSubsystem.java index ea11ccd..f43d3bf 100644 --- a/src/main/java/frc/robot/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/swerve/SwerveSubsystem.java @@ -1,8 +1,10 @@ package frc.robot.swerve; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -10,12 +12,12 @@ import com.ctre.phoenix6.signals.InvertedValue; import dev.doglog.DogLog; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; import frc.robot.config.RobotConfig; import frc.robot.fms.FmsSubsystem; import frc.robot.generated.TunerConstants; @@ -40,25 +42,7 @@ public class SwerveSubsystem extends StateMachine { private static final double rightXDeadband = 0.15; private static final double leftYDeadband = 0.05; - public static final Translation2d FRONT_LEFT_LOCATION = - new Translation2d( - CommandSwerveDrivetrain.FrontLeft.LocationX, CommandSwerveDrivetrain.FrontLeft.LocationY); - public static final Translation2d FRONT_RIGHT_LOCATION = - new Translation2d( - CommandSwerveDrivetrain.FrontRight.LocationX, - CommandSwerveDrivetrain.FrontRight.LocationY); - public static final Translation2d BACK_LEFT_LOCATION = - new Translation2d( - CommandSwerveDrivetrain.BackLeft.LocationX, CommandSwerveDrivetrain.BackLeft.LocationY); - public static final Translation2d BACK_RIGHT_LOCATION = - new Translation2d( - CommandSwerveDrivetrain.BackRight.LocationX, CommandSwerveDrivetrain.BackRight.LocationY); - public static final Translation2d[] MODULE_LOCATIONS = - new Translation2d[] { - FRONT_LEFT_LOCATION, FRONT_RIGHT_LOCATION, BACK_LEFT_LOCATION, BACK_RIGHT_LOCATION - }; - public static final SwerveDriveKinematics KINEMATICS = - new SwerveDriveKinematics(MODULE_LOCATIONS); + private static final double SIM_LOOP_PERIOD = 0.005; // 5 ms private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber) { return switch (moduleNumber) { @@ -70,7 +54,13 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber) }; } - private final CommandSwerveDrivetrain drivetrain = new CommandSwerveDrivetrain(); + public final SwerveDrivetrain drivetrain = + new SwerveDrivetrain( + TunerConstants.DrivetrainConstants, + TunerConstants.FrontLeft, + TunerConstants.FrontRight, + TunerConstants.BackLeft, + TunerConstants.BackRight); public final Pigeon2 drivetrainPigeon = drivetrain.getPigeon2(); @@ -90,6 +80,9 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber) private final SwerveModule backLeft = drivetrain.getModule(2); private final SwerveModule backRight = drivetrain.getModule(3); + private double lastSimTime; + private Notifier simNotifier = null; + private boolean slowEnoughToShoot = false; private List modulePositions; private ChassisSpeeds robotRelativeSpeeds; @@ -103,10 +96,6 @@ private static SwerveModuleConstants constantsForModuleNumber(int moduleNumber) private ChassisSpeeds autoSpeeds = new ChassisSpeeds(); - private ChassisSpeeds intakeAssistTeleopSpeeds = new ChassisSpeeds(); - - private ChassisSpeeds intakeAssistAutoSpeeds = new ChassisSpeeds(); - public ChassisSpeeds getRobotRelativeSpeeds() { return robotRelativeSpeeds; } @@ -183,6 +172,10 @@ public SwerveSubsystem() { : InvertedValue.CounterClockwise_Positive; steerMotorConfigurator.apply(steerMotorOutput); } + + if (Utils.isSimulation()) { + startSimThread(); + } } public void setFieldRelativeAutoSpeeds(ChassisSpeeds speeds) { @@ -263,12 +256,12 @@ private List calculateModulePositions() { } private ChassisSpeeds calculateRobotRelativeSpeeds() { - return KINEMATICS.toChassisSpeeds(drivetrain.getState().ModuleStates); + return drivetrain.getState().speeds; } private ChassisSpeeds calculateFieldRelativeSpeeds() { return ChassisSpeeds.fromRobotRelativeSpeeds( - robotRelativeSpeeds, Rotation2d.fromDegrees(drivetrainPigeon.getYaw().getValueAsDouble())); + robotRelativeSpeeds, drivetrain.getState().Pose.getRotation()); } private static boolean calculateMovingSlowEnoughForSpeakerShot(ChassisSpeeds speeds) { @@ -313,7 +306,7 @@ private void sendSwerveRequest() { } } case INTAKE_ASSIST_TELEOP -> { - intakeAssistTeleopSpeeds = + var intakeAssistTeleopSpeeds = IntakeAssistManager.getRobotRelativeAssistSpeeds(0, teleopSpeeds); /// fix robotHeading @@ -340,7 +333,8 @@ private void sendSwerveRequest() { .withDriveRequestType(DriveRequestType.Velocity)); case INTAKE_ASSIST_AUTO -> { - intakeAssistAutoSpeeds = IntakeAssistManager.getRobotRelativeAssistSpeeds(0, autoSpeeds); + var intakeAssistAutoSpeeds = + IntakeAssistManager.getRobotRelativeAssistSpeeds(0, autoSpeeds); /// fix robotHeading drivetrain.setControl( drive @@ -371,4 +365,21 @@ public void robotPeriodic() { DogLog.log("Swerve/SnapAngle", goalSnapAngle); } + + private void startSimThread() { + lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + drivetrain.updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + simNotifier.startPeriodic(SIM_LOOP_PERIOD); + } }