From 08013c07e9547bda24d90c164eb5fe37f7ff97f2 Mon Sep 17 00:00:00 2001 From: muk Date: Tue, 14 Jan 2025 19:28:37 -0800 Subject: [PATCH 1/4] Merge complete, visually functioning, physical test needed. --- .../drive_and_dribbler_widget.py | 207 ++++++++++++++++-- 1 file changed, 193 insertions(+), 14 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index 174328fc03..e3e04708df 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -1,13 +1,20 @@ from pyqtgraph.Qt.QtCore import Qt from pyqtgraph.Qt.QtWidgets import * from proto.import_all_protos import * - +from enum import IntEnum import software.python_bindings as tbots_cpp from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.common import common_widgets +class ControlMode(IntEnum): + """Enum for the 2 drive modes (direct velocity and per-motor)""" + + VELOCITY = 0 + MOTOR = 1 + + class DriveAndDribblerWidget(QWidget): """This widget provides an interface for controlling our robots' drive and dribbler functionalities. It has sliders for manipulating @@ -28,11 +35,15 @@ def __init__(self, proto_unix_io: ProtoUnixIO) -> None: self.constants = tbots_cpp.create2021RobotConstants() self.enabled = True + self.control_mode = ControlMode.VELOCITY layout = QVBoxLayout() + layout.addWidget(self.__setup_drive_switch_radio()) layout.addWidget(self.__setup_direct_velocity_widgets()) + layout.addWidget(self.__setup_per_motor_widgets()) layout.addWidget(self.__setup_dribbler_widgets()) self.setLayout(layout) + self.use_direct_velocity.click() def enable(self) -> None: """Enable all sliders and buttons in the DriveAndDribblerWidget""" @@ -44,6 +55,10 @@ def enable(self) -> None: common_widgets.enable_slider(self.x_velocity_slider) common_widgets.enable_slider(self.y_velocity_slider) common_widgets.enable_slider(self.angular_velocity_slider) + common_widgets.enable_slider(self.front_left_motor_slider) + common_widgets.enable_slider(self.front_right_motor_slider) + common_widgets.enable_slider(self.back_left_motor_slider) + common_widgets.enable_slider(self.back_left_motor_slider) common_widgets.enable_slider(self.dribbler_speed_rpm_slider) common_widgets.enable_button(self.stop_and_reset_dribbler) common_widgets.enable_button(self.stop_and_reset_direct) @@ -59,6 +74,10 @@ def disable(self) -> None: 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.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_left_motor_slider) common_widgets.disable_slider(self.dribbler_speed_rpm_slider) common_widgets.disable_button(self.stop_and_reset_dribbler) common_widgets.disable_button(self.stop_and_reset_direct) @@ -77,23 +96,49 @@ def override_slider_values(self, motor_control: MotorControl) -> None: self.angular_velocity_slider.setValue( motor_control.direct_velocity_control.angular_velocity.radians_per_second ) + self.front_left_motor_slider.setValue( + motor_control.direct_velocity_control.front_left_wheel_velocity + ) + self.front_right_motor_slider.setValue( + motor_control.direct_velocity_control.front_right_wheel_velocity + ) + self.back_left_motor_slider.setValue( + motor_control.direct_velocity_control.back_left_wheel_velocity + ) + self.back_right_motor_slider.setValue( + motor_control.direct_velocity_control.back_right_wheel_velocity + ) + self.dribbler_speed_rpm_slider.setValue(motor_control.dribbler_speed_rpm) def refresh(self) -> None: - """Send out a MotorControl proto with the currently set direct velocity and - dribbler speed values - """ + """Refresh the widget and send the MotorControl message with the current values depending on the ControlMode""" motor_control = MotorControl() - 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() - ) motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) + if self.control_mode == ControlMode.VELOCITY: + 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) @@ -158,7 +203,114 @@ def __setup_direct_velocity_widgets(self) -> QGroupBox: self.stop_and_reset_direct, alignment=Qt.AlignmentFlag.AlignCenter ) - group_box = QGroupBox("Drive") + group_box = QGroupBox("Velocity Control") + group_box.setLayout(vbox) + self.direct_velocity_widget = group_box + + return group_box + + def __setup_per_motor_widgets(self) -> QGroupBox: + """ + :returns: a QGroupBox containing sliders and controls for controlling individual + speed of the robot's motors. + """ + ( + 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, + ) + + self.front_left_motor_slider.floatValueChanged.connect( + lambda new_value: self.front_left_motor_label.setText("%.2f" % new_value) + ) + self.front_right_motor_slider.floatValueChanged.connect( + lambda new_value: self.front_right_motor_label.setText("%.2f" % new_value) + ) + self.back_left_motor_slider.floatValueChanged.connect( + lambda new_value: self.back_left_motor_label.setText("%.2f" % new_value) + ) + self.back_right_motor_slider.floatValueChanged.connect( + lambda new_value: self.back_right_motor_label.setText("%.2f" % new_value) + ) + + self.stop_and_reset_per_motor = QPushButton("Stop and Reset") + self.stop_and_reset_per_motor.clicked.connect(self.__reset_motor_sliders) + + vbox = QVBoxLayout() + vbox.addLayout(fl_layout) + vbox.addLayout(fr_layout) + vbox.addLayout(bl_layout) + vbox.addLayout(br_layout) + vbox.addWidget( + self.stop_and_reset_per_motor, alignment=Qt.AlignmentFlag.AlignCenter + ) + + group_box = QGroupBox("Per Motor Control") + group_box.setLayout(vbox) + self.per_motor_widget = group_box + + return group_box + + def __setup_drive_switch_radio(self) -> QGroupBox: + """Create a radio button widget to switch between per-motor and velocity drive modes + + :param title: vbox name + """ + group_box = QGroupBox() + 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 @@ -199,12 +351,38 @@ def __setup_dribbler_widgets(self) -> QGroupBox: return group_box + def toggle_control_mode(self, use_control_mode: IntEnum) -> None: + """Switches between 'Direct Velocity' and 'Per Motor' drive modes. + + :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() + + if use_control_mode == ControlMode.VELOCITY: + # Show the direct velocity widget + self.direct_velocity_widget.setVisible(True) + self.per_motor_widget.setVisible(False) + else: + # Show the per motor widget + self.direct_velocity_widget.setVisible(False) + self.per_motor_widget.setVisible(True) + def __reset_direct_sliders(self) -> None: """Reset the direct velocity 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 speed slider back to 0""" self.dribbler_speed_rpm_slider.setValue(0) @@ -213,3 +391,4 @@ def __reset_all_sliders(self) -> None: """Reset all sliders back to 0""" self.__reset_direct_sliders() self.__reset_dribbler_slider() + self.__reset_direct_sliders() From 9c2c4d64e4588c9b109e3d67a0bce77a8fcdaa97 Mon Sep 17 00:00:00 2001 From: muk Date: Sat, 25 Jan 2025 12:39:29 -0800 Subject: [PATCH 2/4] tested good to go --- .../thunderscope/robot_diagnostics/drive_and_dribbler_widget.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index e3e04708df..cac1e0298f 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -293,7 +293,7 @@ def __setup_per_motor_widgets(self) -> QGroupBox: def __setup_drive_switch_radio(self) -> QGroupBox: """Create a radio button widget to switch between per-motor and velocity drive modes - :param title: vbox name + :returns: The group box of the radio button switch. """ group_box = QGroupBox() vbox = QVBoxLayout() From 7683fa3e807819cd70b9e1381c1b42f08540822e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 16 May 2025 00:25:48 +0000 Subject: [PATCH 3/4] [pre-commit.ci lite] apply automatic fixes --- .../robot_diagnostics/drive_and_dribbler_widget.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index cac1e0298f..37beab052c 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -123,8 +123,7 @@ def refresh(self) -> None: 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() + 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 = ( @@ -210,8 +209,7 @@ def __setup_direct_velocity_widgets(self) -> QGroupBox: return group_box def __setup_per_motor_widgets(self) -> QGroupBox: - """ - :returns: a QGroupBox containing sliders and controls for controlling individual + """:returns: a QGroupBox containing sliders and controls for controlling individual speed of the robot's motors. """ ( From f1a02b0b1c5c7ed2b96f7b90100fa6bd4017262a Mon Sep 17 00:00:00 2001 From: muk Date: Thu, 15 May 2025 17:48:56 -0700 Subject: [PATCH 4/4] moved clearfield to run only when switching mode --- .../robot_diagnostics/drive_and_dribbler_widget.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py index cac1e0298f..03e3e56702 100644 --- a/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py +++ b/src/software/thunderscope/robot_diagnostics/drive_and_dribbler_widget.py @@ -116,7 +116,6 @@ def refresh(self) -> None: motor_control = MotorControl() motor_control.dribbler_speed_rpm = int(self.dribbler_speed_rpm_slider.value()) if self.control_mode == ControlMode.VELOCITY: - motor_control.ClearField("direct_per_wheel_control") motor_control.direct_velocity_control.velocity.x_component_meters = ( self.x_velocity_slider.value() ) @@ -126,7 +125,6 @@ def refresh(self) -> None: 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() ) @@ -361,14 +359,18 @@ def toggle_control_mode(self, use_control_mode: IntEnum) -> None: self.__reset_motor_sliders() self.__reset_direct_sliders() + motor_control = MotorControl() if use_control_mode == ControlMode.VELOCITY: # Show the direct velocity widget + motor_control.ClearField("direct_per_wheel_control") self.direct_velocity_widget.setVisible(True) self.per_motor_widget.setVisible(False) else: # Show the per motor widget + motor_control.ClearField("direct_velocity_control") self.direct_velocity_widget.setVisible(False) self.per_motor_widget.setVisible(True) + self.proto_unix_io.send_proto(MotorControl, motor_control) def __reset_direct_sliders(self) -> None: """Reset the direct velocity sliders back to 0"""