Triplex – gamepad control

Alexandre from PLUG asked for a way to control Triplex with a gamepad.

There is a already a good tutorial at ev3dev site made by Anton Vanhoucke so I just post the particular settings for my gamepad, a “Terios T3”-like Bluetooth gamepad.

To pair it to EV3 (running ev3dev) we need to turn Bluetooth ON in the Display Menus (‘brickman’).

We put it in pairable mode by pressing “Home” and “X” until it starts blinking. Then from command line we run bluetoothctl and:

agent on
default-agent
scan on
...
pair 58:00:8E:83:1B:8C
trust 58:00:8E:83:1B:8C
connect 58:00:8E:83:1B:8C
...
Connection successful
exit

The gamepad LEDs should stop blinking and one of the LEDs should stay ON. To change between gamepad mode and keyboard+mouse mode we press HOME+X or HOME+A/B/Y.

After a successful connection, we see something like this in dmesg:

[ 520.522776] Bluetooth: HIDP (Human Interface Emulation) ver 1.2
[ 520.522905] Bluetooth: HIDP socket layer initialized
[ 522.148994] hid-generic 0005:1949:0402.0001: unknown main item tag 0x0
[ 522.181426] input: Bluetooth Gamepad as /devices/platform/serial8250.2/tty/ttyS2/hci0/hci0:1/0005:1949:0402.0001/input/input2
[ 522.205296] hid-generic 0005:1949:0402.0001: input,hidraw0: BLUETOOTH HID v1.1b Keyboard [Bluetooth Gamepad] on 00:17:ec:02:91:b7

The name (‘Bluetooth Gamepad’) is important for our python script.

This is the script I use – a small but important part of it is still based on Anton Vanhoucke’ script.
NOTE: the script lost indents when I copy&pasted it to WordPress. Sorry.

#!/usr/bin/env python3
from math import cos,sin,atan2,pi,sqrt
from time import sleep
import ev3dev.ev3 as ev3
import evdev
import threading
from select import select

M11 = 0.666
M12 = 0
M13 = 0.333
M21 = -0.333
M22 = -0.574
M23 = 0.333 
M31 = -0.333
M32 = 0.574
M33 = 0.333

SPEED = 1560 # /sys/class/tacho-motor/motorx/max_speed
TIME = 50

# before start make sure gamepad is paired

## Initializing ##
print("Finding gamepad...")
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
for device in devices:
 if device.name == 'Bluetooth Gamepad':
 gamepad = evdev.InputDevice(device.fn)
 print("Gamepad found")

x = 0
y = 0
ang = 0
ray = 0
s1 = 0
s2 = 0
s3 = 0

rotate_left=False
rotate_right=False

running = True

class MotorThread(threading.Thread):
 def __init__(self):
 self.m1 = ev3.MediumMotor('outA')
 self.m2 = ev3.MediumMotor('outB')
 self.m3 = ev3.MediumMotor('outC')

# coast seems to help
 self.m1.stop_action='coast'
 self.m2.stop_action='coast'
 self.m3.stop_action='coast'

threading.Thread.__init__(self)
 print("Ready")

def run(self):
 while running:
 if (rotate_left or rotate_right):
 if rotate_left:
 self.m1.run_timed(time_sp=TIME/4, speed_sp=SPEED)
 self.m2.run_timed(time_sp=TIME/4, speed_sp=SPEED)
 self.m3.run_timed(time_sp=TIME/4, speed_sp=SPEED)
 sleep(TIME/4/1000)
 else:
 self.m1.run_timed(time_sp=TIME, speed_sp=-SPEED)
 self.m2.run_timed(time_sp=TIME, speed_sp=-SPEED)
 self.m3.run_timed(time_sp=TIME, speed_sp=-SPEED)
 sleep(TIME/1000)
 else:
 self.m1.run_timed(time_sp=TIME, speed_sp=s1)
 self.m2.run_timed(time_sp=TIME, speed_sp=s2)
 self.m3.run_timed(time_sp=TIME, speed_sp=s3)
 sleep(TIME/1000)

motor_thread = MotorThread()
motor_thread.setDaemon(True)
motor_thread.start()

while True:
 select([gamepad], [], [])
 for event in gamepad.read():
 if event.type == 3:
 # joystick or pag

if event.code == 0:
 # left.joystick - X
 if event.value > 128:
 rotate_right=True
 rotate_left=False
 else:
 if event.value < 128:
 rotate_left=True
 rotate_right=False
 else:
 rotate_right=False
 rotate_left=False

if (event.code ==5) or (event.code == 2):

if event.code == 5:
 # right joystick - Y
 y=(128-event.value)/(128/100)
 if event.code == 2:
 # rigt joystick - X
 x=(-128+event.value)/(128/100)

 ray = sqrt(x*x+y*y)

if x!=0:
 ang=atan2(y,x)
 else:
 if y==0:
 ang=0
 else:
 if y>0:
 ang=pi/2
 else:
 ang=-pi/2

if ray>5:
 ax = cos(ang)
 ay = sin(ang)

f1 = M11 * ax + M12 * ay
 f2 = M21 * ax + M22 * ay
 f3 = M31 * ax + M32 * ay

s1 = f1 * SPEED
 s2 = f2 * SPEED
 s3 = f3 * SPEED

else:
 s1 = 0
 s2 = 0
 s3 = 0