Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

added pose estimator

Browse Source
This commit is contained in:
Abhi
2023-02-10 17:12:53 -07:00
parent 43cfe18f54
commit 1de91d8eb2
2 changed files with 81 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+12 -4
View File
@@ -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()));
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+69 -20
View File
@@ -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());
}
/**