diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eb47e81..c1c7caf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -18,7 +19,9 @@ public class RobotContainer { private CommandXboxController secondary; public RobotContainer() { - drivetrain = new Drivetrain(); + // TODO Need to provide a real initial pose + // TODO Need to provide a real VisualPoseProvider, null means we're not using one at all + drivetrain = new Drivetrain(new Pose2d(), null); primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB); secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB); diff --git a/src/main/java/frc/robot/constants/ModuleConstants.java b/src/main/java/frc/robot/constants/SwerveModuleConstants.java similarity index 100% rename from src/main/java/frc/robot/constants/ModuleConstants.java rename to src/main/java/frc/robot/constants/SwerveModuleConstants.java diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 9889daa..0bbfc2b 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -2,6 +2,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -20,68 +21,90 @@ import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.utilities.MAXSwerveModule; import frc.robot.utilities.SwerveUtils; +import frc.robot.utilities.VisualPoseProvider; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.PIDCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Drivetrain extends SubsystemBase { // Create MAXSwerveModules - private final MAXSwerveModule m_frontLeft = new MAXSwerveModule( - DrivetrainConstants.kFrontLeftDrivingCanId, - DrivetrainConstants.kFrontLeftTurningCanId, - DrivetrainConstants.kFrontLeftChassisAngularOffset - ); - - private final MAXSwerveModule m_frontRight = new MAXSwerveModule( - DrivetrainConstants.kFrontRightDrivingCanId, - DrivetrainConstants.kFrontRightTurningCanId, - DrivetrainConstants.kFrontRightChassisAngularOffset - ); - - private final MAXSwerveModule m_rearLeft = new MAXSwerveModule( - DrivetrainConstants.kRearLeftDrivingCanId, - DrivetrainConstants.kRearLeftTurningCanId, - DrivetrainConstants.kBackLeftChassisAngularOffset - ); - - private final MAXSwerveModule m_rearRight = new MAXSwerveModule( - DrivetrainConstants.kRearRightDrivingCanId, - DrivetrainConstants.kRearRightTurningCanId, - DrivetrainConstants.kBackRightChassisAngularOffset - ); + private final MAXSwerveModule m_frontLeft; + private final MAXSwerveModule m_frontRight; + private final MAXSwerveModule m_rearLeft; + private final MAXSwerveModule m_rearRight; // The gyro sensor - private final AHRS m_gyro = new AHRS(SPI.Port.kMXP); + private final AHRS m_gyro; - // Slew rate filter variables for controlling lateral acceleration - private double m_currentRotation = 0.0; - private double m_currentTranslationDir = 0.0; - private double m_currentTranslationMag = 0.0; + private final SlewRateLimiter m_magLimiter; + private final SlewRateLimiter m_rotLimiter; - private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate); - private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate); - private double m_prevTime = WPIUtilJNI.now() * 1e-6; + private final VisualPoseProvider m_visualPoseProvider; // Odometry class for tracking robot pose - SwerveDriveOdometry m_odometry = new SwerveDriveOdometry( - DrivetrainConstants.kDriveKinematics, - Rotation2d.fromDegrees(getGyroAngle()), - new SwerveModulePosition[] { - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition() - } - ); + SwerveDrivePoseEstimator m_poseEstimator; + + // Slew rate filter variables for controlling lateral acceleration + private double m_currentRotation; + private double m_currentTranslationDir; + private double m_currentTranslationMag; + + private double m_prevTime; /** Creates a new DriveSubsystem. */ - public Drivetrain() { + public Drivetrain(Pose2d initialPose, VisualPoseProvider visualPoseProvider) { + m_frontLeft = new MAXSwerveModule( + DrivetrainConstants.kFrontLeftDrivingCanId, + DrivetrainConstants.kFrontLeftTurningCanId, + DrivetrainConstants.kFrontLeftChassisAngularOffset + ); + m_frontRight = new MAXSwerveModule( + DrivetrainConstants.kFrontRightDrivingCanId, + DrivetrainConstants.kFrontRightTurningCanId, + DrivetrainConstants.kFrontRightChassisAngularOffset + ); + + m_rearLeft = new MAXSwerveModule( + DrivetrainConstants.kRearLeftDrivingCanId, + DrivetrainConstants.kRearLeftTurningCanId, + DrivetrainConstants.kBackLeftChassisAngularOffset + ); + + m_rearRight = new MAXSwerveModule( + DrivetrainConstants.kRearRightDrivingCanId, + DrivetrainConstants.kRearRightTurningCanId, + DrivetrainConstants.kBackRightChassisAngularOffset + ); + + m_gyro = new AHRS(SPI.Port.kMXP); + + m_magLimiter = new SlewRateLimiter(DrivetrainConstants.kMagnitudeSlewRate); + m_rotLimiter = new SlewRateLimiter(DrivetrainConstants.kRotationalSlewRate); + + m_poseEstimator = new SwerveDrivePoseEstimator( + DrivetrainConstants.kDriveKinematics, + Rotation2d.fromDegrees(getGyroAngle()), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }, + initialPose + ); + + m_visualPoseProvider = visualPoseProvider; + + m_currentRotation = 0.0; + m_currentTranslationDir = 0.0; + m_currentTranslationMag = 0.0; + m_prevTime = WPIUtilJNI.now() * 1e-6; } @Override public void periodic() { // Update the odometry in the periodic block - m_odometry.update( + m_poseEstimator.update( Rotation2d.fromDegrees(getGyroAngle()), new SwerveModulePosition[] { m_frontLeft.getPosition(), @@ -90,6 +113,13 @@ public class Drivetrain extends SubsystemBase { m_rearRight.getPosition() } ); + + if (m_visualPoseProvider != null) { + m_poseEstimator.addVisionMeasurement( + m_visualPoseProvider.getVisualPose().visualPose(), + m_visualPoseProvider.getVisualPose().timestamp() + ); + } } /** @@ -98,15 +128,15 @@ public class Drivetrain extends SubsystemBase { * @return The pose. */ public Pose2d getPose() { - return m_odometry.getPoseMeters(); + return m_poseEstimator.getEstimatedPosition(); } public double getPoseX(){ - return m_odometry.getPoseMeters().getX(); + return m_poseEstimator.getEstimatedPosition().getX(); } public double getPoseY(){ - return m_odometry.getPoseMeters().getY(); + return m_poseEstimator.getEstimatedPosition().getY(); } /** @@ -115,7 +145,7 @@ public class Drivetrain extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition( + m_poseEstimator.resetPosition( Rotation2d.fromDegrees(getGyroAngle()), new SwerveModulePosition[] { m_frontLeft.getPosition(), diff --git a/src/main/java/frc/robot/utilities/MAXSwerveModule.java b/src/main/java/frc/robot/utilities/MAXSwerveModule.java index 78862a4..de4dad6 100644 --- a/src/main/java/frc/robot/utilities/MAXSwerveModule.java +++ b/src/main/java/frc/robot/utilities/MAXSwerveModule.java @@ -11,7 +11,7 @@ import com.revrobotics.SparkPIDController; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; -import frc.robot.constants.ModuleConstants; +import frc.robot.constants.SwerveModuleConstants; public class MAXSwerveModule { private final CANSparkMax m_drivingSparkMax; @@ -52,49 +52,49 @@ public class MAXSwerveModule { // Apply position and velocity conversion factors for the driving encoder. The // native units for position and velocity are rotations and RPM, respectively, // but we want meters and meters per second to use with WPILib's swerve APIs. - m_drivingEncoder.setPositionConversionFactor(ModuleConstants.kDrivingEncoderPositionFactor); - m_drivingEncoder.setVelocityConversionFactor(ModuleConstants.kDrivingEncoderVelocityFactor); + m_drivingEncoder.setPositionConversionFactor(SwerveModuleConstants.kDrivingEncoderPositionFactor); + m_drivingEncoder.setVelocityConversionFactor(SwerveModuleConstants.kDrivingEncoderVelocityFactor); // Apply position and velocity conversion factors for the turning encoder. We // want these in radians and radians per second to use with WPILib's swerve // APIs. - m_turningEncoder.setPositionConversionFactor(ModuleConstants.kTurningEncoderPositionFactor); - m_turningEncoder.setVelocityConversionFactor(ModuleConstants.kTurningEncoderVelocityFactor); + m_turningEncoder.setPositionConversionFactor(SwerveModuleConstants.kTurningEncoderPositionFactor); + m_turningEncoder.setVelocityConversionFactor(SwerveModuleConstants.kTurningEncoderVelocityFactor); // Invert the turning encoder, since the output shaft rotates in the opposite direction of // the steering motor in the MAXSwerve Module. - m_turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted); + m_turningEncoder.setInverted(SwerveModuleConstants.kTurningEncoderInverted); // Enable PID wrap around for the turning motor. This will allow the PID // controller to go through 0 to get to the setpoint i.e. going from 350 degrees // to 10 degrees will go through 0 rather than the other direction which is a // longer route. m_turningPIDController.setPositionPIDWrappingEnabled(true); - m_turningPIDController.setPositionPIDWrappingMinInput(ModuleConstants.kTurningEncoderPositionPIDMinInput); - m_turningPIDController.setPositionPIDWrappingMaxInput(ModuleConstants.kTurningEncoderPositionPIDMaxInput); + m_turningPIDController.setPositionPIDWrappingMinInput(SwerveModuleConstants.kTurningEncoderPositionPIDMinInput); + m_turningPIDController.setPositionPIDWrappingMaxInput(SwerveModuleConstants.kTurningEncoderPositionPIDMaxInput); // Set the PID gains for the driving motor. Note these are example gains, and you // may need to tune them for your own robot! - m_drivingPIDController.setP(ModuleConstants.kDrivingP); - m_drivingPIDController.setI(ModuleConstants.kDrivingI); - m_drivingPIDController.setD(ModuleConstants.kDrivingD); - m_drivingPIDController.setFF(ModuleConstants.kDrivingFF); - m_drivingPIDController.setOutputRange(ModuleConstants.kDrivingMinOutput, - ModuleConstants.kDrivingMaxOutput); + m_drivingPIDController.setP(SwerveModuleConstants.kDrivingP); + m_drivingPIDController.setI(SwerveModuleConstants.kDrivingI); + m_drivingPIDController.setD(SwerveModuleConstants.kDrivingD); + m_drivingPIDController.setFF(SwerveModuleConstants.kDrivingFF); + m_drivingPIDController.setOutputRange(SwerveModuleConstants.kDrivingMinOutput, + SwerveModuleConstants.kDrivingMaxOutput); // Set the PID gains for the turning motor. Note these are example gains, and you // may need to tune them for your own robot! - m_turningPIDController.setP(ModuleConstants.kTurningP); - m_turningPIDController.setI(ModuleConstants.kTurningI); - m_turningPIDController.setD(ModuleConstants.kTurningD); - m_turningPIDController.setFF(ModuleConstants.kTurningFF); - m_turningPIDController.setOutputRange(ModuleConstants.kTurningMinOutput, - ModuleConstants.kTurningMaxOutput); + m_turningPIDController.setP(SwerveModuleConstants.kTurningP); + m_turningPIDController.setI(SwerveModuleConstants.kTurningI); + m_turningPIDController.setD(SwerveModuleConstants.kTurningD); + m_turningPIDController.setFF(SwerveModuleConstants.kTurningFF); + m_turningPIDController.setOutputRange(SwerveModuleConstants.kTurningMinOutput, + SwerveModuleConstants.kTurningMaxOutput); - m_drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode); - m_turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode); - m_drivingSparkMax.setSmartCurrentLimit(ModuleConstants.kDrivingMotorCurrentLimit); - m_turningSparkMax.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit); + m_drivingSparkMax.setIdleMode(SwerveModuleConstants.kDrivingMotorIdleMode); + m_turningSparkMax.setIdleMode(SwerveModuleConstants.kTurningMotorIdleMode); + m_drivingSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kDrivingMotorCurrentLimit); + m_turningSparkMax.setSmartCurrentLimit(SwerveModuleConstants.kTurningMotorCurrentLimit); // Save the SPARK MAX configurations. If a SPARK MAX browns out during // operation, it will maintain the above configurations. diff --git a/src/main/java/frc/robot/utilities/VisualPoseProvider.java b/src/main/java/frc/robot/utilities/VisualPoseProvider.java new file mode 100644 index 0000000..62fc574 --- /dev/null +++ b/src/main/java/frc/robot/utilities/VisualPoseProvider.java @@ -0,0 +1,9 @@ +package frc.robot.utilities; + +import edu.wpi.first.math.geometry.Pose2d; + +public interface VisualPoseProvider { + public record VisualPose(Pose2d visualPose, double timestamp) {} + + public VisualPose getVisualPose(); +}