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