Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Fix initial heading behavior in HolonomicDriveController (#3290)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-04-19 00:00:11 -04:00
committed by GitHub
parent 659b37ef9d
commit aaf24e2552
5 changed files with 47 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
View File

@@ -34,6 +34,13 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
// If this is the first run, then we need to reset the theta controller to the
// current pose's heading.
if (m_firstRun) {
m_thetaController.Reset(currentPose.Rotation().Radians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative)
auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
auto yFF = linearVelocityRef * poseRef.Rotation().Sin();

2
wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -105,5 +105,7 @@ class HolonomicDriveController {
frc2::PIDController m_xController;
frc2::PIDController m_yController;
ProfiledPIDController<units::radian> m_thetaController;
bool m_firstRun = true;
};
} // namespace frc

14
wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -47,3 +47,17 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
kAngularTolerance);
}
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::HolonomicDriveController controller{
frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0,
frc::TrapezoidProfile<units::radian>::Constraints{
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.to<double>());
}