From 7c57dd53e8eceace2c400475e5a1ed91845e5fdc Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 10 Feb 2023 17:12:53 -0700 Subject: [PATCH] added pose estimator --- .../java/frc4388/robot/RobotContainer.java | 16 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 89 ++++++++++++++----- 2 files changed, 81 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4058926..14969a8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,8 +92,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -153,9 +153,17 @@ public class RobotContainer { HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); //Command to follow trajectory + // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + // trajectory, + // m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.getKinematics(), + // holoController, + // m_robotSwerveDrive::setModuleStates, + // m_robotSwerveDrive); + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( trajectory, - m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive::getPoseEstimator, m_robotSwerveDrive.getKinematics(), holoController, m_robotSwerveDrive::setModuleStates, @@ -163,7 +171,7 @@ public class RobotContainer { //Init and wrap-up, return everything return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive), // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), swerveControllerCommand, new InstantCommand(() -> m_robotSwerveDrive.stopModules())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5790496..c174c29 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; +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.geometry.Translation2d; @@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - private SwerveDriveOdometry odometry; + // private SwerveDriveOdometry odometry; + + private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - this.odometry = new SwerveDriveOdometry( + // this.odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // getOdometry() + // ); + + this.poseEstimator = new SwerveDrivePoseEstimator( kinematics, - gyro.getRotation2d(), + gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), rightFront.getPosition(), leftBack.getPosition(), rightBack.getPosition() - } + }, + new Pose2d(0,0, new Rotation2d(0)) ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; @@ -113,15 +129,27 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - setOdometry(getOdometry()); + // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } + + public void updatePoseEstimator() { + poseEstimator.update( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -136,16 +164,33 @@ public class SwerveDrive extends SubsystemBase { * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } + + public Pose2d getPoseEstimator() { + return poseEstimator.getEstimatedPosition(); } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } + + public void setPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -157,13 +202,17 @@ public class SwerveDrive extends SubsystemBase { ); } + public void resetPoseEstimator() { + setPoseEstimator(new Pose2d()); + } + /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. */ - public void resetOdometry() { - setOdometry(new Pose2d()); - } + // public void resetOdometry() { + // setOdometry(new Pose2d()); + // } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -172,14 +221,14 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); + // updateOdometry(); + updatePoseEstimator(); - // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - SmartDashboard.putNumber("Odo X", getOdometry().getX()); - SmartDashboard.putNumber("Odo Y", getOdometry().getY()); - SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odo X", getOdometry().getX()); + // SmartDashboard.putNumber("Odo Y", getOdometry().getY()); + // SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } /**