def connection_state_changed(c, _):
global kill_motor
if c:
print("Connection established.")
else:
print("Disconnected.")
# Kill drive motors
kill_motor = True
drive_motor = DriveMotors()
drive_motor.left = 0.0
drive_motor.right = 0.0
lcm_.publish('/motor', drive_motor.encode())
# Kill arm motors
arm_motor = OpenLoopRAMotors()
arm_motor.joint_a = 0.0
arm_motor.joint_b = 0.0
arm_motor.joint_c = 0.0
arm_motor.joint_d = 0.0
arm_motor.joint_e = 0.0
arm_motor.joint_f = 0.0
lcm_.publish('/arm_motors', arm_motor.encode())
# Kill SA motors
sa_motor = SAMotors()
sa_motor.drill = 0.0
sa_motor.lead_screw = 0.0
sa_motor.door_actuator = 0.0
sa_motor.cache = 0.0
lcm_.publish('/sa_motors', sa_motor.encode())
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 open_loop_arm_callback(channel, msg):
m = OpenLoopRAMotors.decode(msg)
exec_later(rover.percent_vbus_drive(Talons.arm_joint_a.value, m.joint_a))
exec_later(rover.percent_vbus_drive(Talons.arm_joint_b.value, m.joint_b))
exec_later(rover.percent_vbus_drive(Talons.arm_joint_c.value, m.joint_c))
exec_later(rover.percent_vbus_drive(Talons.arm_joint_d.value, m.joint_d))
exec_later(rover.percent_vbus_drive(Talons.arm_joint_e.value, m.joint_e))
exec_later(rover.percent_vbus_drive(Talons.arm_joint_f.value, m.joint_f))
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())
'''