Added per-motor control to Thunderscope diagnostics

src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py

@@ -5,6 +5,14 @@
 from software.thunderscope.common import common_widgets
 from proto.import_all_protos import *
 from software.thunderscope.proto_unix_io import ProtoUnixIO
+from enum import IntEnum
+
+
+class ControlMode(IntEnum):
+    """Enum for the 2 drive modes (direct velocity and per-motor)"""
+
+    VELOCITY = 0
+    MOTOR = 1
 
 
 class DriveAndDribblerWidget(QWidget):
@@ -16,32 +24,56 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None:
         self.input_a = time.time()
         self.constants = tbots_cpp.create2021RobotConstants()
         QWidget.__init__(self)
+
         layout = QVBoxLayout()
+        self.control_widget = QStackedWidget()
 
         self.proto_unix_io = proto_unix_io
 
-        # Add widgets to layout
-        layout.addWidget(self.setup_direct_velocity("Drive"))
-        layout.addWidget(self.setup_dribbler("Dribbler"))
+        # create swappable widget system using stacked widgets
+        self.direct_velocity_widget = self.setup_direct_velocity(
+            "Control - Direct Velocity"
+        )
+        self.per_motor_widget = self.setup_per_motor("Control - Per Motor")
+        self.control_widget.addWidget(self.direct_velocity_widget)
+        self.control_widget.addWidget(self.per_motor_widget)
 
-        self.enabled = True
+        layout.addWidget(self.setup_drive_switch_radio("Drive Mode Switch"))
+        layout.addWidget(self.control_widget)
+        layout.addWidget(self.setup_dribbler("Dribbler"))
 
+        # default to direct velocity
+        self.use_direct_velocity.click()
         self.setLayout(layout)
+        self.toggle_dribbler_sliders(True)
 
     def refresh(self) -> None:
         """Refresh the widget and send the a MotorControl message with the current values"""
         motor_control = MotorControl()
         motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value())
-
-        motor_control.direct_velocity_control.velocity.x_component_meters = (
-            self.x_velocity_slider.value()
-        )
-        motor_control.direct_velocity_control.velocity.y_component_meters = (
-            self.y_velocity_slider.value()
-        )
-        motor_control.direct_velocity_control.angular_velocity.radians_per_second = (
-            self.angular_velocity_slider.value()
-        )
+        if self.control_mode == ControlMode.MOTOR:
+            motor_control.ClearField("direct_per_wheel_control")
+            motor_control.direct_velocity_control.velocity.x_component_meters = (
+                self.x_velocity_slider.value()
+            )
+            motor_control.direct_velocity_control.velocity.y_component_meters = (
+                self.y_velocity_slider.value()
+            )
+            motor_control.direct_velocity_control.angular_velocity.radians_per_second = self.angular_velocity_slider.value()
+        else:
+            motor_control.ClearField("direct_velocity_control")
+            motor_control.direct_per_wheel_control.front_left_wheel_velocity = (
+                self.front_left_motor_slider.value()
+            )
+            motor_control.direct_per_wheel_control.front_right_wheel_velocity = (
+                self.front_right_motor_slider.value()
+            )
+            motor_control.direct_per_wheel_control.back_left_wheel_velocity = (
+                self.back_left_motor_slider.value()
+            )
+            motor_control.direct_per_wheel_control.back_right_wheel_velocity = (
+                self.back_right_motor_slider.value()
+            )
 
         self.proto_unix_io.send_proto(MotorControl, motor_control)
 
@@ -54,6 +86,31 @@ def value_change(self, value: float) -> str:
         value_str = "%.2f" % value
         return value_str
 
