예제 #1
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())
예제 #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)

    temp = new_motor.left
    new_motor.left = new_motor.right
    new_motor.right = temp

    lcm_.publish('/drive_vel_cmd', new_motor.encode())
예제 #3
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())
예제 #4
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())