Handle door navigation and turnarounds

tests/test_farming_pitch.py

@@ -2,8 +2,8 @@ import unittest
 
 from munch import Munch
 
-from mosfet.bot import look_direction_from_eye, update_pitch
-from mosfet.vector import Point3D
+from mosfet.bot import look_direction_from_eye, update_pitch, update_yaw
+from mosfet.vector import Point3D, Vector3D
 
 
 class LookDirection:
@@ -38,6 +38,26 @@ class TestFarmingPitch(unittest.TestCase):
             update_pitch(state, directly_below)
         self.assertEqual(state.pitch, 45.0)
 
+    def test_fixed_yaw_turn_returns_over_the_same_180_degree_arc(self):
+        state = Munch(yaw=-90.0)
+        no_look_direction = Vector3D((0.0, 0.0, 0.0))
+
+        for _ in range(6):
+            update_yaw(state, no_look_direction, fixed_target=-270.0)
+        self.assertEqual(state.yaw, -270.0)
+
+        for _ in range(6):
+            update_yaw(state, no_look_direction, fixed_target=-90.0)
+        self.assertEqual(state.yaw, -90.0)
+
+    def test_yaw_turns_across_wrap_without_unbounded_rotation(self):
+        state = Munch(yaw=630.0)
+        update_yaw(state, Vector3D((0, 0, -1)))
+
+        self.assertEqual(state.yaw, -60.0)
+        self.assertGreaterEqual(state.yaw, -180.0)
+        self.assertLess(state.yaw, 180.0)
+
     def test_pitch_tracks_look_target_instead_of_a_fixed_farming_angle(self):
         state = Munch(pitch=0.0)
         direction_to_crop = LookDirection(45.0)