commit c8012caf11ca4666b9632b163d67bc0ba7f587b4
author Tyler Chatow <tchatow@gmail.com> Mon Feb 18 22:33:01 2019 -0800
committer Tyler Chatow <tchatow@gmail.com> Tue Feb 19 18:16:33 2019 -0800
tree 63e6aec42b06cc5ec9812397f795b05097fe3ae3
parent b4e5f42278aa5f769863529fec414537ff3399f4

Modify joystick angle mapper to actual angle

Change-Id: I1587ec41c72038eeab61589a4b1fa68a3286b823

y2019/BUILD

diff --git a/y2019/BUILD b/y2019/BUILD
index 14d0d6f..a4f498b 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -86,6 +86,7 @@
     ],
     deps = [
         "//aos/input:drivetrain_input",
+        "//frc971/zeroing:wrap",
     ],
 )
 

y2019/joystick_angle.cc

diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
index 1923c53..90baf32 100644
--- a/y2019/joystick_angle.cc
+++ b/y2019/joystick_angle.cc
@@ -1,9 +1,20 @@
+#include <cmath>
+
+#include "frc971/zeroing/wrap.h"
 #include "y2019/joystick_angle.h"
 
 namespace y2019 {
 namespace input {
 namespace joysticks {
 
+using ::frc971::zeroing::Wrap;
+
+bool AngleCloseTo(double angle, double near, double range) {
+  double wrapped_angle = Wrap(near, angle, 2 * M_PI);
+
+  return ::std::abs(wrapped_angle - near) < range;
+}
+
 JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
                                   const JoystickAxis &y_axis,
                                   const Data &data) {
@@ -11,26 +22,30 @@
 }
 
 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;
-    }
+  const float magnitude = hypot(x_axis, y_axis);
+
+  if (magnitude < 0.5) {
+    return JoystickAngle::kDefault;
   }
+
+  double angle = atan2(y_axis, x_axis);
+
+  if (AngleCloseTo(angle, M_PI / 3, M_PI / 6)) {
+    return JoystickAngle::kUpperRight;
+  } else if (AngleCloseTo(angle, 2 * M_PI / 3, M_PI / 6)) {
+    return JoystickAngle::kUpperLeft;
+  } else if (AngleCloseTo(angle, M_PI, M_PI / 6)) {
+    return JoystickAngle::kMiddleLeft;
+  } else if (AngleCloseTo(angle, 0, M_PI / 6)) {
+    return JoystickAngle::kMiddleRight;
+  } else if (AngleCloseTo(angle, -M_PI / 3, M_PI / 6)) {
+    return JoystickAngle::kLowerRight;
+  } else if (AngleCloseTo(angle, -2 * M_PI / 3, M_PI / 6)) {
+    return JoystickAngle::kLowerLeft;
+  }
+
   return JoystickAngle::kDefault;
 }
-
 }  // namespace joysticks
 }  // namespace input
 }  // namespace y2019

y2019/joystick_angle.h

diff --git a/y2019/joystick_angle.h b/y2019/joystick_angle.h
index a2c7e36..09ab8af 100644
--- a/y2019/joystick_angle.h
+++ b/y2019/joystick_angle.h
@@ -9,12 +9,7 @@
 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;
-}
+bool AngleCloseTo(double angle, double near, double range);
 
 enum class JoystickAngle {
   kDefault,

y2019/joystick_angle_test.cc

diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
index c352359..dc29004 100644
--- a/y2019/joystick_angle_test.cc
+++ b/y2019/joystick_angle_test.cc
@@ -7,12 +7,12 @@
 using y2019::input::joysticks::GetJoystickPosition;
 
 TEST(JoystickAngleTest, JoystickAngleTest) {
-  EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, -0.75));
+  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::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::kLowerLeft, GetJoystickPosition(-0.75, -0.75));
 
   EXPECT_EQ(JoystickAngle::kDefault, GetJoystickPosition(0, 0));
 }