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 drive_control_callback(channel, msg):
global kill_motor
input_data = Joystick.decode(msg)
input_data.forward_back = -quadratic(input_data.forward_back)
if input_data.kill:
kill_motor = True
elif input_data.restart:
kill_motor = False
new_motor = DriveMotors()
if kill_motor:
new_motor.left = 0
new_motor.right = 0
else:
magnitude = deadzone(input_data.forward_back, 0.04)
theta = deadzone(input_data.left_right, 0.1)
joystick_math(new_motor, magnitude, theta)
damp = (input_data.dampen - 1) / (-2)
new_motor.left *= damp
new_motor.right *= damp
lcm_.publish('/motor', new_motor.encode())
def autonomous_callback(channel, msg):
input_data = Joystick.decode(msg)
new_motor = DriveMotors()
joystick_math(new_motor, input_data.forward_back, input_data.left_right)
lcm_.publish('/motor', new_motor.encode())
def send_drive_kill():
drive_motor = DriveMotors()
drive_motor.left = 0.0
drive_motor.right = 0.0
lcm_.publish('/motor', drive_motor.encode())