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 ik_callback(channel, msg):
# print("recv xbox msg")
xbox = Xbox.decode(msg)
data = {"left_js_x": xbox.left_js_x, "right_js_x": xbox.right_js_x}
try:
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect(("localhost", 6000))
sock.send(json.dumps(data).encode())
except socket.error as exc:
# print(exc)
pass
def arm_control_callback(channel, msg):
xbox = Xbox.decode(msg)
new_arm = OpenLoopRAMotors()
new_arm.joint_a = -deadzone(quadratic(xbox.left_js_x), 0.09) * .5
new_arm.joint_b = -deadzone(quadratic(xbox.left_js_y), 0.09) * .5
new_arm.joint_c = quadratic(xbox.left_trigger - xbox.right_trigger) * .60
new_arm.joint_d = deadzone(quadratic(xbox.right_js_y), 0.09) * .75
new_arm.joint_e = deadzone(quadratic(xbox.right_js_x), 0.09) * .75
new_arm.joint_f = (xbox.right_bumper - xbox.left_bumper)
lcm_.publish('/arm_motors', new_arm.encode())
def sa_control_callback(channel, msg):
xboxData = Xbox.decode(msg)
saMotorsData = [-deadzone(quadratic(xboxData.left_js_x), 0.09),
-deadzone(quadratic(xboxData.left_js_y), 0.09),
quadratic(xboxData.left_trigger -
xboxData.right_trigger)]
openloop_msg = SAOpenLoopCmd()
openloop_msg.throttle = saMotorsData
lcm_.publish('/sa_openloop_cmd', openloop_msg.encode())
def arm_control_callback(channel, msg):
xbox = Xbox.decode(msg)
motorSpeeds = [-deadzone(quadratic(xbox.left_js_x), 0.09)*.5,
-deadzone(quadratic(xbox.left_js_y), 0.09)*.5,
quadratic(xbox.left_trigger - xbox.right_trigger)*.60,
deadzone(quadratic(xbox.right_js_y), 0.09)*.75,
deadzone(quadratic(xbox.right_js_x), 0.09)*.75,
(xbox.d_pad_right - xbox.d_pad_left)*0.60,
(xbox.right_bumper - xbox.left_bumper)]
for i in range(7):
openLoopMsg = OpenLoopRAMotor()
openLoopMsg.joint_id = i
openLoopMsg.speed = motorSpeeds[i]
lcm_.publish('/arm_motors', openLoopMsg.encode())
def sa_control_callback(channel, msg):
xboxData = Xbox.decode(msg)
saMotorsData = [deadzone(quadratic(xboxData.left_js_x), 0.09),
-deadzone(quadratic(xboxData.left_js_y), 0.09),
-deadzone(quadratic(xboxData.right_js_y), 0.09)]
openloop_msg = SAOpenLoopCmd()
openloop_msg.throttle = saMotorsData
lcm_.publish('/sa_openloop_cmd', openloop_msg.encode())
foot_msg = FootCmd()
foot_msg.claw = xboxData.a - xboxData.y
foot_msg.sensor = 0.5 * (xboxData.left_bumper - xboxData.right_bumper)
lcm_.publish('/foot_openloop_cmd', foot_msg.encode())
def arm_control_callback(channel, msg):
# global enc_in
xbox = Xbox.decode(msg)
new_arm = OpenLoopRAMotors()
new_arm.joint_a = -deadzone(quadratic(xbox.left_js_x), 0.09) * .5
new_arm.joint_b = -deadzone(quadratic(xbox.left_js_y), 0.09) * .5
new_arm.joint_c = quadratic(xbox.left_trigger - xbox.right_trigger) * .60
new_arm.joint_d = deadzone(quadratic(xbox.right_js_y), 0.09) * .75
new_arm.joint_e = deadzone(quadratic(xbox.right_js_x), 0.09) * .75
new_arm.joint_f = (xbox.right_bumper - xbox.left_bumper)
print("Arm:\nA: {}\nB: {}\nC: {}\nD: {}\nE: {}\nF: {}\n".format(
new_arm.joint_a, new_arm.joint_b, new_arm.joint_c, new_arm.joint_d,
new_arm.joint_e, new_arm.joint_f))
lcm_.publish('/arm_motors', new_arm.encode())
'''
def sa_control_callback(channel, msg):
global drill_on, door_open
xbox = Xbox.decode(msg)
new_sa_motors = SAMotors()
val = drill_on.new_reading(xbox.right_bumper > 0.5)
new_sa_motors.drill = -0.9 if val else 0.0
new_sa_motors.lead_screw = deadzone(xbox.left_js_y, 0.1)
new_sa_motors.lead_screw = math.copysign(new_sa_motors.lead_screw**2,
new_sa_motors.lead_screw)
if xbox.d_pad_up:
new_sa_motors.door_actuator = 0.5
elif xbox.d_pad_down:
new_sa_motors.door_actuator = -0.5
else:
new_sa_motors.door_actuator = 0
new_sa_motors.cache = deadzone(xbox.right_js_x, 0.2)
lcm_.publish('/sa_motors', new_sa_motors.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 sa_control_callback(self, channel, msg):
if (self.arm_control_state != "open-loop"):
return
self.arm_type = self.ArmType.SA
xboxData = Xbox.decode(msg)
saMotorsData = [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(xboxData.right_trigger - xboxData.left_trigger)]
openloop_msg = SAOpenLoopCmd()
openloop_msg.throttle = saMotorsData
lcm_.publish('/sa_open_loop_cmd', openloop_msg.encode())
foot_msg = FootCmd()
foot_msg.scoop = xboxData.a - xboxData.y
foot_msg.microscope_triad = -(xboxData.left_bumper - xboxData.right_bumper)
lcm_.publish('/foot_open_loop_cmd', foot_msg.encode())