Refactor Embedded Data On Robot Diagnostics CLI

src/software/embedded/robot_diagnostics_cli/BUILD

@@ -61,3 +61,8 @@ py_library(
     name = "embedded_data",
     srcs = [":embedded_data.py"],
 )
+
+py_library(
+    name = "primitive_factory",
+    srcs = ["primitive_factory.py"],
+)

src/software/embedded/robot_diagnostics_cli/embedded_data.py

@@ -1,18 +1,5 @@
-import math
 import os
 import tomllib
-from software.py_constants import *
-from proto.import_all_protos import *
-from software.embedded.constants.py_constants import (
-    ROBOT_MAX_ANG_SPEED_RAD_PER_S,
-    ROBOT_MAX_SPEED_M_PER_S,
-    MAX_FORCE_DRIBBLER_SPEED_RPM,
-)
-from software.py_constants import (
-    AUTO_CHIP_DISTANCE_DEFAULT_M,
-    AUTO_KICK_SPEED_DEFAULT_M_PER_S,
-    WHEEL_ROTATION_MAX_SPEED_M_PER_S,
-)
 
 
 class EmbeddedData:
@@ -72,145 +59,3 @@ def get_battery_volt(self) -> str:
 
     def get_cap_volt(self) -> str:
         return self._get_value(ROBOT_CAPACITOR_VOLTAGE_CONFIG_KEY)
