Refactored HandleDrivetrain from joystick_reader to aos_input.
Change-Id: I4ed796cf90e698ed2198f0da4a2b75e88e28c434
diff --git a/aos/common/commonmath.h b/aos/common/commonmath.h
index f9337b2..acbcb1c 100644
--- a/aos/common/commonmath.h
+++ b/aos/common/commonmath.h
@@ -1,6 +1,8 @@
#ifndef AOS_COMMON_MATH_H_
#define AOS_COMMON_MATH_H_
+#include <cmath>
+
namespace aos {
// Clips a value so that it is in [min, max]
@@ -22,6 +24,20 @@
}
}
+// Adds deadband to provided value. deadband is the region close to the origin
+// to add the deadband to, and max is the maximum input value used to re-scale
+// the output after adding the deadband.
+static inline double Deadband(double value, const double deadband,
+ const double max) {
+ if (::std::abs(value) < deadband) {
+ value = 0.0;
+ } else if (value > 0.0) {
+ value = (value - deadband) / (max - deadband);
+ } else {
+ value = (value + deadband) / (max - deadband);
+ }
+ return value;
+}
} // namespace aos
#endif // AOS_COMMON_MATH_H_