PS3 gamepad controlled dump truck

remote.py

#!/usr/bin/env python3

# Remote control for a dump truck
# Copyright 2017 David Lechner <david@lechnology.com>
#
# Based on PS3 gamepad tutorial by Anton Vanhoucke
# http://www.ev3dev.org/docs/tutorials/using-ps3-sixaxis

import evdev
import evdev.ecodes as e
import ev3dev.auto as ev3
import threading, sched, time

# Unfortunately, the button codes returned by the PS3 controller do not match evdev.ecodes.
# Using the names anyway. I might regret this since BTN_X does not have an X on it.
BTN_SELECT = e.BTN_JOYSTICK + 0
BTN_THUMBL = e.BTN_JOYSTICK + 1  # Left analog
BTN_THUMBR = e.BTN_JOYSTICK + 2  # Right analog
BTN_START = e.BTN_JOYSTICK + 3
BTN_NORTH = e.BTN_JOYSTICK + 4  # Up
BTN_EAST = e.BTN_JOYSTICK + 5   # Right
BTN_SOUTH = e.BTN_JOYSTICK + 6  # Down
BTN_WEST = e.BTN_JOYSTICK + 7   # Left
BTN_TL2 = e.BTN_JOYSTICK + 8
BTN_TR2 = e.BTN_JOYSTICK + 9
BTN_TL = e.BTN_JOYSTICK + 10
BTN_TR = e.BTN_JOYSTICK + 11
BTN_X = e.BTN_JOYSTICK + 12  # Triangle
BTN_B = e.BTN_JOYSTICK + 13  # Circle
BTN_A = e.BTN_JOYSTICK + 14  # Eks
BTN_Y = e.BTN_JOYSTICK + 15  # Square
# this one doesn't have a good name to copy
BTN_PS = e.BTN_TRIGGER_HAPPY  # PlayStation


def scale(val, src, dst):
    """
    Scale the given value from the scale of src to the scale of dst.

    val: float or int
    src: tuple
    dst: tuple

    example: print(scale(99, (0.0, 99.0), (-1.0, +1.0)))
    """
    return (float(val - src[0]) / (src[1] - src[0])) * (dst[1] - dst[0]) + dst[0]


def scale_stick(value):
    return scale(value, (0,255), (-100,100))

gamepad = None
beep = None

print('Finding ps3 controller...')
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
for device in devices:
    if device.name == 'PLAYSTATION(R)3 Controller':
        gamepad = device
    if e.EV_SND in device.capabilities():
        beep = device

steer_center = 0
steer_target = 0
dump_speed = 0
drive_speed = 0
running = True


class MotorThread(threading.Thread):
    def __init__(self):
        self.dump_motor = ev3.LargeMotor(ev3.OUTPUT_A)
        self.steer_motor = ev3.MediumMotor(ev3.OUTPUT_B)
        self.steer_motor.reset()
        self.steer_motor.speed_sp = 500
        self.last_steer = 0
        self.drive_motor = ev3.LargeMotor(ev3.OUTPUT_D)
        threading.Thread.__init__(self)

    def run(self):
        print("Engine running!")
        while running:
            # update the motor speeds
            self.dump_motor.run_direct(duty_cycle_sp=dump_speed)
            steer = steer_center + steer_target
            if steer != self.last_steer:
                self.steer_motor.run_to_abs_pos(position_sp=steer)
                self.last_steer = steer
            self.drive_motor.run_direct(duty_cycle_sp=drive_speed)

            # beep when backing up
            if drive_speed > 10 and s.empty():
                evdev._uinput.write(beep.fd, e.EV_SND, e.SND_TONE, 2000)
                s.enter(0.5, 1, lambda: evdev._uinput.write(beep.fd, e.EV_SND, e.SND_TONE, 0))
                s.enter(1, 1, lambda: 0)  # keep the scheduler busy for the sound "off" time

        # stop motors when the program is done
        self.dump_motor.stop()
        self.steer_motor.stop()
        self.drive_motor.stop()

s = sched.scheduler(time.time, time.sleep)
motor_thread = MotorThread()
motor_thread.setDaemon(True)
motor_thread.start()

for event in gamepad.read_loop():
    s.run(blocking=False)

    if event.type == e.EV_ABS:
        # left analog drives and steers
        if event.code == e.ABS_X:
            steer_target = (127 - event.value) // 3
        elif event.code == e.ABS_Y:
            drive_speed = scale_stick(event.value)

        # right analog dumps
        elif event.code == e.ABS_RZ:
            dump_speed = scale_stick(event.value)

    elif event.type == e.EV_KEY:
        # use left/right trigger buttons to adjust steering alignment
        if event.code == BTN_TL and event.value == 1:
            steer_center += 1
        elif event.code == BTN_TR and event.value == 1:
            steer_center -= 1

        # circle button beeps the horn
        elif event.code == BTN_B:
            evdev._uinput.write(beep.fd, e.EV_SND, e.SND_TONE, 220 * event.value)
            # TODO: newer versions of evdev have a public method for this
            # beep.set(e.EV_SND, e.SND_TONE, 220 * event.value)

        # playstation button exits the program
        elif event.code == BTN_PS and event.value == 1:
            print("PS button is pressed. Stopping.")
            running = False
            break

Exellent code!!!! :)