+    def setup_drive_switch_radio(self, title: str) -> QGroupBox:
+        """Create a radio button widget to switch between per-motor and velocity drive modes
+
+        :param title: vbox name
+        """
+        group_box = QGroupBox(title)
+        vbox = QVBoxLayout()
+        self.connect_options_group = QButtonGroup()
+        radio_button_names = ["Velocity Control", "Per Motor Control"]
+        self.connect_options_box, self.connect_options = common_widgets.create_radio(
+            radio_button_names, self.connect_options_group
+        )
+        self.use_direct_velocity = self.connect_options[ControlMode.VELOCITY]
+        self.use_per_motor = self.connect_options[ControlMode.MOTOR]
+        self.use_direct_velocity.clicked.connect(
+            lambda: self.toggle_control_mode(ControlMode.VELOCITY)
+        )
+        self.use_per_motor.clicked.connect(
+            lambda: self.toggle_control_mode(ControlMode.MOTOR)
+        )
+        vbox.addWidget(self.connect_options_box)
+        group_box.setLayout(vbox)
+
+        return group_box
+
     def setup_direct_velocity(self, title: str) -> QGroupBox:
         """Create a widget to control the direct velocity of the robot's motors
 
@@ -121,6 +178,92 @@ def setup_direct_velocity(self, title: str) -> QGroupBox:
 
         return group_box
 
+    def setup_per_motor(self, title: str) -> QGroupBox:
+        """Create a widget to control the rotation rate of each motor
+
+        :param title: the name of the group box
+        """
+        group_box = QGroupBox(title)
+        dbox = QVBoxLayout()
+
+        (
+            fl_layout,
+            self.front_left_motor_slider,
+            self.front_left_motor_label,
+        ) = common_widgets.create_float_slider(
+            "Front Left Motor (m/s)",
+            2,
+            -self.constants.robot_max_speed_m_per_s,
+            self.constants.robot_max_speed_m_per_s,
+            1,
+        )
+        (
+            fr_layout,
+            self.front_right_motor_slider,
+            self.front_right_motor_label,
+        ) = common_widgets.create_float_slider(
+            "Front Right Motor (m/s)",
+            2,
+            -self.constants.robot_max_speed_m_per_s,
+            self.constants.robot_max_speed_m_per_s,
+            1,
+        )
+        (
+            bl_layout,
+            self.back_left_motor_slider,
+            self.back_left_motor_label,
+        ) = common_widgets.create_float_slider(
+            "Back Left Motor (m/s)",
+            2,
+            -self.constants.robot_max_speed_m_per_s,
+            self.constants.robot_max_speed_m_per_s,
+            1,
+        )
+        (
+            br_layout,
+            self.back_right_motor_slider,
+            self.back_right_motor_label,
+        ) = common_widgets.create_float_slider(
+            "Back Right Motor (m/s)",
+            2,
+            -self.constants.robot_max_speed_m_per_s,
+            self.constants.robot_max_speed_m_per_s,
+            1,
+        )
+
+        # Add listener functions for each motor's slider to update labels with slider values
+        common_widgets.enable_slider(
+            self.front_left_motor_slider, self.front_left_motor_label, self.value_change
+        )
+        common_widgets.enable_slider(
+            self.front_right_motor_slider,
+            self.front_right_motor_label,
+            self.value_change,
+        )
+        common_widgets.enable_slider(
+            self.back_left_motor_slider, self.back_left_motor_label, self.value_change
+        )
+        common_widgets.enable_slider(
+            self.back_right_motor_slider, self.back_right_motor_label, self.value_change
+        )
+
+        # Stop and Reset button for per-motor sliders
+        self.stop_and_reset_per_motor = QPushButton("Stop and Reset")
+        self.stop_and_reset_per_motor.clicked.connect(self.reset_motor_sliders)
+
+        # Adding layouts to the box
+        dbox.addLayout(fl_layout)
+        dbox.addLayout(fr_layout)
+        dbox.addLayout(bl_layout)
+        dbox.addLayout(br_layout)
+        dbox.addWidget(
+            self.stop_and_reset_per_motor, alignment=Qt.AlignmentFlag.AlignCenter
+        )
+
+        group_box.setLayout(dbox)
+
+        return group_box
+
     def setup_dribbler(self, title: str) -> QGroupBox:
         """Create a widget to control the dribbler RPM
 
@@ -159,79 +302,126 @@ def setup_dribbler(self, title: str) -> QGroupBox:
 
         return group_box
 
-    def toggle_all(self, enable: bool) -> None:
-        """Disables or enables all sliders and buttons depending on boolean parameter
+    def toggle_control_mode(self, use_control_mode: IntEnum) -> None:
+        """Switches between 'Direct Velocity' and 'Per Motor' drive modes.
 
