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

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

@@ -0,0 +1,33 @@
// 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() {}
}

15
wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/AngleStatistics.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.estimator;
import edu.wpi.first.wpiutil.math.MathUtil;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Num;
import edu.wpi.first.wpiutil.math.numbers.N1;
@@ -26,7 +27,7 @@ public final class AngleStatistics {
public static <S extends Num> Matrix<S, N1> angleResidual(
Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
Matrix<S, N1> ret = a.minus(b);
ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
return ret;
}
@@ -52,7 +53,7 @@ public final class AngleStatistics {
public static <S extends Num> Matrix<S, N1> angleAdd(
Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
Matrix<S, N1> ret = a.plus(b);
ret.set(angleStateIdx, 0, normalizeAngle(ret.get(angleStateIdx, 0)));
ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
return ret;
}
@@ -68,16 +69,6 @@ public final class AngleStatistics {
return (a, b) -> angleAdd(a, b, angleStateIdx);
}
static double normalizeAngle(double angle) {
final double tau = 2 * Math.PI;
angle -= Math.floor(angle / tau) * tau;
if (angle > Math.PI) {
angle -= tau;
}
return angle;
}
/**
* Computes the mean of sigmas with the weights Wm while computing a special angle mean for a
* select row.