Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +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

33
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
View File

@@ -1,33 +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.
package edu.wpi.first.wpilibj.controller;
public final class ControllerUtil {
/**
* 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.
*/
public static double getModulusError(
double reference, double measurement, double minimumInput, double maximumInput) {
double error = reference - measurement;
double modulus = maximumInput - minimumInput;
// Wrap error above maximum input
int numMax = (int) ((error + maximumInput) / modulus);
error -= numMax * modulus;
// Wrap error below minimum input
int numMin = (int) ((error + minimumInput) / modulus);
error -= numMin * modulus;
return error;
}
private ControllerUtil() {}
}

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -201,7 +201,7 @@ public class PIDController implements Sendable, AutoCloseable {
double positionError;
if (m_continuous) {
positionError =
ControllerUtil.getModulusError(m_setpoint, m_measurement, m_minimumInput, m_maximumInput);
MathUtil.inputModulus(m_setpoint - m_measurement, m_minimumInput, m_maximumInput);
} else {
positionError = m_setpoint - m_measurement;
}
@@ -308,7 +308,7 @@ public class PIDController implements Sendable, AutoCloseable {
if (m_continuous) {
m_positionError =
ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput, m_maximumInput);
MathUtil.inputModulus(m_setpoint - measurement, m_minimumInput, m_maximumInput);
} else {
m_positionError = m_setpoint - measurement;
}

7
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
View File

@@ -9,6 +9,7 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpiutil.math.MathUtil;
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
@@ -270,11 +271,9 @@ public class ProfiledPIDController implements Sendable {
if (m_controller.isContinuousInputEnabled()) {
// Get error which is smallest distance between goal and measurement
double goalMinDistance =
ControllerUtil.getModulusError(
m_goal.position, measurement, m_minimumInput, m_maximumInput);
MathUtil.inputModulus(m_goal.position - measurement, m_minimumInput, m_maximumInput);
double setpointMinDistance =
ControllerUtil.getModulusError(
m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
MathUtil.inputModulus(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 operation, but that's OK because the controller