Exemplo n.º 1
0
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())
Exemplo n.º 2
0
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())
Exemplo n.º 3
0
    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())
Exemplo n.º 4
0
    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())
Exemplo n.º 5
0
def drive_vel_cmd_callback(channel, msg):
    # set the odrive's velocity to the float specified in the message
    # no state change

    global speed_lock, odrive_bridge
    try:
        cmd = DriveVelCmd.decode(msg)
        if (odrive_bridge.get_state_string() == "ArmedState"):
            global left_speed, right_speed

            speed_lock.acquire()
            left_speed, right_speed = cmd.left, cmd.right
            speed_lock.release()
    except Exception:
        pass
Exemplo n.º 6
0
def drive_vel_cmd_callback(channel, msg):
    # set the odrive's velocity to the float specified in the message
    # no state change

    global speedlock
    global odrive_bridge
    try:
        cmd = DriveVelCmd.decode(msg)
        if (odrive_bridge.get_state() == "ArmedState"):
            global left_speed
            global right_speed

            speedlock.acquire()
            left_speed = cmd.left
            right_speed = cmd.right
            speedlock.release()
    except NameError:
        print("waiting to find odrive")
Exemplo n.º 7
0
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())