Pose estimation, VisualPoseProvider setup, plus a little more cleanup of Drivetrain subsystem to have things make slightly more sense
This commit is contained in:
parent
73b068903e
commit
69fd4b76cc
@ -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);
|
||||
|
||||
@ -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(),
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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();
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user