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())