Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -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()));