mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
678176cd3c
Resolves #8587 This PR implements the requested anti-tipping utility and refactors the math to correctly adhere to the NWU coordinate system. **Key Changes:** * Fixed the axis mapping: Positive pitch now correctly maps to a forward tip (+X), and positive roll maps to a rightward tip (-Y). * Inverted the proportional control logic: The correction vector now applies a positive `kP` to drive *into* the direction of the fall to get the wheels back under the center of gravity, rather than driving away from it. * Added a comprehensive JUnit test suite (`AntiTippingTest.java`) to verify the calculated `ChassisSpeeds` correctly zero out the orthogonal axis and provide the correct positive/negative velocity across all four tipping directions. Tested locally against `testDesktopJava` and passes all style/formatting guidelines. --------- Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com>
60 lines
2.1 KiB
C++
60 lines
2.1 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "wpi/math/controller/AntiTipping.hpp"
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
static constexpr double kTolerance = 1e-6;
|
|
|
|
// Shared constructor parameters used by all tests
|
|
static constexpr wpi::units::unit_t<wpi::math::AntiTipping::kp_unit> kKp{0.1};
|
|
static constexpr wpi::units::radian_t kThreshold = 3_deg;
|
|
static constexpr wpi::units::meters_per_second_t kMaxSpeed = 2_mps;
|
|
|
|
TEST(AntiTippingTest, BelowThresholdGeneratesNoCorrection) {
|
|
wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
|
|
auto correction =
|
|
antiTipping.Calculate(wpi::math::Rotation3d{1_deg, 1_deg, 0_deg});
|
|
|
|
EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
|
|
EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
|
|
}
|
|
|
|
TEST(AntiTippingTest, ForwardTipDrivesForward) {
|
|
wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
|
|
auto correction =
|
|
antiTipping.Calculate(wpi::math::Rotation3d{0_deg, 10_deg, 0_deg});
|
|
|
|
EXPECT_GT(correction.vx.value(), 0.0);
|
|
EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
|
|
}
|
|
|
|
TEST(AntiTippingTest, BackwardTipDrivesBackward) {
|
|
wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
|
|
auto correction =
|
|
antiTipping.Calculate(wpi::math::Rotation3d{0_deg, -10_deg, 0_deg});
|
|
|
|
EXPECT_LT(correction.vx.value(), 0.0);
|
|
EXPECT_NEAR(0.0, correction.vy.value(), kTolerance);
|
|
}
|
|
|
|
TEST(AntiTippingTest, RightRollDrivesRight) {
|
|
wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
|
|
auto correction =
|
|
antiTipping.Calculate(wpi::math::Rotation3d{15_deg, 0_deg, 0_deg});
|
|
|
|
EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
|
|
EXPECT_LT(correction.vy.value(), 0.0);
|
|
}
|
|
|
|
TEST(AntiTippingTest, LeftRollDrivesLeft) {
|
|
wpi::math::AntiTipping antiTipping{kKp, kThreshold, kMaxSpeed};
|
|
auto correction =
|
|
antiTipping.Calculate(wpi::math::Rotation3d{-15_deg, 0_deg, 0_deg});
|
|
|
|
EXPECT_NEAR(0.0, correction.vx.value(), kTolerance);
|
|
EXPECT_GT(correction.vy.value(), 0.0);
|
|
}
|