mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
8f1f64ffb6
Also update copyright to include "and other WPILib contributors" and clarify license referral language to not be restricted to FIRST teams.
40 lines
1.1 KiB
C++
40 lines
1.1 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 <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
|