-        Updates listener functions and stylesheets accordingly
+        :param use_control_mode: ControlMode.VELOCITY or ControlMode.MOTOR, switch to that mode.
+        """
+        self.control_mode = use_control_mode
+        # reset sliders
+        self.reset_motor_sliders()
+        self.reset_direct_sliders()
+        self.disconnect_direct_sliders()
+        self.disconnect_motor_sliders()
+
+        if use_control_mode == ControlMode.VELOCITY:
+            # Show the direct velocity widget
+            self.control_widget.setCurrentWidget(self.direct_velocity_widget)
+
+            # Enable direct sliders and disable motor sliders
+            common_widgets.enable_slider(
+                self.x_velocity_slider, self.x_velocity_label, self.value_change
+            )
+            common_widgets.enable_slider(
+                self.y_velocity_slider, self.y_velocity_label, self.value_change
+            )
+            common_widgets.enable_slider(
+                self.angular_velocity_slider,
+                self.angular_velocity_label,
+                self.value_change,
+            )
+
+            common_widgets.disable_slider(self.front_left_motor_slider)
+            common_widgets.disable_slider(self.front_right_motor_slider)
+            common_widgets.disable_slider(self.back_left_motor_slider)
+            common_widgets.disable_slider(self.back_right_motor_slider)
+
+            common_widgets.change_button_state(self.stop_and_reset_direct, True)
+            common_widgets.change_button_state(self.stop_and_reset_per_motor, False)
 
-        :param enable: boolean parameter, True is enable and False is disable
+        else:
+            # Show the per motor widget
+            self.control_widget.setCurrentWidget(self.per_motor_widget)
+
+            # Enable motor sliders and disable direct sliders
+            common_widgets.enable_slider(
+                self.front_left_motor_slider,
+                self.front_left_motor_label,
+                self.value_change,
+            )
+            common_widgets.enable_slider(
+                self.front_right_motor_slider,
+                self.front_right_motor_label,
+                self.value_change,
+            )
+            common_widgets.enable_slider(
+                self.back_left_motor_slider,
+                self.back_left_motor_label,
+                self.value_change,
+            )
+            common_widgets.enable_slider(
+                self.back_right_motor_slider,
+                self.back_right_motor_label,
+                self.value_change,
+            )
+
+            common_widgets.disable_slider(self.x_velocity_slider)
+            common_widgets.disable_slider(self.y_velocity_slider)
+            common_widgets.disable_slider(self.angular_velocity_slider)
+
+            common_widgets.change_button_state(self.stop_and_reset_per_motor, True)
+            common_widgets.change_button_state(self.stop_and_reset_direct, False)
+
+    def toggle_dribbler_sliders(self, enable: bool) -> None:
+        """Enables or disables dribbler sliders.
+
+        :param enable: True to enable, False to disable
         """
         if enable:
-            if not self.enabled:
-                # disconnect all sliders
-                self.disconnect_sliders()
-
-                # enable all sliders by adding listener to update label with slider value
-                common_widgets.enable_slider(
-                    self.x_velocity_slider, self.x_velocity_label, self.value_change
-                )
-                common_widgets.enable_slider(
-                    self.y_velocity_slider, self.y_velocity_label, self.value_change
-                )
-                common_widgets.enable_slider(
-                    self.angular_velocity_slider,
-                    self.angular_velocity_label,
-                    self.value_change,
-                )
-                common_widgets.enable_slider(
-                    self.dribbler_speed_rpm_slider,
-                    self.dribbler_speed_rpm_label,
-                    self.value_change,
-                )
-
-                # enable buttons
-                common_widgets.change_button_state(self.stop_and_reset_dribbler, True)
-                common_widgets.change_button_state(self.stop_and_reset_direct, True)
-
-                # change enabled field
-                self.enabled = True
+            common_widgets.enable_slider(
+                self.dribbler_speed_rpm_slider,
+                self.dribbler_speed_rpm_label,
+                self.value_change,
+            )
+            common_widgets.change_button_state(self.stop_and_reset_dribbler, True)
         else:
-            if self.enabled:
-                # reset slider values and disconnect
-                self.reset_all_sliders()
-                self.disconnect_sliders()
-
-                # disable all sliders by adding listener to keep slider value the same
-                common_widgets.disable_slider(self.x_velocity_slider)
-                common_widgets.disable_slider(self.y_velocity_slider)
-                common_widgets.disable_slider(self.angular_velocity_slider)
-                common_widgets.disable_slider(self.dribbler_speed_rpm_slider)
-
-                # disable buttons
-                common_widgets.change_button_state(self.stop_and_reset_dribbler, False)
-                common_widgets.change_button_state(self.stop_and_reset_direct, False)
-
-                # change enabled field
-                self.enabled = False
-
-    def disconnect_sliders(self) -> None:
-        """Disconnect listener for changing values for all sliders"""
+            common_widgets.disable_slider(self.dribbler_speed_rpm_slider)
+            common_widgets.change_button_state(self.stop_and_reset_dribbler, False)
+
+    def disconnect_direct_sliders(self) -> None:
+        """Disconnect listener for changing values for motor sliders"""
         self.x_velocity_slider.valueChanged.disconnect()
         self.y_velocity_slider.valueChanged.disconnect()
         self.angular_velocity_slider.valueChanged.disconnect()
+
+    def disconnect_dribbler_sliders(self) -> None:
         self.dribbler_speed_rpm_slider.valueChanged.disconnect()
 
+    def disconnect_motor_sliders(self) -> None:
+        self.front_left_motor_slider.valueChanged.disconnect()
+        self.front_right_motor_slider.valueChanged.disconnect()
+        self.back_left_motor_slider.valueChanged.disconnect()
+        self.back_right_motor_slider.valueChanged.disconnect()
+
     def reset_direct_sliders(self) -> None:
         """Reset direct sliders back to 0"""
         self.x_velocity_slider.setValue(0)
         self.y_velocity_slider.setValue(0)
         self.angular_velocity_slider.setValue(0)
 
+    def reset_motor_sliders(self) -> None:
+        """Reset direct sliders back to 0"""
+        self.front_left_motor_slider.setValue(0)
+        self.front_right_motor_slider.setValue(0)
+        self.back_left_motor_slider.setValue(0)
+        self.back_right_motor_slider.setValue(0)
+
     def reset_dribbler_slider(self) -> None:
         """Reset the dribbler slider back to 0"""
         self.dribbler_speed_rpm_slider.setValue(0)
 
     def reset_all_sliders(self) -> None:
         """Reset all sliders back to 0"""
         self.reset_direct_sliders()
+        self.reset_motor_sliders()
         self.reset_dribbler_slider()