diff --git a/src/software/embedded/robot_diagnostics_cli/BUILD b/src/software/embedded/robot_diagnostics_cli/BUILD index ae24d0cbf7..4cb0bd324a 100644 --- a/src/software/embedded/robot_diagnostics_cli/BUILD +++ b/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"], +) diff --git a/src/software/embedded/robot_diagnostics_cli/embedded_data.py b/src/software/embedded/robot_diagnostics_cli/embedded_data.py index 1633c19036..0585f1b8d8 100644 --- a/src/software/embedded/robot_diagnostics_cli/embedded_data.py +++ b/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) diff --git a/src/software/embedded/robot_diagnostics_cli/primitive_factory.py b/src/software/embedded/robot_diagnostics_cli/primitive_factory.py new file mode 100644 index 0000000000..cec061fffb --- /dev/null +++ b/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) diff --git a/src/software/embedded/robot_diagnostics_cli/robot_diagnostics_cli.py b/src/software/embedded/robot_diagnostics_cli/robot_diagnostics_cli.py index 3341780204..116324b0b4 100644 --- a/src/software/embedded/robot_diagnostics_cli/robot_diagnostics_cli.py +++ b/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, )