def autonomous_callback(channel, msg):
input_data = Joystick.decode(msg)
new_motor = DriveVelCmd()
joystick_math(new_motor, input_data.forward_back, input_data.left_right)
lcm_.publish('/drive_vel_cmd', new_motor.encode())
def drive_control_callback(channel, msg):
global kill_motor, connection
if not connection:
return
input_data = Joystick.decode(msg)
if input_data.kill:
kill_motor = True
elif input_data.restart:
kill_motor = False
if kill_motor:
send_drive_kill()
else:
new_motor = DriveVelCmd()
input_data.forward_back = -quadratic(input_data.forward_back)
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('/drive_vel_cmd', new_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 = Motors()
joystick_math(new_motor, input_data.forward_back, input_data.left_right)
lcm_.publish('/motor', new_motor.encode())
def teleop_drive_callback(self, channel, msg):
if self.auton_enabled or not self.teleop_enabled:
return
input = Joystick.decode(msg)
# TODO: possibly add quadratic control
linear = deadzone(input.forward_back, 0.05) * input.dampen
angular = deadzone(input.left_right, 0.1) * input.dampen
# Convert arcade drive to tank drive
angular_op = (angular / 2) / (abs(linear) + 0.5)
vel_left = linear - angular_op
vel_right = linear + angular_op
# Account for reverse
if self.reverse:
tmp = vel_left
vel_left = -1 * vel_right
vel_right = -1 * tmp
# Scale to be within [-1, 1], if necessary
if abs(vel_left) > 1 or abs(vel_right) > 1:
if abs(vel_left) > abs(vel_right):
vel_right /= abs(vel_left)
vel_left /= abs(vel_left)
else:
vel_left /= abs(vel_right)
vel_right /= abs(vel_right)
command = DriveVelCmd()
command.left = vel_left
command.right = vel_right
lcm_.publish('/drive_vel_cmd', command.encode())
def joystick_cb(self, channel, msg):
m = Joystick.decode(msg)
self.JoystickMsg.forward_back = m.forward_back
self.JoystickMsg.left_right = m.left_right
self.JoystickMsg.dampen = m.dampen
# 1-dampen/2
self.JoystickMsg.kill = m.kill
self.JoystickMsg.restart = m.restart
def joystick_callback(channel, msg):
global kill_motor
input_data = Joystick.decode(msg)
new_kill_msg = Kill_switches()
if kill_motor:
if input_data.restart:
kill_motor = False
new_kill_msg.killed = False
lcm_.publish('/kill_switch', new_kill_msg.encode())
else:
return
damp = (input_data.dampen - 1)/(-2)
new_motor = Motors()
if input_data.kill:
new_motor.left = 0
new_motor.right = 0
kill_motor = True
new_kill_msg.killed = True
lcm_.publish('/kill_switch', new_kill_msg.encode())
else:
magnitude = deadzone(input_data.forward_back, 0.2)
theta = deadzone(input_data.left_right, 0.1)
joystick_math(new_motor, magnitude, theta)
new_motor.left *= new_motor.left*damp
new_motor.right *= new_motor.right*damp
lcm_.publish('/motor', new_motor.encode())
