mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
Rover arm pretty much working! Still some minor changes needed probably, but very happy with it. Lots of misc changes.
This commit is contained in:
@@ -5,21 +5,26 @@
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets, QtGui
|
||||
import logging
|
||||
from time import time
|
||||
import rospy
|
||||
|
||||
from rover_arm.msg import ArmStatusMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
THREAD_HERTZ = 2
|
||||
|
||||
ROTATION_SPEED_MODIFIER = 0.15
|
||||
ARM_STATUS_TOPIC = "/rover_arm/status"
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
#####################################
|
||||
class ArmIndication(QtCore.QThread):
|
||||
arm_joint_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
class ArmIndication(QtCore.QObject):
|
||||
base_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
shoulder_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
elbow_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
roll_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
wrist_pitch_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
wrist_roll_position_updated__signal = QtCore.pyqtSignal(int)
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(ArmIndication, self).__init__()
|
||||
@@ -41,56 +46,26 @@ class ArmIndication(QtCore.QThread):
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.wait_time = 1.0 / THREAD_HERTZ
|
||||
self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage, self.on_arm_status_update_received__callback)
|
||||
|
||||
self.current_position_delta = 0
|
||||
self.shown_position = 0
|
||||
|
||||
def run(self):
|
||||
while self.run_thread_flag:
|
||||
start_time = time()
|
||||
|
||||
self.change_position_if_needed()
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
self.msleep(max(int(self.wait_time - time_diff), 0))
|
||||
|
||||
def change_position_if_needed(self):
|
||||
self.shown_position += self.current_position_delta * ROTATION_SPEED_MODIFIER
|
||||
self.shown_position %= 360
|
||||
|
||||
self.arm_joint_position_updated__signal.emit(self.shown_position)
|
||||
|
||||
def __on_position_change_requested__slot(self, event):
|
||||
if event.button() == QtCore.Qt.LeftButton:
|
||||
self.current_position_delta = 1
|
||||
elif event.button() == QtCore.Qt.RightButton:
|
||||
self.current_position_delta = 0
|
||||
# ########## Connect Signals and Slots ##########
|
||||
self.connect_signals_and_slots()
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.arm_joint_position_updated__signal.connect(self.base_rotation_dial.setValue)
|
||||
self.arm_joint_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
|
||||
self.arm_joint_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
|
||||
self.arm_joint_position_updated__signal.connect(self.elbow_roll_dial.setValue)
|
||||
self.arm_joint_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
|
||||
self.arm_joint_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
|
||||
self.base_position_updated__signal.connect(self.base_rotation_dial.setValue)
|
||||
self.shoulder_position_updated__signal.connect(self.shoulder_pitch_dial.setValue)
|
||||
self.elbow_position_updated__signal.connect(self.elbow_pitch_dial.setValue)
|
||||
self.roll_position_updated__signal.connect(self.elbow_roll_dial.setValue)
|
||||
self.wrist_pitch_position_updated__signal.connect(self.end_effector_pitch_dial.setValue)
|
||||
self.wrist_roll_position_updated__signal.connect(self.end_effector_rotation_dial.setValue)
|
||||
|
||||
self.base_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
self.shoulder_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
self.elbow_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
self.elbow_roll_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
self.end_effector_pitch_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
self.end_effector_rotation_dial.mousePressEvent = self.__on_position_change_requested__slot
|
||||
def on_arm_status_update_received__callback(self, data):
|
||||
self.base_position_updated__signal.emit(int(data.base * 1000))
|
||||
self.shoulder_position_updated__signal.emit(int(data.shoulder * 1000))
|
||||
self.elbow_position_updated__signal.emit(int(data.elbow * 1000))
|
||||
self.roll_position_updated__signal.emit(int(data.roll * 1000))
|
||||
self.wrist_pitch_position_updated__signal.emit(int(data.wrist_pitch * 1000))
|
||||
self.wrist_roll_position_updated__signal.emit(int(data.wrist_roll * 1000))
|
||||
|
||||
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
kill_signal.connect(self.on_kill_threads_requested__slot)
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
|
||||
@@ -8,7 +8,7 @@ from inputs import devices, GamePad
|
||||
from time import time
|
||||
|
||||
import rospy
|
||||
from rover_control.msg import DriveCommandMessage, TowerPanTiltControlMessage
|
||||
from rover_arm.msg import ArmControlMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
@@ -17,6 +17,21 @@ GAME_CONTROLLER_NAME = "Afterglow Gamepad for Xbox 360"
|
||||
|
||||
DRIVE_COMMAND_HERTZ = 20
|
||||
|
||||
RELATIVE_ARM_CONTROL_TOPIC = "/rover_arm/control/relative"
|
||||
|
||||
BASE_SCALAR = 0.003
|
||||
SHOULDER_SCALAR = 0.002
|
||||
ELBOW_SCALAR = 0.002
|
||||
ROLL_SCALAR = 0.003
|
||||
WRIST_PITCH_SCALAR = 0.003
|
||||
WRIST_ROLL_SCALAR = 0.006
|
||||
|
||||
LEFT_X_AXIS_DEADZONE = 1500
|
||||
LEFT_Y_AXIS_DEADZONE = 1500
|
||||
|
||||
RIGHT_X_AXIS_DEADZONE = 1500
|
||||
RIGHT_Y_AXIS_DEADZONE = 1500
|
||||
|
||||
|
||||
#####################################
|
||||
# Controller Class Definition
|
||||
@@ -156,13 +171,16 @@ class ControllerControlSender(QtCore.QThread):
|
||||
|
||||
self.wait_time = 1.0 / DRIVE_COMMAND_HERTZ
|
||||
|
||||
self.relative_arm_control_publisher = rospy.Publisher(RELATIVE_ARM_CONTROL_TOPIC, ArmControlMessage,
|
||||
queue_size=1)
|
||||
|
||||
def run(self):
|
||||
self.logger.debug("Starting Joystick Thread")
|
||||
|
||||
while self.run_thread_flag:
|
||||
start_time = time()
|
||||
|
||||
# print self.controller.controller_states
|
||||
self.process_and_send_arm_control()
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
@@ -173,6 +191,43 @@ class ControllerControlSender(QtCore.QThread):
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
def process_and_send_arm_control(self):
|
||||
message = ArmControlMessage()
|
||||
|
||||
should_publish = False
|
||||
|
||||
left_trigger = self.controller.controller_states["left_trigger"]
|
||||
right_trigger = self.controller.controller_states["right_trigger"]
|
||||
|
||||
left_x_axis = self.controller.controller_states["left_x_axis"] if abs(self.controller.controller_states[
|
||||
"left_x_axis"]) > LEFT_X_AXIS_DEADZONE else 0
|
||||
left_y_axis = self.controller.controller_states["left_y_axis"] if abs(self.controller.controller_states[
|
||||
"left_y_axis"]) > LEFT_Y_AXIS_DEADZONE else 0
|
||||
right_y_axis = self.controller.controller_states["right_y_axis"] if abs(self.controller.controller_states[
|
||||
"right_y_axis"]) > RIGHT_Y_AXIS_DEADZONE else 0
|
||||
right_x_axis = self.controller.controller_states["right_x_axis"] if abs(self.controller.controller_states[
|
||||
"right_x_axis"]) > RIGHT_X_AXIS_DEADZONE else 0
|
||||
|
||||
# print left_x_axis, ":", left_y_axis, ":", right_x_axis, ":", right_y_axis
|
||||
|
||||
left_trigger_ratio = left_trigger / 255.0
|
||||
right_trigger_ratio = right_trigger / 255.0
|
||||
|
||||
if left_trigger > 25:
|
||||
should_publish = True
|
||||
message.base = ((left_x_axis / 32768.0) * BASE_SCALAR) * left_trigger_ratio
|
||||
message.shoulder = (-(left_y_axis / 32768.0) * SHOULDER_SCALAR) * left_trigger_ratio
|
||||
message.elbow = ((right_y_axis / 32768.0) * ELBOW_SCALAR) * left_trigger_ratio
|
||||
message.roll = (-(right_x_axis / 32768.0) * ROLL_SCALAR) * left_trigger_ratio
|
||||
|
||||
elif right_trigger > 25:
|
||||
should_publish = True
|
||||
message.wrist_roll = ((right_x_axis / 32768.0) * WRIST_ROLL_SCALAR) * right_trigger_ratio
|
||||
message.wrist_pitch = ((left_y_axis / 32768.0) * WRIST_PITCH_SCALAR) * right_trigger_ratio
|
||||
|
||||
if should_publish:
|
||||
self.relative_arm_control_publisher.publish(message)
|
||||
|
||||
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
|
||||
@@ -0,0 +1,367 @@
|
||||
# coding=utf-8
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
from PyQt5 import QtCore, QtWidgets
|
||||
import logging
|
||||
import rospy
|
||||
from time import time
|
||||
|
||||
from rover_arm.msg import ArmControlMessage, ArmStatusMessage
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
#####################################
|
||||
ARM_RELATIVE_CONTROL_TOPIC = "/rover_arm/control/relative"
|
||||
ARM_ABSOLUTE_CONTROL_TOPIC = "/rover_arm/control/absolute"
|
||||
ARM_STATUS_TOPIC = "/rover_arm/status"
|
||||
|
||||
THREAD_HERTZ = 5
|
||||
|
||||
COMMS_TO_STRING = {
|
||||
0: "NO STATUS",
|
||||
1: "COMMS OK",
|
||||
2: "NO DEVICE",
|
||||
4: "BUS ERROR",
|
||||
8: "GEN COMM ERROR",
|
||||
16: "PARAMETER ERROR",
|
||||
32: "LENGTH ERROR"
|
||||
}
|
||||
|
||||
TARGET_REACHED_BIT_POSITION = 1
|
||||
|
||||
STATUS_TO_STRING = {
|
||||
1: "TARGET REACHED",
|
||||
2: "ERROR RECOVERY",
|
||||
3: "RUN",
|
||||
4: "ENABLED",
|
||||
5: "FAULT STOP",
|
||||
6: "WARNING",
|
||||
7: "STO ACTIVE",
|
||||
8: "SERVO READY",
|
||||
10: "BRAKING",
|
||||
11: "HOMING",
|
||||
12: "INITIALIZED",
|
||||
13: "VOLT OK",
|
||||
15: "PERMANENT STOP"
|
||||
}
|
||||
|
||||
FAULT_TO_STRING = {
|
||||
1: "TRACKING ERROR",
|
||||
2: "OVER CURRENT",
|
||||
# 3: "COMMUNICATION ERROR", # Was showing even though things were working???
|
||||
4: "ENCODER FAILURE",
|
||||
5: "OVER TEMP",
|
||||
6: "UNDER VOLTAGE",
|
||||
7: "OVER VOLTAGE",
|
||||
8: "PROG OR MEM ERROR",
|
||||
9: "HARDWARE ERROR",
|
||||
10: "OVER VELOCITY",
|
||||
11: "INIT ERROR",
|
||||
12: "MOTION ERROR",
|
||||
13: "RANGE ERROR",
|
||||
14: "POWER STAGE FORCED OFF",
|
||||
15: "HOST COMM ERROR"
|
||||
}
|
||||
|
||||
POSITIONAL_TOLERANCE = 0.02
|
||||
|
||||
# Order is [base, shoulder, elbow, roll, wrist_pitch, wrist_roll]
|
||||
ARM_STOW_PROCEDURE = [
|
||||
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0],
|
||||
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
|
||||
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
|
||||
[0.0, -0.25, -0.5, -0.25, 0.25, 0.0]
|
||||
]
|
||||
|
||||
ARM_UNSTOW_PROCEDURE = [
|
||||
[0.0, -0.25, -0.5, -0.25, 0.25, 0.0],
|
||||
[0.0, -0.035, -0.5, -0.25, 0.25, 0.0],
|
||||
[0.0, -0.035, -0.28, -0.25, 0.25, 0.0],
|
||||
[0.0, -0.035, -0.28, 0.0, 0.0, 0.0]
|
||||
]
|
||||
|
||||
|
||||
#####################################
|
||||
# UbiquitiRadioSettings Class Definition
|
||||
#####################################
|
||||
class MiscArm(QtCore.QThread):
|
||||
base_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
shoulder_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
elbow_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_pitch_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_roll_comms_state_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
|
||||
base_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
shoulder_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
elbow_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
roll_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_pitch_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_roll_status_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
|
||||
base_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
shoulder_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
elbow_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_pitch_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
wrist_roll_faults_update_ready__signal = QtCore.pyqtSignal(str)
|
||||
|
||||
def __init__(self, shared_objects):
|
||||
super(MiscArm, self).__init__()
|
||||
|
||||
# ########## Reference to class init variables ##########
|
||||
self.shared_objects = shared_objects
|
||||
self.left_screen = self.shared_objects["screens"]["left_screen"]
|
||||
|
||||
self.arm_control_upright_zeroed_button = self.left_screen.arm_control_upright_zeroed_button # type:QtWidgets.QPushButton
|
||||
self.arm_controls_stow_arm_button = self.left_screen.arm_controls_stow_arm_button # type:QtWidgets.QPushButton
|
||||
self.arm_controls_unstow_arm_button = self.left_screen.arm_controls_unstow_arm_button # type:QtWidgets.QPushButton
|
||||
|
||||
self.arm_controls_calibration_button = self.left_screen.arm_controls_calibration_button # type:QtWidgets.QPushButton
|
||||
self.arm_controls_clear_faults_button = self.left_screen.arm_controls_clear_faults_button # type:QtWidgets.QPushButton
|
||||
self.arm_controls_reset_motor_drivers_button = self.left_screen.arm_controls_reset_motor_drivers_button # type:QtWidgets.QPushButton
|
||||
|
||||
self.arm_controls_base_comms_label = self.left_screen.arm_controls_base_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_base_status_label = self.left_screen.arm_controls_base_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_base_faults_label = self.left_screen.arm_controls_base_faults_label # type:QtWidgets.QLabel
|
||||
|
||||
self.arm_controls_shoulder_comms_label = self.left_screen.arm_controls_shoulder_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_shoulder_status_label = self.left_screen.arm_controls_shoulder_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_shoulder_faults_label = self.left_screen.arm_controls_shoulder_faults_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_elbow_comms_label = self.left_screen.arm_controls_elbow_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_elbow_status_label = self.left_screen.arm_controls_elbow_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_elbow_faults_label = self.left_screen.arm_controls_elbow_faults_label # type:QtWidgets.QLabel
|
||||
|
||||
self.arm_controls_roll_comms_label = self.left_screen.arm_controls_roll_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_roll_status_label = self.left_screen.arm_controls_roll_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_roll_faults_label = self.left_screen.arm_controls_roll_faults_label # type:QtWidgets.QLabel
|
||||
|
||||
self.arm_controls_wrist_pitch_comms_label = self.left_screen.arm_controls_wrist_pitch_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_wrist_pitch_status_label = self.left_screen.arm_controls_wrist_pitch_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_wrist_pitch_faults_label = self.left_screen.arm_controls_wrist_pitch_faults_label # type:QtWidgets.QLabel
|
||||
|
||||
self.arm_controls_wrist_roll_comms_label = self.left_screen.arm_controls_wrist_roll_comms_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_wrist_roll_status_label = self.left_screen.arm_controls_wrist_roll_status_label # type:QtWidgets.QLabel
|
||||
self.arm_controls_wrist_roll_faults_label = self.left_screen.arm_controls_wrist_roll_faults_label # type:QtWidgets.QLabel
|
||||
|
||||
# ########## Get the settings instance ##########
|
||||
self.settings = QtCore.QSettings()
|
||||
|
||||
# ########## Get the Pick And Plate instance of the logger ##########
|
||||
self.logger = logging.getLogger("groundstation")
|
||||
|
||||
# ########## Thread Flags ##########
|
||||
self.run_thread_flag = True
|
||||
|
||||
# ########## Class Variables ##########
|
||||
self.wait_time = 1.0 / THREAD_HERTZ
|
||||
|
||||
self.arm_status_subscriber = rospy.Subscriber(ARM_STATUS_TOPIC, ArmStatusMessage,
|
||||
self.new_arm_status_message_received__callback)
|
||||
|
||||
self.arm_relative_control_publisher = rospy.Publisher(ARM_RELATIVE_CONTROL_TOPIC, ArmControlMessage,
|
||||
queue_size=1)
|
||||
self.arm_absolute_control_publisher = rospy.Publisher(ARM_ABSOLUTE_CONTROL_TOPIC, ArmControlMessage,
|
||||
queue_size=1)
|
||||
|
||||
self.base_position = 0
|
||||
self.shoulder_position = 0
|
||||
self.elbow_position = 0
|
||||
self.roll_position = 0
|
||||
self.wrist_pitch_position = 0
|
||||
self.wrist_roll_position = 0
|
||||
|
||||
self.should_stow_arm = False
|
||||
self.should_unstow_arm = False
|
||||
|
||||
def run(self):
|
||||
self.logger.debug("Starting MiscArm Thread")
|
||||
|
||||
while self.run_thread_flag:
|
||||
start_time = time()
|
||||
|
||||
if self.should_stow_arm:
|
||||
self.stow_rover_arm()
|
||||
self.should_stow_arm = False
|
||||
elif self.should_unstow_arm:
|
||||
self.unstow_rover_arm()
|
||||
self.should_unstow_arm = False
|
||||
|
||||
time_diff = time() - start_time
|
||||
|
||||
self.msleep(max(int(self.wait_time - time_diff), 0))
|
||||
|
||||
self.logger.debug("Stopping MiscArm Thread")
|
||||
|
||||
def stow_rover_arm(self):
|
||||
for movement in ARM_STOW_PROCEDURE:
|
||||
self.process_absolute_move_command(movement)
|
||||
|
||||
def unstow_rover_arm(self):
|
||||
for movement in ARM_UNSTOW_PROCEDURE:
|
||||
self.process_absolute_move_command(movement)
|
||||
|
||||
def process_absolute_move_command(self, movement):
|
||||
message = ArmControlMessage()
|
||||
|
||||
message.base = movement[0]
|
||||
message.shoulder = movement[1]
|
||||
message.elbow = movement[2]
|
||||
message.roll = movement[3]
|
||||
message.wrist_pitch = movement[4]
|
||||
message.wrist_roll = movement[5]
|
||||
|
||||
print message
|
||||
self.arm_absolute_control_publisher.publish(message)
|
||||
|
||||
self.wait_for_targets_reached(movement)
|
||||
|
||||
def wait_for_targets_reached(self, movement):
|
||||
base_set = movement[0]
|
||||
shoulder_set = movement[1]
|
||||
elbow_set = movement[2]
|
||||
roll_set = movement[3]
|
||||
wrist_pitch_set = movement[4]
|
||||
wrist_roll_set = movement[5] - (wrist_pitch_set / 2.0)
|
||||
|
||||
while abs(self.base_position - base_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for base| %f\t%f" % (self.base_position, base_set))
|
||||
self.msleep(10)
|
||||
|
||||
while abs(self.shoulder_position - shoulder_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for shoulder| %f\t%f" % (self.shoulder_position, shoulder_set))
|
||||
self.msleep(10)
|
||||
|
||||
while abs(self.elbow_position - elbow_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for elbow| %f\t%f" % (self.elbow_position, elbow_set))
|
||||
self.msleep(10)
|
||||
|
||||
while abs(self.roll_position - roll_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for roll| %f\t%f" % (self.roll_position, roll_set))
|
||||
self.msleep(10)
|
||||
|
||||
while abs(self.wrist_pitch_position - wrist_pitch_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for wrist_pitch| %f\t%f" % (self.wrist_pitch_position, wrist_pitch_set))
|
||||
self.msleep(10)
|
||||
|
||||
while abs(self.wrist_roll_position - wrist_roll_set) > POSITIONAL_TOLERANCE:
|
||||
self.logger.debug("Waiting for wrist_roll| %f\t%f" % (self.wrist_roll_position, wrist_roll_set))
|
||||
self.msleep(10)
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
self.arm_controls_stow_arm_button.clicked.connect(self.on_stow_arm_button_pressed__slot)
|
||||
self.arm_controls_unstow_arm_button.clicked.connect(self.on_unstow_arm_button_pressed__slot)
|
||||
self.arm_control_upright_zeroed_button.clicked.connect(self.on_upright_zeroed_button_pressed__slot)
|
||||
|
||||
self.arm_controls_calibration_button.clicked.connect(self.on_set_calibration_button_pressed__slot)
|
||||
self.arm_controls_clear_faults_button.clicked.connect(self.on_clear_faults_button_pressed__slot)
|
||||
self.arm_controls_reset_motor_drivers_button.clicked.connect(self.on_reset_drivers_button_pressed__slot)
|
||||
|
||||
self.base_comms_state_update_ready__signal.connect(self.arm_controls_base_comms_label.setText)
|
||||
self.shoulder_comms_state_update_ready__signal.connect(self.arm_controls_shoulder_comms_label.setText)
|
||||
self.elbow_comms_state_update_ready__signal.connect(self.arm_controls_elbow_comms_label.setText)
|
||||
self.roll_comms_state_update_ready__signal.connect(self.arm_controls_roll_comms_label.setText)
|
||||
self.wrist_pitch_comms_state_update_ready__signal.connect(self.arm_controls_wrist_pitch_comms_label.setText)
|
||||
self.wrist_roll_comms_state_update_ready__signal.connect(self.arm_controls_wrist_roll_comms_label.setText)
|
||||
|
||||
self.base_status_update_ready__signal.connect(self.arm_controls_base_status_label.setText)
|
||||
self.shoulder_status_update_ready__signal.connect(self.arm_controls_shoulder_status_label.setText)
|
||||
self.elbow_status_update_ready__signal.connect(self.arm_controls_elbow_status_label.setText)
|
||||
self.roll_status_update_ready__signal.connect(self.arm_controls_roll_status_label.setText)
|
||||
self.wrist_pitch_status_update_ready__signal.connect(self.arm_controls_wrist_pitch_status_label.setText)
|
||||
self.wrist_roll_status_update_ready__signal.connect(self.arm_controls_wrist_roll_status_label.setText)
|
||||
|
||||
self.base_faults_update_ready__signal.connect(self.arm_controls_base_faults_label.setText)
|
||||
self.shoulder_faults_update_ready__signal.connect(self.arm_controls_shoulder_faults_label.setText)
|
||||
self.elbow_faults_update_ready__signal.connect(self.arm_controls_elbow_faults_label.setText)
|
||||
self.roll_faults_update_ready__signal.connect(self.arm_controls_roll_faults_label.setText)
|
||||
self.wrist_pitch_faults_update_ready__signal.connect(self.arm_controls_wrist_pitch_faults_label.setText)
|
||||
self.wrist_roll_faults_update_ready__signal.connect(self.arm_controls_wrist_roll_faults_label.setText)
|
||||
|
||||
def on_upright_zeroed_button_pressed__slot(self):
|
||||
self.process_absolute_move_command([0 for _ in range(6)])
|
||||
|
||||
def on_set_calibration_button_pressed__slot(self):
|
||||
message = ArmControlMessage()
|
||||
message.calibrate = True
|
||||
self.arm_relative_control_publisher.publish(message)
|
||||
|
||||
def on_clear_faults_button_pressed__slot(self):
|
||||
message = ArmControlMessage()
|
||||
message.clear_faults = True
|
||||
self.arm_relative_control_publisher.publish(message)
|
||||
|
||||
def on_reset_drivers_button_pressed__slot(self):
|
||||
message = ArmControlMessage()
|
||||
message.reset_controllers = True
|
||||
self.arm_relative_control_publisher.publish(message)
|
||||
|
||||
def on_stow_arm_button_pressed__slot(self):
|
||||
self.should_stow_arm = True
|
||||
|
||||
def on_unstow_arm_button_pressed__slot(self):
|
||||
self.should_unstow_arm = True
|
||||
|
||||
def new_arm_status_message_received__callback(self, data):
|
||||
self.base_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.base_comm_status))
|
||||
self.shoulder_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.shoulder_comm_status))
|
||||
self.elbow_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.elbow_comm_status))
|
||||
self.roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.roll_comm_status))
|
||||
self.wrist_pitch_comms_state_update_ready__signal.emit(
|
||||
self.process_comms_to_string(data.wrist_pitch_comm_status))
|
||||
self.wrist_roll_comms_state_update_ready__signal.emit(self.process_comms_to_string(data.wrist_roll_comm_status))
|
||||
|
||||
self.base_status_update_ready__signal.emit(self.process_statuses_to_string(data.base_status))
|
||||
self.shoulder_status_update_ready__signal.emit(self.process_statuses_to_string(data.shoulder_status))
|
||||
self.elbow_status_update_ready__signal.emit(self.process_statuses_to_string(data.elbow_status))
|
||||
self.roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.roll_status))
|
||||
self.wrist_pitch_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_pitch_status))
|
||||
self.wrist_roll_status_update_ready__signal.emit(self.process_statuses_to_string(data.wrist_roll_status))
|
||||
|
||||
self.base_faults_update_ready__signal.emit(self.process_faults_to_string(data.base_faults))
|
||||
self.shoulder_faults_update_ready__signal.emit(self.process_faults_to_string(data.shoulder_faults))
|
||||
self.elbow_faults_update_ready__signal.emit(self.process_faults_to_string(data.elbow_faults))
|
||||
self.roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.roll_faults))
|
||||
self.wrist_pitch_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_pitch_faults))
|
||||
self.wrist_roll_faults_update_ready__signal.emit(self.process_faults_to_string(data.wrist_roll_faults))
|
||||
|
||||
self.base_position = data.base
|
||||
self.shoulder_position = data.shoulder
|
||||
self.elbow_position = data.elbow
|
||||
self.roll_position = data.roll
|
||||
self.wrist_pitch_position = data.wrist_pitch
|
||||
self.wrist_roll_position = data.wrist_roll
|
||||
|
||||
@staticmethod
|
||||
def process_faults_to_string(faults):
|
||||
fault_output = ""
|
||||
|
||||
for bit_position in FAULT_TO_STRING:
|
||||
if (1 << bit_position) & faults:
|
||||
fault_output += FAULT_TO_STRING[bit_position] + "\n"
|
||||
|
||||
return fault_output[:-1]
|
||||
|
||||
@staticmethod
|
||||
def process_statuses_to_string(statuses):
|
||||
status_output = ""
|
||||
|
||||
for bit_position in STATUS_TO_STRING:
|
||||
if (1 << bit_position) & statuses:
|
||||
status_output += STATUS_TO_STRING[bit_position] + "\n"
|
||||
|
||||
return status_output[:-1]
|
||||
|
||||
@staticmethod
|
||||
def process_comms_to_string(comms):
|
||||
return COMMS_TO_STRING[comms] if comms in COMMS_TO_STRING else "UNKNOWN"
|
||||
|
||||
def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
|
||||
start_signal.connect(self.start)
|
||||
signals_and_slots_signal.connect(self.connect_signals_and_slots)
|
||||
kill_signal.connect(self.on_kill_threads_requested__slot)
|
||||
|
||||
def on_kill_threads_requested__slot(self):
|
||||
self.run_thread_flag = False
|
||||
@@ -1392,6 +1392,708 @@ N/A</string>
|
||||
<property name="currentIndex">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="tab_4">
|
||||
<attribute name="title">
|
||||
<string>Arm</string>
|
||||
</attribute>
|
||||
<layout class="QGridLayout" name="gridLayout_15">
|
||||
<item row="0" column="0">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_14">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_67">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>18</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Arm Controls</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_15">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_15">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_68">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Preset Movements</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_13">
|
||||
<item row="3" column="0">
|
||||
<widget class="QPushButton" name="arm_control_upright_zeroed_button">
|
||||
<property name="text">
|
||||
<string>Upright Zeroed</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<spacer name="verticalSpacer_4">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="0" column="0">
|
||||
<widget class="QPushButton" name="arm_controls_stow_arm_button">
|
||||
<property name="text">
|
||||
<string>Stow Arm</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QPushButton" name="arm_controls_unstow_arm_button">
|
||||
<property name="text">
|
||||
<string>Unstow Arm</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="4" column="0">
|
||||
<widget class="QLabel" name="label_71">
|
||||
<property name="styleSheet">
|
||||
<string notr="true">color:red;</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>^ CAREFUL! ^</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_17">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_70">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Special Functions</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_14">
|
||||
<item row="0" column="0">
|
||||
<widget class="QPushButton" name="arm_controls_calibration_button">
|
||||
<property name="text">
|
||||
<string>Set Calibration</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QPushButton" name="arm_controls_clear_faults_button">
|
||||
<property name="text">
|
||||
<string>Clear Faults</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QPushButton" name="arm_controls_reset_motor_drivers_button">
|
||||
<property name="text">
|
||||
<string>Reset Motor Drivers</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="verticalSpacer_5">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<spacer name="verticalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>20</width>
|
||||
<height>40</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<layout class="QHBoxLayout" name="horizontalLayout_14">
|
||||
<item>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_7">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_19">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>18</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Motor Driver Statuses</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_9">
|
||||
<item row="0" column="0">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_6">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_20">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Base</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_21">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_base_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_23">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_27">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_base_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_base_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_9">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_32">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Shoulder</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_2">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_33">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_shoulder_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_35">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_36">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_shoulder_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_shoulder_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="0" column="2">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_10">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_39">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Elbow</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_3">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_40">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_elbow_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_42">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_43">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_elbow_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_elbow_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_11">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_46">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Roll</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_4">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_47">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_roll_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_49">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_50">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_roll_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_roll_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_12">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_53">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Wrist Pitch</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_5">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_54">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_pitch_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_56">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_57">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_pitch_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_pitch_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="1" column="2">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_13">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_60">
|
||||
<property name="font">
|
||||
<font>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Wrist Roll</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QFormLayout" name="formLayout_6">
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="label_61">
|
||||
<property name="text">
|
||||
<string>Comms:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_roll_comms_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QLabel" name="label_63">
|
||||
<property name="text">
|
||||
<string>Status:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="0">
|
||||
<widget class="QLabel" name="label_64">
|
||||
<property name="text">
|
||||
<string>Faults:</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_roll_status_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="arm_controls_wrist_roll_faults_label">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>6</pointsize>
|
||||
<weight>50</weight>
|
||||
<bold>false</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="Line" name="line_8">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<widget class="QWidget" name="tab_3">
|
||||
<attribute name="title">
|
||||
<string>Mining</string>
|
||||
|
||||
@@ -151,7 +151,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Base Rotation</string>
|
||||
<string>Base</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -167,10 +167,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-500</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>500</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -218,7 +218,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Shoulder Pitch</string>
|
||||
<string>Shoulder</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -234,10 +234,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-250</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>250</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -267,7 +267,7 @@
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -289,7 +289,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Elbow Pitch</string>
|
||||
<string>Elbow</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -305,10 +305,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-500</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>500</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -338,7 +338,7 @@
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -358,7 +358,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Elbow Roll</string>
|
||||
<string>Roll</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -374,10 +374,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-250</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>250</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -407,7 +407,7 @@
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -431,7 +431,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>End Effector Pitch</string>
|
||||
<string>Wrist Pitch</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -447,10 +447,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-250</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>250</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -480,7 +480,7 @@
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@@ -498,7 +498,7 @@
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>End Effector Rotation</string>
|
||||
<string>Wrist Roll</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignCenter</set>
|
||||
@@ -514,10 +514,10 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="minimum">
|
||||
<number>0</number>
|
||||
<number>-1000</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<number>360</number>
|
||||
<number>1000</number>
|
||||
</property>
|
||||
<property name="pageStep">
|
||||
<number>10</number>
|
||||
@@ -547,7 +547,7 @@
|
||||
<double>90.000000000000000</double>
|
||||
</property>
|
||||
<property name="notchesVisible">
|
||||
<bool>false</bool>
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
||||
@@ -26,6 +26,7 @@ import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
|
||||
import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
|
||||
import Framework.MiscSystems.MiningCore as MiningCore
|
||||
import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
|
||||
import Framework.MiscSystems.MiscArmCore as MiscArmCore
|
||||
|
||||
#####################################
|
||||
# Global Variables
|
||||
@@ -105,6 +106,7 @@ class GroundStation(QtCore.QObject):
|
||||
|
||||
# ##### Instantiate Regular Classes ######
|
||||
self.__add_non_thread("Mining System", MiningCore.Mining(self.shared_objects))
|
||||
self.__add_non_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
|
||||
|
||||
# ##### Instantiate Threaded Classes ######
|
||||
self.__add_thread("Video Coordinator", RoverVideoCoordinator.RoverVideoCoordinator(self.shared_objects))
|
||||
@@ -112,13 +114,13 @@ class GroundStation(QtCore.QObject):
|
||||
self.__add_thread("Joystick Sender", JoystickControlSender.JoystickControlSender(self.shared_objects))
|
||||
self.__add_thread("Controller Sender", ControllerControlSender.ControllerControlSender(self.shared_objects))
|
||||
self.__add_thread("Speed and Heading", SpeedAndHeading.SpeedAndHeadingIndication(self.shared_objects))
|
||||
self.__add_thread("Arm Indication", ArmIndication.ArmIndication(self.shared_objects))
|
||||
self.__add_thread("Rover Status", StatusCore.SensorCore(self.shared_objects))
|
||||
self.__add_thread("Ubiquiti Status", UbiquitiStatusCore.UbiquitiStatus(self.shared_objects))
|
||||
self.__add_thread("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects))
|
||||
self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(self.shared_objects))
|
||||
self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects))
|
||||
self.__add_thread("Bash Console", BashConsoleCore.BashConsole(self.shared_objects))
|
||||
self.__add_thread("Misc Arm", MiscArmCore.MiscArm(self.shared_objects))
|
||||
|
||||
self.connect_signals_and_slots_signal.emit()
|
||||
self.__connect_signals_to_slots()
|
||||
|
||||
Reference in New Issue
Block a user