mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
62f0f8190d
frc::NormalizeAngle(), units::math::NormalizeAngle(), and frc::GetModulusError() were replaced with frc::InputModulus() and frc::AngleModulus(). They were placed in wpimath/src/main/native/include/frc/MathUtil.h for C++ and wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java for Java.
46 lines
1.2 KiB
C++
46 lines
1.2 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.
|
|
|
|
#pragma once
|
|
|
|
#include <wpi/math>
|
|
|
|
#include "units/angle.h"
|
|
|
|
namespace frc {
|
|
|
|
/**
|
|
* Returns modulus of input.
|
|
*
|
|
* @param input Input value to wrap.
|
|
* @param minimumInput The minimum value expected from the input.
|
|
* @param maximumInput The maximum value expected from the input.
|
|
*/
|
|
template <typename T>
|
|
constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
|
|
T modulus = maximumInput - minimumInput;
|
|
|
|
// Wrap input if it's above the maximum input
|
|
int numMax = (input - minimumInput) / modulus;
|
|
input -= numMax * modulus;
|
|
|
|
// Wrap input if it's below the minimum input
|
|
int numMin = (input - maximumInput) / modulus;
|
|
input -= numMin * modulus;
|
|
|
|
return input;
|
|
}
|
|
|
|
/**
|
|
* Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
|
|
*
|
|
* @param angle Angle to wrap.
|
|
*/
|
|
constexpr units::radian_t AngleModulus(units::radian_t angle) {
|
|
return InputModulus<units::radian_t>(angle, units::radian_t{-wpi::math::pi},
|
|
units::radian_t{wpi::math::pi});
|
|
}
|
|
|
|
} // namespace frc
|