def ra_control_callback(self, channel, msg):
if (self.arm_control_state != "open-loop"):
return
self.arm_type = self.ArmType.RA
xboxData = Xbox.decode(msg)
motor_speeds = [quadratic(deadzone(xboxData.left_js_x, 0.15)),
quadratic(-deadzone(xboxData.left_js_y, 0.15)),
quadratic(-deadzone(xboxData.right_js_y, 0.15)),
quadratic(deadzone(xboxData.right_js_x, 0.15)),
quadratic(xboxData.right_trigger - xboxData.left_trigger),
(xboxData.right_bumper - xboxData.left_bumper)]
if self.slow_mode:
# slow down joints a, c, e, and f
motor_speeds[0] *= 0.5
motor_speeds[2] *= 0.5
motor_speeds[4] *= 0.5
motor_speeds[5] *= 0.5
openloop_msg = RAOpenLoopCmd()
openloop_msg.throttle = motor_speeds
lcm_.publish('/ra_open_loop_cmd', openloop_msg.encode())
hand_msg = HandCmd()
hand_msg.finger = xboxData.y - xboxData.a
hand_msg.grip = xboxData.b - xboxData.x
lcm_.publish('/hand_open_loop_cmd', hand_msg.encode())
def ra_calibration_callback(self, channel, msg):
if Calibrate.decode(msg).calibrated or self.arm_control_state != 'calibrating':
return
ra_openloop_msg = RAOpenLoopCmd()
ra_openloop_msg.throttle = [0, 1, 0, 0, 0, 0]
lcm_.publish('/ra_open_loop_cmd', ra_openloop_msg.encode())
def send_zero_arm_command():
openloop_msg = RAOpenLoopCmd()
openloop_msg.throttle = [0, 0, 0, 0, 0, 0]
lcm_.publish('/ra_openloop_cmd', openloop_msg.encode())
hand_msg = HandCmd()
hand_msg.finger = 0
hand_msg.grip = 0
lcm_.publish('/hand_openloop_cmd', hand_msg.encode())
def send_ra_kill(self):
if self.arm_type != self.ArmType.RA:
return
arm_motor = RAOpenLoopCmd()
arm_motor.throttle = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
lcm_.publish('/ra_open_loop_cmd', arm_motor.encode())
hand_msg = HandCmd()
hand_msg.finger = 0
hand_msg.grip = 0
lcm_.publish('/hand_open_loop_cmd', hand_msg.encode())
def ra_control_callback(channel, msg):
xboxData = Xbox.decode(msg)
motor_speeds = [-deadzone(quadratic(xboxData.left_js_x), 0.09),
-deadzone(quadratic(xboxData.left_js_y), 0.09),
quadratic(xboxData.left_trigger -
xboxData.right_trigger),
deadzone(quadratic(xboxData.right_js_y), 0.09),
deadzone(quadratic(xboxData.right_js_x), 0.09),
(xboxData.d_pad_right-xboxData.d_pad_left)]
openloop_msg = RAOpenLoopCmd()
openloop_msg.throttle = motor_speeds
lcm_.publish('/ra_openloop_cmd', openloop_msg.encode())
hand_msg = HandCmd()
hand_msg.finger = xboxData.y - xboxData.a
hand_msg.grip = xboxData.b - xboxData.x
lcm_.publish('/hand_openloop_cmd', hand_msg.encode())
def send_arm_kill():
arm_motor = RAOpenLoopCmd()
arm_motor.throttle = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
lcm_.publish('/ra_openloop_cmd', arm_motor.encode())