diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py index ae0fabb87..c8eb9e828 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/__init__.py @@ -18,3 +18,5 @@ from .surface import Surface from .torpedos_test import TorpedosTest from .vampire_slayer import VampireSlayer +from .start_signal import StartSignal + diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py index 6eafca064..76ce43966 100644 --- a/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/pose_editor.py @@ -444,6 +444,14 @@ def turn_vec_towards(self, body_vec, towards_point): def turn_vec_towards_rel(self, body_vec, towards_rel_point) -> PoseEditor: return self.set_orientation(triad((UP, towards_rel_point), (UP, body_vec))) + + def set_roll_pitch_yaw(self, roll: float, pitch: float, yaw: float) -> PoseEditor: + return self.set_orientation( + transformations.quaternion_multiply( + transformations.quaternion_from_euler(roll, pitch, yaw), + self.orientation, + ), + ) def yaw_left(self, angle: float) -> PoseEditor: """ diff --git a/SubjuGator/command/subjugator_missions/subjugator_missions/start_signal.py b/SubjuGator/command/subjugator_missions/subjugator_missions/start_signal.py new file mode 100644 index 000000000..80b29f6a3 --- /dev/null +++ b/SubjuGator/command/subjugator_missions/subjugator_missions/start_signal.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +from .sub_singleton import SubjuGatorMission +import numpy as np +import math + + +class StartSignal(SubjuGatorMission): + + buoy_positions = [[5, 10, 3], [6, 8, 1], [3, 12, 1]] # x, y, z displacements + + async def run(self, args): + for i in range(len(self.buoy_positions)): + pitch_angle = np.arctan(self.buoy_positions[i][2]/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} with yaw {math.degrees(yaw_angle)} and pitch {math.degrees(pitch_angle)}") + + rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) + # Multiply quaternarion by 1? (makes sure angles dont overwrite) + await self.go(rotate) + + # does this actually go forward relative to orientation + 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])) + ) + + + self.send_feedback("Undo Rotation") + + pitch_angle = -np.arctan(self.buoy_positions[i][2]/self.buoy_positions[i][1]) + yaw_angle = -np.arctan(self.buoy_positions[i][2]/self.buoy_positions[i][0]) + rotate = self.move().set_roll_pitch_yaw(0, pitch_angle, yaw_angle) + await self.go(rotate) + + # 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][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 + # self.move() + # .yaw_left(yaw_angle) if self.buoy_positions[i][0] < 0 else self.move().yaw_right(yaw_angle) + # # Pitch up if angle is postive, pitch down is angle is negative + # .pitch_up(pitch_angle) if pitch_angle > 0 else self.move().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]) + # ) + + # async def run(self, args): + # for i in len(self.buoy_positions): + # self.send_feedback(f"Travelling to Buoy {i}") + # await self.go( + # self.move() + # .right(self.buoy_positions[i][0]) + # .up(self.buoy_positions[i][1].forward(self.buoy_positions[i][2])), + # ) + # self.send_feedback("Back to Origin") + # await self.go( + # self.move() + # .right(-self.buoy_positions[i][0]) + # .up(-self.buoy_positions[i][1].forward(-self.buoy_positions[i][2])), + # ) + +