-
-    def __clamp(self, val: float, min_val: float, max_val: float) -> float:
-        """Simple Math Clamp function (Faster than numpy & fewer dependencies)
-        :param val: Value to clamp
-        :param min_val: Minimum (Lower) Bound
-        :param max_val: Maximum (Upper) Bound
-        """
-        return min(max(val, min_val), max_val)
-
-    # TODO (#3435): Refactor Get Primitives
-    def get_rotate_primitive(self, velocity: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a velocity
-        :param velocity: Angular Velocity to rotate the robot
-        """
-        velocity = self.__clamp(
-            velocity, -ROBOT_MAX_ANG_SPEED_RAD_PER_S, ROBOT_MAX_ANG_SPEED_RAD_PER_S
-        )
-        motor_control_primitive = MotorControl()
-        motor_control_primitive.direct_velocity_control.angular_velocity.radians_per_second = velocity
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=motor_control_primitive, power_control=PowerControl()
-        )
-        return Primitive(direct_control=direct_control_primitive)
-
-    def get_move_primitive(self, angle: float, speed: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a speed.
-        :param angle: Angle to move the robot at in degrees
-        :param speed: Speed to move the robot at in m/s
-        """
-        speed = self.__clamp(val=speed, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S)
-        motor_control_primitive = MotorControl()
-        motor_control_primitive.direct_velocity_control.velocity.x_component_meters = (
-            speed * math.cos(angle)
-        )
-        motor_control_primitive.direct_velocity_control.velocity.y_component_meters = (
-            speed * math.sin(angle)
-        )
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=motor_control_primitive, power_control=PowerControl()
-        )
-        return Primitive(direct_control=direct_control_primitive)
-
-    def get_chip_primitive(self, auto: bool, distance: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a distance and state.
-        :param auto: Determines whether auto-chip is enabled
-        :param distance: Distance to chip the "ball"
-        """
-        distance = self.__clamp(
-            val=distance, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S
-        )
-        power_control_primitive = PowerControl()
-        if not auto:
-            power_control_primitive.chicker.chip_distance_meters = distance
-        else:
-            power_control_primitive.chicker.auto_chip_or_kick.autochip_distance_meters = AUTO_CHIP_DISTANCE_DEFAULT_M
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=MotorControl(), power_control=power_control_primitive
-        )
-        return Primitive(direct_control=direct_control_primitive)
-
-    def get_zero_power_control_primitive(self) -> Primitive:
-        """Creates a PowerControl primitive with zeroed/default base values"""
-        power_control_primitive = PowerControl()
-        power_control_primitive.geneva_slot = Slot.CENTRE_RIGHT
-        return power_control_primitive
-
-    def get_zero_motor_control_primitive(self) -> Primitive:
-        """Creates a MotorControl primitive with zeroed/default base values"""
-        motor_control_primitive = MotorControl()
-        motor_control_primitive.direct_velocity_control.velocity.x_component_meters = 0
-        motor_control_primitive.direct_velocity_control.velocity.y_component_meters = 0
-        motor_control_primitive.direct_velocity_control.angular_velocity.radians_per_second = 0
-        return motor_control_primitive
-
-    def get_kick_primitive(self, auto: bool, speed: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a speed and state.
-        :param auto: Determines whether auto-kick is enabled
-        :param speed: Speed to kick the "ball" at
-        """
-        speed = self.__clamp(val=speed, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S)
-        power_control_primitive = self.get_zero_power_control_primitive()
-        if not auto:
-            power_control_primitive.chicker.kick_speed_m_per_s = speed
-        else:
-            power_control_primitive.chicker.auto_chip_or_kick.autokick_speed_m_per_s = (
-                AUTO_KICK_SPEED_DEFAULT_M_PER_S
-            )
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=self.get_zero_motor_control_primitive(),
-            power_control=power_control_primitive,
-        )
-        return Primitive(direct_control=direct_control_primitive)
-
-    def get_dribble_primitive(self, velocity: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a velocity.
-        :param velocity: Speed & direction of the dribbler
-        """
-        velocity = self.__clamp(
-            val=velocity,
-            min_val=MAX_FORCE_DRIBBLER_SPEED_RPM,
-            max_val=-MAX_FORCE_DRIBBLER_SPEED_RPM,
-        )
-        motor_control_primitive = self.get_zero_motor_control_primitive()
-        motor_control_primitive.dribbler_speed_rpm = int(velocity)
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=motor_control_primitive, power_control=PowerControl()
-        )
-        return Primitive(direct_control=direct_control_primitive)
-
-    def get_move_wheel_primitive(self, wheels: list[int], velocity: float) -> Primitive:
-        """Prepares and returns the processed direct control primitive given a velocity mapped to
-        wheel_velocity_map = {1: 0, 2: 0, 3: 0, 4: 0} where {1:"NE", 2:"SE", 3:"SW", 4:"NW"}
-        :param wheels: The wheels to rotate
-        :param velocity: The speed & direction to rotate at
-        """
-        wheel_velocity_map = {1: 0, 2: 0, 3: 0, 4: 0}
-        velocity = self.__clamp(
-            val=velocity,
-            min_val=-WHEEL_ROTATION_MAX_SPEED_M_PER_S,
-            max_val=WHEEL_ROTATION_MAX_SPEED_M_PER_S,
-        )
-
-        for wheel in wheels:
-            wheel_velocity_map[wheel] = velocity
-        motor_control_primitive = MotorControl()
-        motor_control_primitive.direct_per_wheel_control.front_left_wheel_velocity = (
-            wheel_velocity_map[1]
-        )
-        motor_control_primitive.direct_per_wheel_control.back_left_wheel_velocity = (
-            wheel_velocity_map[2]
-        )
-        motor_control_primitive.direct_per_wheel_control.front_right_wheel_velocity = (
-            wheel_velocity_map[3]
-        )
-        motor_control_primitive.direct_per_wheel_control.back_right_wheel_velocity = (
-            wheel_velocity_map[4]
-        )
-
-        direct_control_primitive = DirectControlPrimitive(
-            motor_control=motor_control_primitive, power_control=PowerControl()
-        )
-        return Primitive(direct_control=direct_control_primitive)

src/software/embedded/robot_diagnostics_cli/primitive_factory.py

@@ -0,0 +1,135 @@
+import math
+from proto.import_all_protos import *
+from software.embedded.constants.py_constants import (
+    ROBOT_MAX_ANG_SPEED_RAD_PER_S,
+    ROBOT_MAX_SPEED_M_PER_S,
+    MAX_FORCE_DRIBBLER_SPEED_RPM,
+)
+from software.py_constants import (
+    AUTO_CHIP_DISTANCE_DEFAULT_M,
+    AUTO_KICK_SPEED_DEFAULT_M_PER_S,
+    WHEEL_ROTATION_MAX_SPEED_M_PER_S,
+)
+
+
+class PrimitiveFactory:
+    def __init__(self) -> None:
+        pass
+
+    @staticmethod
+    def get_primitive(self, **kwargs) -> Primitive:
+        if len(kwargs) != 1:
+            raise ValueError("Primitive constructor takes exactly 1 argument")
+
+        primitive_type, args = next(iter(kwargs.items()))
+
+        if not isinstance(args, (list, tuple)):
+            raise TypeError(
+                f"Arguments for '{primitive_type}' must be a list or tuple"
+            )
+
+        motor_control_primitive = MotorControl()
+        power_control_primitive = PowerControl()
+
+        match primitive_type:
+            case "rotate": # rotate=[velocity]
+                self._rotate(motor_control_primitive, *args)
+            case "move": # move=[angle, speed]
+                self._move(motor_control_primitive, *args)
+            case "chip": # chip=[auto, distance]
+                self._chip(power_control_primitive, *args)
+            case "kick": # kick=[auto, speed]
+                self._kick(motor_control_primitive, power_control_primitive, *args)
+            case "dribble": # dribble=[velocity]
+                self._dribble(motor_control_primitive, *args)
+            case "move_wheel": # move_wheels=[wheels, velocity]
+                self._move_wheel(motor_control_primitive, *args)
+            case "zero_power": # zero_power=[]
+                self._zero_power(power_control_primitive)
+                return power_control_primitive
+            case "zero_motor": # zero_motor=[]
+                self._zero_motor(motor_control_primitive)
+                return motor_control_primitive
+            case _:
+                raise ValueError(f"Unknown primitive type: '{primitive_type}'")
+        direct_control_primitive = DirectControlPrimitive(
+            motor_control=motor_control_primitive, power_control=power_control_primitive
+        )
+        return Primitive(direct_control=direct_control_primitive)
+
+    def _rotate(self, motor_control_primitive, velocity: float) -> None:
+        velocity = self._clamp(
+            velocity, -ROBOT_MAX_ANG_SPEED_RAD_PER_S, ROBOT_MAX_ANG_SPEED_RAD_PER_S
+        )
+        motor_control_primitive.direct_velocity_control.angular_velocity.radians_per_second = velocity
+
+    def _move(self, motor_control_primitive, angle: float, speed: float) -> None:
+        speed = self._clamp(val=speed, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S)
+        motor_control_primitive.direct_velocity_control.velocity.x_component_meters = (
+            speed * math.cos(angle)
+        )
+        motor_control_primitive.direct_velocity_control.velocity.y_component_meters = (
+            speed * math.sin(angle)
+        )
+
+    def _chip(self, power_control_primitive, auto: bool, distance: float) -> None:
+        distance = self._clamp(
+            val=distance, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S
+        )
+        if not auto:
+            power_control_primitive.chicker.chip_distance_meters = distance
+        else:
+            power_control_primitive.chicker.auto_chip_or_kick.autochip_distance_meters = AUTO_CHIP_DISTANCE_DEFAULT_M
+
+    def _kick(self, motor_control_primitive, power_control_primitive, auto: bool, speed: float) -> None:
+        speed = self._clamp(val=speed, min_val=0, max_val=ROBOT_MAX_SPEED_M_PER_S)
+        self._zero_power(power_control_primitive)
+        self._zero_motor(motor_control_primitive)
+        if not auto:
+            power_control_primitive.chicker.kick_speed_m_per_s = speed
+        else:
+            power_control_primitive.chicker.auto_chip_or_kick.autokick_speed_m_per_s = (
+                AUTO_KICK_SPEED_DEFAULT_M_PER_S
+            )
+
+    def _dribble(self, motor_control_primitive, velocity: float):
+        velocity = self._clamp(
+            val=velocity,
+            min_val=MAX_FORCE_DRIBBLER_SPEED_RPM,
+            max_val=-MAX_FORCE_DRIBBLER_SPEED_RPM,
+        )
+        self._zero_motor(motor_control_primitive)
+        motor_control_primitive.dribbler_speed_rpm = int(velocity)
+
+    def _move_wheel(self, motor_control_primitive, wheels: list[int], velocity: float) -> None:
+        wheel_velocity_map = {1: 0, 2: 0, 3: 0, 4: 0}
+        velocity = self._clamp(
+            val=velocity,
+            min_val=-WHEEL_ROTATION_MAX_SPEED_M_PER_S,
+            max_val=WHEEL_ROTATION_MAX_SPEED_M_PER_S,
+        )
+        for wheel in wheels:
+            wheel_velocity_map[wheel] = velocity
+        motor_control_primitive.direct_per_wheel_control.front_left_wheel_velocity = (
+            wheel_velocity_map[1]
+        )
+        motor_control_primitive.direct_per_wheel_control.back_left_wheel_velocity = (
+            wheel_velocity_map[2]
+        )
+        motor_control_primitive.direct_per_wheel_control.front_right_wheel_velocity = (
+            wheel_velocity_map[3]
+        )
+        motor_control_primitive.direct_per_wheel_control.back_right_wheel_velocity = (
+            wheel_velocity_map[4]
+        )
+
+    def _zero_power(self, power_control_primitive) -> None:
+        power_control_primitive.geneva_slot = Slot.CENTRE_RIGHT
+
+    def _zero_motor(self, motor_control_primitive) -> None:
+        motor_control_primitive.direct_velocity_control.velocity.x_component_meters = 0
+        motor_control_primitive.direct_velocity_control.velocity.y_component_meters = 0
+        motor_control_primitive.direct_velocity_control.angular_velocity.radians_per_second = 0
+
+    def _clamp(self, val: float, min_val: float, max_val: float) -> float:
+        return min(max(val, min_val), max_val)

src/software/embedded/robot_diagnostics_cli/robot_diagnostics_cli.py

@@ -14,6 +14,9 @@
 from software.embedded.robot_diagnostics_cli.embedded_communication import (
     EmbeddedCommunication,
 )
+from software.embedded.robot_diagnostics_cli.primitive_factory import (
+    PrimitiveFactory,
+)
 from proto.import_all_protos import *
 from software.embedded.constants.py_constants import (
     DEFAULT_PRIMITIVE_DURATION,
@@ -223,7 +226,7 @@ def rotate(
         description = f"Rotating at {velocity} rad/s for {duration_seconds} seconds"
         self.embedded_communication.run_primitive_over_time(
             duration_seconds,
-            self.embedded_data.get_rotate_primitive(velocity),
+            PrimitiveFactory.get_primitive(self, rotate=[velocity]),
             description,
         )
 
@@ -252,7 +255,7 @@ def move(
         )
         self.embedded_communication.run_primitive_over_time(
             duration_seconds,
-            self.embedded_data.get_move_primitive(angle, speed),
+            PrimitiveFactory.get_primitive(self, move=[angle, speed]),
             description,
         )
 
@@ -280,7 +283,7 @@ def chip(
             if not auto
             else f"Auto Chip Enabled for {duration_seconds} seconds"
         )
-        primitive = self.embedded_data.get_kick_primitive(auto, distance)
+        primitive = PrimitiveFactory.get_primitive(self, kick=[auto, distance])
         if auto:
             self.embedded_communication.run_primitive_over_time(
                 duration_seconds, primitive, description
@@ -313,15 +316,15 @@ def kick(
             if not auto
             else f"Auto Kick Enabled for {duration_seconds} seconds"
         )
-        primitive = self.embedded_data.get_kick_primitive(auto, speed)
+        primitive = PrimitiveFactory.get_primitive(self, kick=[auto, speed])
         if auto:
             self.embedded_communication.run_primitive_over_time(
                 duration_seconds, primitive, description
             )
         else:
             zero_direct_control_primitive = DirectControlPrimitive(
-                motor_control=self.embedded_data.get_zero_motor_control_primitive(),
-                power_control=self.embedded_data.get_zero_power_control_primitive(),
+                motor_control=PrimitiveFactory.get_primitive(self, zero_motor=[]),
+                power_control=PrimitiveFactory.get_primitive(self, zero_power=[]),
             )
             self.embedded_communication.run_primitive(primitive)
             print(description)
@@ -354,7 +357,7 @@ def dribble(
         )
         self.embedded_communication.run_primitive_over_time(
             duration_seconds,
-            self.embedded_data.get_dribble_primitive(velocity),
+            PrimitiveFactory.get_primitive(self, dribble=[velocity]),
             description,
         )
 
@@ -387,7 +390,7 @@ def move_wheel(
         )
         self.embedded_communication.run_primitive_over_time(
             duration_seconds,
-            self.embedded_data.get_move_wheel_primitive(wheels, velocity),
+            PrimitiveFactory.get_primitive(self, move_wheels=[wheels, velocity]),
             description,
         )