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 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 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 auton_drive_callback(self, channel, msg):
if not self.auton_enabled:
return
input = AutonDriveControl.decode(msg)
command = DriveVelCmd()
command.left = input.left_percent_velocity
command.right = input.right_percent_velocity
lcm_.publish('/drive_vel_cmd', command.encode())
def send_drive_kill():
drive_motor = DriveVelCmd()
drive_motor.left = 0.0
drive_motor.right = 0.0
lcm_.publish('/drive_vel_cmd', drive_motor.encode())