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 process_and_send_arm_control(self):
arm_control_message = ArmControlMessage()
gripper_control_message = GripperControlMessage()
should_publish_arm = False
should_publish_gripper = 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
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
arm_control_message.wrist_pitch = (
-(left_y_axis / THUMB_STICK_MAX) *
WRIST_PITCH_SCALAR) * speed_limit
gripper_control_message.target = int(
(-(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 send_gripper_home_on_back_press(self):
gripper_control_message = GripperControlMessage()
back_state = self.controller.controller_states["back_button"]
if self.last_back_button_state == 0 and back_state == 1:
gripper_control_message.should_home = True
self.gripper_control_publisher.publish(gripper_control_message)
self.last_back_button_state = 1
elif self.last_back_button_state == 1 and back_state == 0:
self.last_back_button_state = 0
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
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
# 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 > 0:
should_publish_arm = True
arm_control_message.base = ((left_x_axis / THUMB_STICK_MAX) *
BASE_SCALAR) * left_trigger_ratio
arm_control_message.shoulder = (
(left_y_axis / THUMB_STICK_MAX) *
SHOULDER_SCALAR) * left_trigger_ratio
arm_control_message.elbow = (-(right_y_axis / THUMB_STICK_MAX) *
ELBOW_SCALAR) * left_trigger_ratio
arm_control_message.roll = (-(right_x_axis / THUMB_STICK_MAX) *
ROLL_SCALAR) * left_trigger_ratio
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) * right_trigger_ratio
# ##### FIXME #####
# Remove this once the arm is fixed
# arm_control_message.wrist_pitch = (-(left_y_axis / THUMB_STICK_MAX) * WRIST_PITCH_SCALAR) * right_trigger_ratio
# #################
gripper_control_message.gripper_position_relative = (
-(right_y_axis / THUMB_STICK_MAX) *
GRIPPER_MOVEMENT_SCALAR) * right_trigger_ratio
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