def process_and_send_arm_control(self):
arm_control_message = ArmControlMessage()
gripper_control_message = GripperControlMessage()
should_publish_arm = False
should_publish_gripper = True if self.send_new_gripper_mode else False
if self.send_new_gripper_mode:
gripper_control_message.gripper_position_absolute = 0
else:
gripper_control_message.gripper_position_absolute = -1
gripper_control_message.gripper_mode = self.gripper_control_mode + 1
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
speed_limit = self.arm_speed_limit_slider.value() / 100.0
if left_trigger > 0:
should_publish_arm = True
arm_control_message.base = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * speed_limit
arm_control_message.shoulder = ((left_y_axis / THUMB_STICK_MAX) * SHOULDER_SCALAR) * speed_limit
arm_control_message.elbow = (-(right_y_axis / THUMB_STICK_MAX) * ELBOW_SCALAR) * speed_limit
arm_control_message.roll = (-(right_x_axis / THUMB_STICK_MAX) * ROLL_SCALAR) * speed_limit
elif right_trigger > 0:
should_publish_arm = True
should_publish_gripper = True
arm_control_message.wrist_roll = ((left_x_axis / THUMB_STICK_MAX) * BASE_SCALAR) * speed_limit
# ##### FIXME #####
# Remove this once the arm is fixed
# arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * speed_limit
# #################
gripper_control_message.gripper_position_relative = (-(right_y_axis / THUMB_STICK_MAX) * GRIPPER_MOVEMENT_SCALAR)
if should_publish_arm:
self.relative_arm_control_publisher.publish(arm_control_message)
if should_publish_gripper:
self.gripper_control_publisher.publish(gripper_control_message)
self.send_new_gripper_mode = False
def on_gripper_toggle_light_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 2
message.toggle_light = True
message.gripper_position_absolute = -1
self.gripper_control_publisher.publish(message)
def on_gripper_home_pressed(self):
message = GripperControlMessage()
message.gripper_mode = 2
message.gripper_position_absolute = -1
message.should_home = True
self.gripper_control_publisher.publish(message)
def change_control_state_if_needed(self):
xbox_state = self.controller.controller_states["xbox_button"]
left_bumper_state = self.controller.controller_states["left_bumper"]
right_bumper_state = self.controller.controller_states["right_bumper"]
if self.last_xbox_button_state == 0 and xbox_state == 1:
self.xbox_current_control_state += 1
self.xbox_current_control_state = self.xbox_current_control_state % len(self.XBOX_CONTROL_STATES)
self.xbox_control_state_just_changed = True
self.last_xbox_button_state = 1
elif self.last_xbox_button_state == 1 and xbox_state == 0:
self.last_xbox_button_state = 0
if self.xbox_control_state_just_changed:
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_NONE)
elif self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("MINING"):
self.xbox_control_arm_stylesheet_update_ready__signal.emit(COLOR_NONE)
self.xbox_control_mining_stylesheet_update_ready__signal.emit(COLOR_GREEN)
self.xbox_control_state_just_changed = False
if self.xbox_current_control_state == self.XBOX_CONTROL_STATES.index("ARM"):
if self.last_left_bumper_state == 0 and left_bumper_state == 1:
# self.gripper_control_mode = ((self.gripper_control_mode - 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_left_bumper_state = 1
elif self.last_left_bumper_state == 1 and left_bumper_state == 0:
self.last_left_bumper_state = 0
if self.last_right_bumper_state == 0 and right_bumper_state == 1:
# self.gripper_control_mode = ((self.gripper_control_mode + 1) % len(self.GRIPPER_CONTROL_MODES))
# self.gripper_control_mode_just_changed = True
self.last_right_bumper_state = 1
elif self.last_right_bumper_state == 1 and right_bumper_state == 0:
self.last_right_bumper_state = 0
if self.gripper_control_mode_just_changed:
signal_map = self.GRIPPER_DISPLAY_SIGNAL_MAPPING[self.gripper_control_mode + 1]
for signal in signal_map["SET"]:
signal.emit(COLOR_GREEN)
for signal in signal_map["UNSET"]:
signal.emit(COLOR_NONE)
if "ABS_MOVE" in signal_map:
gripper_control_message = GripperControlMessage()
gripper_control_message.gripper_mode = self.GRIPPER_CONTROL_MODES["SCISSOR"]
gripper_control_message.gripper_position_absolute = signal_map["ABS_MOVE"]
self.gripper_control_publisher.publish(gripper_control_message)
else:
self.send_new_gripper_mode = True
self.gripper_control_mode_just_changed = False