Add joystick angle to position mapper
Change-Id: I427b02b656662aad7726ca264d73900f388f9ae3
diff --git a/y2019/BUILD b/y2019/BUILD
index ab4a842..2ec06e6 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -74,6 +74,30 @@
],
)
+cc_library(
+ name = "joystick_angle",
+ srcs = [
+ "joystick_angle.cc",
+ ],
+ hdrs = [
+ "joystick_angle.h",
+ ],
+ deps = [
+ "//aos/input:drivetrain_input",
+ ],
+)
+
+cc_test(
+ name = "joystick_angle_test",
+ srcs = [
+ "joystick_angle_test.cc",
+ ],
+ deps = [
+ ":joystick_angle",
+ "//aos/testing:googletest",
+ ],
+)
+
cc_binary(
name = "joystick_reader",
srcs = [
diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
new file mode 100644
index 0000000..1923c53
--- /dev/null
+++ b/y2019/joystick_angle.cc
@@ -0,0 +1,36 @@
+#include "y2019/joystick_angle.h"
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+ const JoystickAxis &y_axis,
+ const Data &data) {
+ return GetJoystickPosition(data.GetAxis(x_axis), data.GetAxis(y_axis));
+}
+
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis) {
+ if (x_axis > kJoystickRight) {
+ if (y_axis < kJoystickDown) {
+ return JoystickAngle::kUpperRight;
+ } else if (y_axis > kJoystickUp) {
+ return JoystickAngle::kLowerRight;
+ } else {
+ return JoystickAngle::kMiddleRight;
+ }
+ } else if (x_axis < kJoystickLeft) {
+ if (y_axis < kJoystickDown) {
+ return JoystickAngle::kUpperLeft;
+ } else if (y_axis > kJoystickUp) {
+ return JoystickAngle::kLowerLeft;
+ } else {
+ return JoystickAngle::kMiddleLeft;
+ }
+ }
+ return JoystickAngle::kDefault;
+}
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
diff --git a/y2019/joystick_angle.h b/y2019/joystick_angle.h
new file mode 100644
index 0000000..a2c7e36
--- /dev/null
+++ b/y2019/joystick_angle.h
@@ -0,0 +1,37 @@
+#ifndef Y2019_JOYSTICK_ANGLE_H_
+#define Y2019_JOYSTICK_ANGLE_H_
+
+#include "aos/input/driver_station_data.h"
+
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::Data;
+
+namespace y2019 {
+namespace input {
+namespace joysticks {
+namespace {
+constexpr double kJoystickLeft = -0.5;
+constexpr double kJoystickRight = 0.5;
+constexpr double kJoystickUp = 0.5;
+constexpr double kJoystickDown = -0.5;
+}
+
+enum class JoystickAngle {
+ kDefault,
+ kUpperRight,
+ kMiddleRight,
+ kLowerRight,
+ kUpperLeft,
+ kMiddleLeft,
+ kLowerLeft
+};
+
+JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
+ const JoystickAxis &y_axis, const Data &data);
+JoystickAngle GetJoystickPosition(float x_axis, float y_axis);
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2019
+
+#endif // Y2019_JOYSTICK_ANGLE_H_
diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
new file mode 100644
index 0000000..c352359
--- /dev/null
+++ b/y2019/joystick_angle_test.cc
@@ -0,0 +1,18 @@
+#include "y2019/joystick_angle.h"
+#include <iostream>
+#include "aos/input/driver_station_data.h"
+#include "gtest/gtest.h"
+
+using y2019::input::joysticks::JoystickAngle;
+using y2019::input::joysticks::GetJoystickPosition;
+
+TEST(JoystickAngleTest, JoystickAngleTest) {
+ EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kMiddleRight, GetJoystickPosition(0.75, 0));
+ EXPECT_EQ(JoystickAngle::kLowerRight, GetJoystickPosition(0.75, 0.75));
+ EXPECT_EQ(JoystickAngle::kUpperLeft, GetJoystickPosition(-0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kMiddleLeft, GetJoystickPosition(-0.75, 0));
+ EXPECT_EQ(JoystickAngle::kLowerLeft, GetJoystickPosition(-0.75, 0.75));
+
+ EXPECT_EQ(JoystickAngle::kDefault, GetJoystickPosition(0, 0));
+}