Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[examples] Call resetOdometry() when controller command is executed (#5905)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Elliot Scher
2023-12-07 01:14:54 -05:00
committed by GitHub
parent f5fc101fda
commit 39a0bf4b98
6 changed files with 55 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
View File

@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveCo
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -85,7 +86,7 @@ public class RobotContainer {
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -121,10 +122,11 @@ public class RobotContainer {
m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return mecanumControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
// Reset odometry to the initial pose of the trajectory, run path following
// command, then stop at the end.
return Commands.sequence(
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
mecanumControllerCommand,
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
}
}

16
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
View File

@@ -4,8 +4,6 @@
package edu.wpi.first.wpilibj.examples.ramsetecommand;
import static edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -17,11 +15,13 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -97,7 +97,7 @@ public class RobotContainer {
// Apply the voltage constraint
.addConstraint(autoVoltageConstraint);
// An example trajectory to follow. All units in meters.
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -126,10 +126,10 @@ public class RobotContainer {
m_robotDrive::tankDriveVolts,
m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
// Reset odometry to the initial pose of the trajectory, run path following
// command, then stop at the end.
return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))
.andThen(ramseteCommand)
.andThen(Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)));
}
}

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
View File

@@ -19,6 +19,8 @@ import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleCo
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -82,7 +84,7 @@ public class RobotContainer {
// Add kinematics to ensure max speed is actually obeyed
.setKinematics(DriveConstants.kDriveKinematics);
// An example trajectory to follow. All units in meters.
// An example trajectory to follow. All units in meters.
Trajectory exampleTrajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
@@ -111,10 +113,11 @@ public class RobotContainer {
m_robotDrive::setModuleStates,
m_robotDrive);
// Reset odometry to the starting pose of the trajectory.
m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
// Run path following command, then stop at the end.
return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
// Reset odometry to the initial pose of the trajectory, run path following
// command, then stop at the end.
return Commands.sequence(
new InstantCommand(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> m_robotDrive.drive(0, 0, 0, false)));
}
}