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