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

[wpimath] Deduplicate angle modulus functions (#2998)

Browse Source 
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.
This commit is contained in:
Tyler Veness
2021-01-01 16:22:00 -08:00
committed by GitHub
parent bf8c0da4be
commit 62f0f8190d
23 changed files with 210 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
View File

@@ -1,39 +0,0 @@
// 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 <cmath>
#include <type_traits>
#include <units/math.h>
namespace frc {
/**
* Returns modulus of error where error is the difference between the reference
* and a measurement.
*
* @param reference Reference input of a controller.
* @param measurement The current measurement.
* @param minimumInput The minimum value expected from the input.
* @param maximumInput The maximum value expected from the input.
*/
template <typename T>
T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
T error = reference - measurement;
T modulus = maximumInput - minimumInput;
// Wrap error above maximum input
int numMax = (error + maximumInput) / modulus;
error -= numMax * modulus;
// Wrap error below minimum input
int numMin = (error + minimumInput) / modulus;
error -= numMin * modulus;
return error;
}
} // namespace frc

10
wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -11,7 +11,7 @@
#include <units/time.h>
#include "frc/controller/ControllerUtil.h"
#include "frc/MathUtil.h"
#include "frc/controller/PIDController.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableBuilder.h"
@@ -253,10 +253,10 @@ class ProfiledPIDController
double Calculate(Distance_t measurement) {
if (m_controller.IsContinuousInputEnabled()) {
// Get error which is smallest distance between goal and measurement
auto goalMinDistance = frc::GetModulusError<Distance_t>(
m_goal.position, measurement, m_minimumInput, m_maximumInput);
auto setpointMinDistance = frc::GetModulusError<Distance_t>(
m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
auto goalMinDistance = frc::InputModulus<Distance_t>(
m_goal.position - measurement, m_minimumInput, m_maximumInput);
auto setpointMinDistance = frc::InputModulus<Distance_t>(
m_setpoint.position - measurement, m_minimumInput, m_maximumInput);
// Recompute the profile goal with the smallest error, thus giving the
// shortest path. The goal may be outside the input range after this