added the new rotation approach for buoys

SubjuGator/command/subjugator_missions/subjugator_missions/start_signal.py

@@ -2,7 +2,7 @@
 from .sub_singleton import SubjuGatorMission
 import numpy as np
 
-
+""""
 class StartSignal(SubjuGatorMission):
     buoy_positions = [[5, 10, 3], [6, 8, 5], [3, 12, 10]]  # x, y, z displacements
 
@@ -20,15 +20,30 @@ async def run(self, args):
                 .right(-self.buoy_positions[i][0])
                 .up(-self.buoy_positions[i][1].forward(-self.buoy_positions[i][2])),
             )
+"""
 
     async def run_rotation(self, args):
         for i in len(self.args):
             pitch_angle = np.arctan(self.buoy_positions[i][2]/self.buoy_positions[i][1])
-            yaw_angle = np.arctan(self.buoy_positions[i][0]/self.buoy_positions[i][1])
+            yaw_angle = np.arctan(self.buoy_positions[i][2]/self.buoy_positions[i][0])
             self.send_feedback(f"Rotating towards Buoy {i}")
             await self.go(
                 # Yaw to the right if the x distance is positive and to the left if negative
-                # Yaw to left otherwise
-                #
+                self.move()
+                .yaw_left(yaw_angle) if self.buoy_positions[i][0] < 0 else .yaw_right(yaw_angle)
+
                 # If the pitch angle is positive, pitch up, if negative, pitch down
+                .pitch_up(pitch_angle) if pitch_angle > 0 else .pitch_down(abs(pitch_angle))
             )
+            self.send_feedback(f"Traveling forward to buoy {i}")
+            await self.go(
+                self.move(
+                    .forward(self.buoy_positions[i][2])
+                )
+            )
+            self.send_feedback("Back to Origin")
+            await self.go(
+                self.move()
+                .forward(-self.buoy_positions[i][2])
+            )
+