mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
38 lines
1.7 KiB
C++
38 lines
1.7 KiB
C++
|
/*----------------------------------------------------------------------------*/
|
||
|
|
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
|
||
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||
|
|
/* the project. */
|
||
|
|
/*----------------------------------------------------------------------------*/
|
||
|
|
|
||
|
|
#include <units/units.h>
|
||
|
|
|
||
|
|
#include "frc/controller/ControllerUtil.h"
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
|
||
|
|
TEST(ControllerUtilTest, GetModulusError) {
|
||
|
|
// Test symmetric range
|
||
|
|
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0));
|
||
|
|
EXPECT_FLOAT_EQ(-20.0,
|
||
|
|
frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
|
||
|
|
EXPECT_FLOAT_EQ(-20.0,
|
||
|
|
frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
|
||
|
|
|
||
|
|
// Test range starting at zero
|
||
|
|
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
|
||
|
|
EXPECT_FLOAT_EQ(-20.0,
|
||
|
|
frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
|
||
|
|
EXPECT_FLOAT_EQ(-20.0,
|
||
|
|
frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
|
||
|
|
|
||
|
|
// Test asymmetric range that doesn't start at zero
|
||
|
|
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
|
||
|
|
|
||
|
|
// Test all supported types
|
||
|
|
EXPECT_FLOAT_EQ(-20.0,
|
||
|
|
frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
|
||
|
|
EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
|
||
|
|
EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
|
||
|
|
-170_deg, 190_deg));
|
||
|
|
}
|